#include "stdafx.h"
#include "TubeAngleController.h"
#include "ICommunicateEntity.h"
#include "IOInterfaceMapper.h"
#include "IServoDrive.h"

using namespace DIOS::Dev::Detail::MachineryECOM;

const float PULSE_DUTYCYCLE_MIN = 0.2f;
const float PULSE_DUTYCYCLE_MAX = 0.95f;
const float PULSE_DUTYCYCLE_DEF = 0.7f;

TubeAngleController::TubeAngleController()
:m_communicate(nullptr),
m_driveNumber(-1),
m_pulseDutyCycle(PULSE_DUTYCYCLE_DEF),
m_servo(nullptr)
{
}


TubeAngleController::~TubeAngleController()
{
}

std::string TubeAngleController::CLASSID()
{
	return "E040F04B-538E-48D0-95CD-9CA7367DD24A";
}

void TubeAngleController::Initialize(const std::string &name)
{
	SetName(name);
}

void TubeAngleController::AttachServoDrive(IServoDrive *servodrive)
{
	m_servo = servodrive;
}

void TubeAngleController::SetPulseOneCircle(unsigned short pulseonecircle)
{
	if (m_servo)
	{
		RS232_PARAM param;
		if (m_servo->MakePulseOnCirclePacket(pulseonecircle, param.rs485))
		{
			DoWrite485(param);
		}
	}
}

void TubeAngleController::SetPulseDutyCycle(float dutyCycle)
{
	m_pulseDutyCycle = dutyCycle;

	if (dutyCycle < PULSE_DUTYCYCLE_MIN || dutyCycle > PULSE_DUTYCYCLE_MAX)
	{
		m_pulseDutyCycle = PULSE_DUTYCYCLE_DEF;
	}
}

void TubeAngleController::OnCommunicationEstablished(ICommunicateEntity *communicate)
{
	m_communicate = communicate;
	if (!m_communicate)
	{
		return;
	}

	ActivePWMMode();
	InitializeServo();
}

void TubeAngleController::StopRotation()
{
	GPIO_DO_PARAM pwmparam;
	pwmparam.active_level = 0x00;
	m_communicate->GPIO_DO_Ctrl(OP_WRITE, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_NONE, pwmparam);

	//ServoOn(FALSE);
}

void TubeAngleController::Rotate(int direction, int steps, DWORD pwmperiod, float pulseDutyCycle)
{
	bool clockWise = direction > 0 ? false : true;
	SetRotateOrientation(clockWise);

	if (pulseDutyCycle < PULSE_DUTYCYCLE_MIN || pulseDutyCycle > PULSE_DUTYCYCLE_MAX)
	{
		pulseDutyCycle = m_pulseDutyCycle;
	}

	GPIO_DO_PARAM pwmparam;
	pwmparam.pwm_step.pwmstep_int = (int)steps;
	pwmparam.peroid.pwmperiod_short = (unsigned short)pwmperiod;
	pwmparam.effective.pwmperiod_short = (unsigned short)(pwmperiod * pulseDutyCycle);
	pwmparam.output_mode = (unsigned char)GOM_PWM;
	pwmparam.active_level = 0x01;
	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_DELETE, pwmparam);
	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_MODE, pwmparam);

	ServoOn(TRUE);

	if (!IsBrakeOpening())
	{
		if (gmotionLog) gmotionLog->Error("[TubeAngleController::Rotate][Brak is not opening, no pwm will be sent]");

		return;
	}

	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
	m_communicate->GPIO_DO_Ctrl(OP_WRITE, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_NONE, pwmparam);
}

void TubeAngleController::SetRotateOrientation(bool clockWise)
{
	GPIO_DO_PARAM doparam;
	doparam.active_level = clockWise ? 0x0 : 0x1;
	m_communicate->GPIO_DO_Ctrl(OP_WRITE, (GPIO_DO_ID)m_functionIds[ID_TUBE_ANGLE_DIRECTION], DO_ATTR_NONE, doparam);
}

void TubeAngleController::ClearSignal()
{
	GPIO_DO_PARAM doparam;
	doparam.autoupload_switch = GAUTO_DO_UPLOAD_OFF;
	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_DELETE, doparam);
	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_AUTO_UPLOAD, doparam);
}

void TubeAngleController::AppendPWM(int steps, int period, float pulseDutyCycle)
{
	if (pulseDutyCycle < PULSE_DUTYCYCLE_MIN || pulseDutyCycle > PULSE_DUTYCYCLE_MAX)
	{
		pulseDutyCycle = m_pulseDutyCycle;
	}

	GPIO_DO_PARAM pwmparam;
	pwmparam.pwm_step.pwmstep_int = (int)steps;
	pwmparam.peroid.pwmperiod_short = (unsigned short)period;
	pwmparam.effective.pwmperiod_short = (unsigned short)(period * pulseDutyCycle);
	pwmparam.output_mode = (unsigned char)GOM_PWM;
	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
}

void TubeAngleController::ClearServoDriveWarning()
{
	RS232_PARAM param;
	if (m_servo && m_servo->MakeClearWarningPacket(param.rs485))
	{
		DoWrite485(param);
	}
}

void TubeAngleController::SetServoStatus(int svostatus)
{
	ServoOn(svostatus != 0);
}

void TubeAngleController::InitializeServo()
{
	if (m_servo)
	{
		RS232_PARAM param;
		if (m_servo->MakeServoOnEnablePacket(param.rs485))
		{
			DoWrite485(param);
		}

		ServoOn(FALSE);
	}
}

void TubeAngleController::ActivePWMMode()
{
	GPIO_DO_PARAM doparam;
	doparam.output_mode = (unsigned char)GOM_PWM;
	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_MODE, doparam);
}

void TubeAngleController::ServoOn(BOOL servoon)
{
	RS232_PARAM param;
	if (m_servo->MakeServoOnPacket(servoon,param.rs485))
	{
		DoWrite485(param);
	}
	else
	{
		if (m_functionIds.find(ID_TUBE_ANGLE_SERVO_ON) != m_functionIds.end()
			&& m_functionIds[ID_TUBE_ANGLE_SERVO_ON] > 0)
		{
			GPIO_DO_PARAM doparam;
			doparam.active_level = servoon ? 0x1 : 0x0;
			m_communicate->GPIO_DO_Ctrl(OP_WRITE, (GPIO_DO_ID)m_functionIds[ID_TUBE_ANGLE_SERVO_ON], DO_ATTR_NONE, doparam);
		}
	}
}

void TubeAngleController::DoWrite485(RS232_PARAM &param)
{
	m_communicate->RS232_Ctrl(OP_WRITE, (RS232_ID)m_functionIds[ID_RS232], RS232_ATTR_NONE, param);

	Sleep(300);
}

bool TubeAngleController::IsBrakeOpening()
{
	bool bRes = false;

	GPIO_DI_PARAM params;
	params.input_mode = GIM_DIGITAL;
	params.active_level = 0x0;

	if (IOInterfaceMapper::ID_TUBE_ANGLE_BRAKE > 0)
	{
		for (int i = 0; i < 10; i++)
		{
			m_communicate->GPIO_DI_Ctrl(OP_READ, (GPIO_DI_ID)IOInterfaceMapper::ID_TUBE_ANGLE_BRAKE, DI_ATTR_NONE, params);

			if (params.active_level == 1)	//1:ɲ³µËÉ¿ª   0:ɲ³µ±ÕºÏ
			{
				bRes = true;
				break;
			}

			Sleep(50);
		}
	}
	else
	{
		bRes = true;
	}

	return bRes;
}