#include "stdafx.h"
#include "TubeHorizontalController.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;

TubeHorizontalController::TubeHorizontalController()
:m_communicate(nullptr),
m_driveNumber(-1),
m_pulseInOneCircle(0),
m_pulseDutyCycle(PULSE_DUTYCYCLE_DEF),
m_servo(nullptr)
{
}


TubeHorizontalController::~TubeHorizontalController()
{
}

std::string TubeHorizontalController::CLASSID()
{
	return "8120C155-F0C6-4e1b-ADD9-E403A099EE91";
}

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

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

void TubeHorizontalController::SetPulseOneCircle(unsigned short pulseonecircle)
{
	m_pulseInOneCircle = pulseonecircle;

	if (m_servo)
	{
		RS232_PARAM param;
		if (m_servo->MakePulseOnCirclePacket(pulseonecircle, param.rs485))
		{
			DoWrite485(param);
		}
	}
}

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

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

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

	InitializeServo();
}

void TubeHorizontalController::StopMove()
{
	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 TubeHorizontalController::Move(int direction, int steps, int pwmperiod, float pulseDutyCycle)
{
	SetDirection(direction);

	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("[TubeHorizontalController::Move][Brak is not opening, no pwm will be sent]");

		return;
	}

	//速度规划
	//int dectotal = m_pulseInOneCircle / 2;
	//if (steps <= dectotal)
	//{
	//	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
	//}
	//else
	//{		
	//	int decstep1 = 2 * dectotal / 3;
	//	int decstep2 = dectotal / 3;

	//	int decperiod1 = 3 * pwmperiod / 2;
	//	int decperiod2 = 3 * pwmperiod;
	//	pwmparam.pwm_step.pwmstep_int = steps - dectotal;
	//	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
	//	pwmparam.pwm_step.pwmstep_int = decstep1;
	//	pwmparam.peroid.pwmperiod_short = (unsigned short)decperiod1;
	//	pwmparam.effective.pwmperiod_short = (unsigned short)(decperiod1 * pulseDutyCycle);
	//	m_communicate->GPIO_DO_Ctrl(OP_SET, (GPIO_DO_ID)m_communicateInterfaceID, DO_ATTR_SET_PWM_PARAM_TABLE, pwmparam);
	//	pwmparam.pwm_step.pwmstep_int = decstep2;
	//	pwmparam.peroid.pwmperiod_short = (unsigned short)decperiod2;
	//	pwmparam.effective.pwmperiod_short = (unsigned short)(decperiod2 * pulseDutyCycle);
	//	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_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 TubeHorizontalController::SetServoStatus(int svostatus)
{
	ServoOn(svostatus != 0);
}

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

		ServoOn(FALSE);
	}
}

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

void TubeHorizontalController::ServoOn(BOOL servoon)
{
	if (m_servo)
	{
		RS232_PARAM params;
		if (m_servo->MakeServoOnPacket(servoon, params.rs485))
		{
			DoWrite485(params);
		}
		else
		{
			if (m_functionIds.find(ID_TUBE_HORIZONTAL_SERVO_ON) != m_functionIds.end()
				&& m_functionIds[ID_TUBE_HORIZONTAL_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_HORIZONTAL_SERVO_ON], DO_ATTR_NONE, doparam);
			}
		}
	}
}

void TubeHorizontalController::SetDirection(int direction)
{
	GPIO_DO_PARAM oriparam;
	oriparam.active_level = (direction > 0) ? 0x1 : 0x0;
	m_communicate->GPIO_DO_Ctrl(OP_WRITE, (GPIO_DO_ID)m_functionIds[ID_TUBE_HORIZONTAL_DIRECTION], DO_ATTR_NONE, oriparam);
}

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

	Sleep(300);
}

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

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

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

			if (params.active_level == 1)	//1:刹车松开   0:刹车闭合
			{
				bRes = true;
				break;
			}

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

	return bRes; 
}