#include "manual_encoder.h"
#include "io_port.h"
#include "port.h"
#include "step_motor.h"

#define ME_ENCODER_VAL_MASK			0x0FFF
#define ME_ENCODER_HALF_VAL			0x0800

TManualEncoderInfo ManualEncoderInfo;

char ManualEncoderMotorMap[MAX_STEP_MOTOR_COUNT];

void Manual_Encoder_Init(void)
{
	ManualEncoderInfo.Enabled = TRUE;
	ManualEncoderInfo.PulseEnabled = FALSE;
	ManualEncoderInfo.CurrentEncoderVal = 0;
	ManualEncoderInfo.ControlMotorIndex = -1;
	ManualEncoderInfo.FeedRate = 0;
	ManualEncoderInfo.StepMotorPlusDir[0] = 1;
	ManualEncoderInfo.StepMotorMinusDir[0] = 0;
	ManualEncoderInfo.StepMotorPlusDir[1] = 0;
	ManualEncoderInfo.StepMotorMinusDir[1] = 1;
	ManualEncoderInfo.StepMotorPlusDir[2] = 1;
	ManualEncoderInfo.StepMotorMinusDir[2] = 0;
	ManualEncoderInfo.StepMotorPlusDir[3] = 1;
	ManualEncoderInfo.StepMotorMinusDir[3] = 0;
	ManualEncoderInfo.StepMotorPlusDir[4] = 0;
	ManualEncoderInfo.StepMotorMinusDir[4] = 1;

	ManualEncoderMotorMap[0] = VG1_UP_DOWN_MOTOR_INDEX;
	ManualEncoderMotorMap[1] = VG2_UP_DOWN_MOTOR_INDEX;
	ManualEncoderMotorMap[2] = BF_UP_DOWN_MOTOR_INDEX;
	ManualEncoderMotorMap[3] = SD2_LEFT_RIGHT_MOTOR_INDEX;
	ManualEncoderMotorMap[4] = BF_IN_OUT_MOTOR_INDEX;
}

void ME_Process_20ms(void)
{
	char currentMotorIndex;
	short feedRate;
	unsigned short newEncoderVal;
	int stepMotorIndex;
	if (ManualEncoderInfo.Enabled)
	{
		currentMotorIndex = 0;
		if (PORT_ReadInPort(IP_ME_AXIS_BIT_2))
		{
			currentMotorIndex |= 0x04;
		}
		if (PORT_ReadInPort(IP_ME_AXIS_BIT_1))
		{
			currentMotorIndex |= 0x02;
		}
		if (PORT_ReadInPort(IP_ME_AXIS_BIT_0))
		{
			currentMotorIndex |= 0x01;
		}
		currentMotorIndex = currentMotorIndex - 1;
		if (currentMotorIndex >= 0)
		{
			currentMotorIndex = ManualEncoderMotorMap[currentMotorIndex];
		}

		feedRate = 0;
		if (PORT_ReadInPort(IP_ME_FEED_BIT_1))
		{
			feedRate |= 0x02;
		}
		if (PORT_ReadInPort(IP_ME_FEED_BIT_0))
		{
			feedRate |= 0x01;
		}
		 
		ManualEncoderInfo.FeedRate = feedRate;

		if (PORT_ReadInPort(IP_ME_EMERGENCY_OFF))
		{
            /*
			if (feedRate == 3)
			{
				if (PORT_ReadInPort(IP_ME_ENABLE))
				{
					PORT_ModifyOutPort(OP_LINE_VAC, 1);
				}
				else
				{
					PORT_ModifyOutPort(OP_LINE_VAC, 0);
				}
			}
			PORT_ModifyOutPort(OP_ME_LED_ON, 1);*/
		}
		else
		{
			if (PORT_ReadInPort(IP_ME_ENABLE))
			{
				MC_StepMotor_EmergencyStop();
			}
			//PORT_ModifyOutPort(OP_ME_LED_ON, 0);
		}

		if (ManualEncoderInfo.ControlMotorIndex != currentMotorIndex)
		{
			if (ManualEncoderInfo.PulseEnabled && ManualEncoderInfo.ControlMotorIndex >= 0)
			{
				Step_Motor_Stop(ManualEncoderInfo.ControlMotorIndex);
			}
			ManualEncoderInfo.ControlMotorIndex = currentMotorIndex;
		}

		if (PORT_ReadInPort(IP_ME_ENABLE))
		{
			if (!ManualEncoderInfo.PulseEnabled)
			{
				for (stepMotorIndex = 0; stepMotorIndex < MAX_STEP_MOTOR_COUNT; stepMotorIndex++)
				{
					ManualEncoderInfo.CurrentAbsPosition[stepMotorIndex] = (StepMotorData[stepMotorIndex].AbsEncoderVal * 100 
                                                                            * StepMotorData[stepMotorIndex].NCPosParamDiv / StepMotorData[stepMotorIndex].NCPosParamMul) 
                                                                            * 4;
				}
				ManualEncoderInfo.CurrentEncoderVal = (*ENCODER_8) & ME_ENCODER_VAL_MASK;
				ManualEncoderInfo.PulseEnabled = TRUE;
			}
			else
			{
				if (ManualEncoderInfo.ControlMotorIndex >= 0)
				{
					newEncoderVal = (*ENCODER_8) & ME_ENCODER_VAL_MASK;
					if (newEncoderVal != ManualEncoderInfo.CurrentEncoderVal)
					{
						if (((newEncoderVal - ManualEncoderInfo.CurrentEncoderVal) & ME_ENCODER_VAL_MASK) < ME_ENCODER_HALF_VAL)	// plus position
						{						
							if (ManualEncoderInfo.ControlMotorIndex == 0 && ManualEncoderInfo.FeedRate > 0)
							{
								ManualEncoderInfo.CurrentAbsPosition[ManualEncoderInfo.ControlMotorIndex] += (((newEncoderVal - ManualEncoderInfo.CurrentEncoderVal) & ME_ENCODER_VAL_MASK) * 10);
							}
							else
							{
								ManualEncoderInfo.CurrentAbsPosition[ManualEncoderInfo.ControlMotorIndex] += (((newEncoderVal - ManualEncoderInfo.CurrentEncoderVal) & ME_ENCODER_VAL_MASK));
							}
						}
						else		// minus position
						{
							if (ManualEncoderInfo.ControlMotorIndex == 0 && ManualEncoderInfo.FeedRate > 0)
							{
								ManualEncoderInfo.CurrentAbsPosition[ManualEncoderInfo.ControlMotorIndex] -= (((ManualEncoderInfo.CurrentEncoderVal - newEncoderVal) & ME_ENCODER_VAL_MASK) * 10);
							}
							else
							{
								ManualEncoderInfo.CurrentAbsPosition[ManualEncoderInfo.ControlMotorIndex] -= (((ManualEncoderInfo.CurrentEncoderVal - newEncoderVal) & ME_ENCODER_VAL_MASK));
							}
						}
						Step_Motor_MovePos(ManualEncoderInfo.ControlMotorIndex, ManualEncoderInfo.CurrentAbsPosition[ManualEncoderInfo.ControlMotorIndex] *
							StepMotorData[ManualEncoderInfo.ControlMotorIndex].NCPosParamMul / StepMotorData[ManualEncoderInfo.ControlMotorIndex].NCPosParamDiv / 100 / 4);

						ManualEncoderInfo.CurrentEncoderVal = newEncoderVal;
					}
				}
			}
		}
		else
		{
			if (ManualEncoderInfo.PulseEnabled)
			{
				Step_Motor_Stop(ManualEncoderInfo.ControlMotorIndex);
			}
			ManualEncoderInfo.PulseEnabled = FALSE;
		}
	}

}
