#include "step_motor.h"
#include "port.h"
#include "system_setup.h"

#define STEP_PULSE_BIT_MASK		0x0001u
#define STEP_DIR_BIT_MASK		0x0002u
#define STEP_DIR_BIT_SHIFT		0x0001u


#define STEP_MOTOR_PLUS_DIRECTION				0
#define STEP_MOTOR_MINUS_DIRECTION				1

#define STEP_MOTOR_1_ENCODER_INDEX				0
#define STEP_MOTOR_2_ENCODER_INDEX				1
#define STEP_MOTOR_3_ENCODER_INDEX				2
#define STEP_MOTOR_4_ENCODER_INDEX				3
#define STEP_MOTOR_5_ENCODER_INDEX				4

#define STEP_MOTOR_STALL_ENCODER_DIFF			100


#define BF_UP_DOWN_MOTOR_SLOW_SPEED_VALUE		2000
#define BF_UP_DOWN_MOTOR_HIGH_SPEED_VALUE		5000

int StepMotorControlMode;
int StepMotorInterlockAValue;
TStepMotorData StepMotorData[MAX_STEP_MOTOR_COUNT];

void Step_Motor_ClearPulse(int motorIndex);

void Step_Motor_Init(void)
{
	int motorIndex;

	for (motorIndex = 0; motorIndex < MAX_STEP_MOTOR_COUNT; motorIndex++)
	{
		StepMotorData[motorIndex].PulseRemain   = 0;
		StepMotorData[motorIndex].Ctrl          = 0;
		StepMotorData[motorIndex].HomeDetected  = 0;
		StepMotorData[motorIndex].AbsEncoderVal = 0;
		StepMotorData[motorIndex].TempEncoderVal = 0;
		StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;
	}
	StepMotorData[0].ControlAddr = STEP_MOTOR_0_PULSE;
	StepMotorData[1].ControlAddr = STEP_MOTOR_1_PULSE;
	StepMotorData[2].ControlAddr = STEP_MOTOR_2_PULSE;
	StepMotorData[3].ControlAddr = STEP_MOTOR_3_PULSE;
	StepMotorData[4].ControlAddr = STEP_MOTOR_4_PULSE;

	StepMotorData[0].MinusLimitPortIndex    = IP_NC_1_LIMIT_H;
	StepMotorData[0].HomePortIndex          = IP_NC_1_HOME;
	StepMotorData[0].PlusLimitPortIndex     = IP_NC_1_LIMIT_L;
	StepMotorData[1].MinusLimitPortIndex    = IP_NC_2_LIMIT_L;
	StepMotorData[1].HomePortIndex          = IP_NC_2_HOME;
	StepMotorData[1].PlusLimitPortIndex     = IP_NC_2_LIMIT_H;
	StepMotorData[2].MinusLimitPortIndex    = IP_NC_3_LIMIT_L;
	StepMotorData[2].HomePortIndex          = IP_NC_3_HOME;
	StepMotorData[2].PlusLimitPortIndex     = IP_NC_3_LIMIT_H;
	StepMotorData[3].MinusLimitPortIndex    = IP_NC_4_LIMIT_L;
	StepMotorData[3].HomePortIndex          = IP_NC_4_HOME;
	StepMotorData[3].PlusLimitPortIndex     = IP_NC_4_LIMIT_H;
	StepMotorData[4].MinusLimitPortIndex    = IP_NC_5_LIMIT_L;
	StepMotorData[4].HomePortIndex          = IP_NC_5_HOME;
	StepMotorData[4].PlusLimitPortIndex     = IP_NC_5_LIMIT_H;

	*StepMotorData[0].ControlAddr = 0;
	*StepMotorData[1].ControlAddr = 0;
	*StepMotorData[2].ControlAddr = 0;
	*StepMotorData[3].ControlAddr = 0;
	*StepMotorData[4].ControlAddr = 0;

	StepMotorData[0].MinFeedAddVal = 100;
	StepMotorData[1].MinFeedAddVal = 500;
//	StepMotorData[2].MinFeedAddVal = 500;
//	StepMotorData[3].MinFeedAddVal = 500;
	StepMotorData[2].MinFeedAddVal = 200;
	StepMotorData[3].MinFeedAddVal = 200;
	StepMotorData[4].MinFeedAddVal = 500;

	StepMotorData[0].MaxFeedAddVal = BF_UP_DOWN_MOTOR_HIGH_SPEED_VALUE;
	StepMotorData[1].MaxFeedAddVal = 2500;
//	StepMotorData[2].MaxFeedAddVal = 500;
//	StepMotorData[3].MaxFeedAddVal = 500;
	StepMotorData[2].MaxFeedAddVal = 200;
	StepMotorData[3].MaxFeedAddVal = 200;
	StepMotorData[4].MaxFeedAddVal = 2500;


	StepMotorData[0].FeedDelay = 10000;
	StepMotorData[1].FeedDelay = 10000;
	StepMotorData[2].FeedDelay = 10000;
	StepMotorData[3].FeedDelay = 10000;
	StepMotorData[4].FeedDelay = 10000;


	StepMotorData[0].FeedDelayCount = 0;
	StepMotorData[1].FeedDelayCount = 0;
	StepMotorData[2].FeedDelayCount = 0;
	StepMotorData[3].FeedDelayCount = 0;
	StepMotorData[4].FeedDelayCount = 0;

	//StepMotorData[0].FeedAccPulseCount = 10;
	StepMotorData[0].FeedAccPulseCount = 5;
	StepMotorData[1].FeedAccPulseCount = 5;
	StepMotorData[2].FeedAccPulseCount = 5;
	StepMotorData[3].FeedAccPulseCount = 5;
	StepMotorData[4].FeedAccPulseCount = 5;

	StepMotorData[0].FeedAccChangeVal = 10;		// 490 steps
	StepMotorData[1].FeedAccChangeVal = 20;		// 100 steps
	StepMotorData[2].FeedAccChangeVal = 1;
	StepMotorData[3].FeedAccChangeVal = 1;
	StepMotorData[4].FeedAccChangeVal = 20;		// 100 steps

	StepMotorData[0].HomeDir = STEP_MOTOR_MINUS_DIRECTION;
	StepMotorData[1].HomeDir = STEP_MOTOR_PLUS_DIRECTION;
	StepMotorData[2].HomeDir = STEP_MOTOR_MINUS_DIRECTION;
	StepMotorData[3].HomeDir = STEP_MOTOR_MINUS_DIRECTION;
	StepMotorData[4].HomeDir = STEP_MOTOR_PLUS_DIRECTION;

	StepMotorData[0].EncoderPlusDir = STEP_MOTOR_PLUS_DIRECTION;
	StepMotorData[1].EncoderPlusDir = STEP_MOTOR_MINUS_DIRECTION;
	StepMotorData[2].EncoderPlusDir = STEP_MOTOR_MINUS_DIRECTION;
	StepMotorData[3].EncoderPlusDir = STEP_MOTOR_MINUS_DIRECTION;
	StepMotorData[4].EncoderPlusDir = STEP_MOTOR_PLUS_DIRECTION;

	StepMotorData[0].EncoderMinusDir = STEP_MOTOR_MINUS_DIRECTION;
	StepMotorData[1].EncoderMinusDir = STEP_MOTOR_PLUS_DIRECTION;
	StepMotorData[2].EncoderMinusDir = STEP_MOTOR_PLUS_DIRECTION;
	StepMotorData[3].EncoderMinusDir = STEP_MOTOR_PLUS_DIRECTION;
	StepMotorData[4].EncoderMinusDir = STEP_MOTOR_MINUS_DIRECTION;

	StepMotorData[0].EncoderIndex = STEP_MOTOR_1_ENCODER_INDEX;
	StepMotorData[1].EncoderIndex = STEP_MOTOR_2_ENCODER_INDEX;
	StepMotorData[2].EncoderIndex = STEP_MOTOR_3_ENCODER_INDEX;
	StepMotorData[3].EncoderIndex = STEP_MOTOR_4_ENCODER_INDEX;
	StepMotorData[4].EncoderIndex = STEP_MOTOR_5_ENCODER_INDEX;

	StepMotorData[0].EncoderMinusLimitPortIndex = IP_NC_1_LIMIT_H;
	StepMotorData[0].EncoderPlusLimitPortIndex  = IP_NC_1_LIMIT_L;
	StepMotorData[1].EncoderMinusLimitPortIndex = IP_NC_2_LIMIT_H;
	StepMotorData[1].EncoderPlusLimitPortIndex  = IP_NC_2_LIMIT_L;
	StepMotorData[2].EncoderMinusLimitPortIndex = IP_NC_3_LIMIT_H;
	StepMotorData[2].EncoderPlusLimitPortIndex  = IP_NC_3_LIMIT_L;
	StepMotorData[3].EncoderMinusLimitPortIndex = IP_NC_4_LIMIT_H;
	StepMotorData[3].EncoderPlusLimitPortIndex  = IP_NC_4_LIMIT_L;
	StepMotorData[4].EncoderMinusLimitPortIndex = IP_NC_5_LIMIT_L;
	StepMotorData[4].EncoderPlusLimitPortIndex  = IP_NC_5_LIMIT_H;

	StepMotorData[0].NCPosParamMul = NC_POS_PARAM_1_MUL;
	StepMotorData[1].NCPosParamMul = NC_POS_PARAM_2_MUL;
	StepMotorData[2].NCPosParamMul = NC_POS_PARAM_3_MUL;
	StepMotorData[3].NCPosParamMul = NC_POS_PARAM_4_MUL;
	StepMotorData[4].NCPosParamMul = NC_POS_PARAM_5_MUL;

	StepMotorData[0].NCPosParamDiv = NC_POS_PARAM_1_DIV;
	StepMotorData[1].NCPosParamDiv = NC_POS_PARAM_2_DIV;
	StepMotorData[2].NCPosParamDiv = NC_POS_PARAM_3_DIV;
	StepMotorData[3].NCPosParamDiv = NC_POS_PARAM_4_DIV;
	StepMotorData[4].NCPosParamDiv = NC_POS_PARAM_5_DIV;


	// step motor 0 -> 5000 : 2000
	StepMotorData[0].MotorPulseCount = 5000;
	StepMotorData[0].EncoderPulseCount = 2000;
	// step motor 1 -> 5000 : 2000
	StepMotorData[1].MotorPulseCount = 5000;
	StepMotorData[1].EncoderPulseCount = 2000;
	// step motor 2 -> 500 : 2000
	StepMotorData[2].MotorPulseCount = 500;
	StepMotorData[2].EncoderPulseCount = 2000;
	// step motor 3 -> 500 : 2000
	StepMotorData[3].MotorPulseCount = 500;
	StepMotorData[3].EncoderPulseCount = 2000;
	// step motor 4 -> 5000 : 2000
	StepMotorData[4].MotorPulseCount = 5000;
	StepMotorData[4].EncoderPulseCount = 2000;
	

	for (motorIndex = 0; motorIndex < MAX_STEP_MOTOR_COUNT; motorIndex++)
	{
		//StepMotorData[motorIndex].CurrentFeedDelay = StepMotorData[motorIndex].MaxFeedDelay;
		StepMotorData[motorIndex].CurrentFeedAddVal = StepMotorData[motorIndex].MinFeedAddVal;
		StepMotorData[motorIndex].FeedChangeCount = 0;
	}

	StepMotorControlMode = STEP_MOTOR_CONTROL_MODE_INTERLOCK;
	StepMotorInterlockAValue = 0;
}

void Step_Motor_50us_Process(void)
{
	int motorIndex;

	for (motorIndex = 0; motorIndex < MAX_STEP_MOTOR_COUNT; motorIndex++)
	{
		if (StepMotorData[motorIndex].PulseRemain)
		{
			StepMotorData[motorIndex].FeedDelayCount += StepMotorData[motorIndex].CurrentFeedAddVal;
			//if (StepMotorData[motorIndex].FeedDelayCount >= StepMotorData[motorIndex].CurrentFeedDelay)
			if (StepMotorData[motorIndex].FeedDelayCount >= StepMotorData[motorIndex].FeedDelay)
			{
				if (StepMotorControlMode == STEP_MOTOR_CONTROL_MODE_INTERLOCK)
				{ 
					if (StepMotorData[motorIndex].Direction == STEP_MOTOR_PLUS_DIRECTION)
					{
						if (PORT_ReadInPort(StepMotorData[motorIndex].PlusLimitPortIndex))
						{
							StepMotorData[motorIndex].Ctrl &= (~STEP_PULSE_BIT_MASK);
							StepMotorData[motorIndex].PulseRemain = 0;
						}
					}
					else
					{
						if (PORT_ReadInPort(StepMotorData[motorIndex].MinusLimitPortIndex))
						{
							StepMotorData[motorIndex].Ctrl &= (~STEP_PULSE_BIT_MASK);
							StepMotorData[motorIndex].PulseRemain = 0;
						}
					}
				}

				if (StepMotorData[motorIndex].Ctrl & STEP_PULSE_BIT_MASK)
				{
					StepMotorData[motorIndex].Ctrl &= (~STEP_PULSE_BIT_MASK);
					*StepMotorData[motorIndex].ControlAddr = StepMotorData[motorIndex].Ctrl;
					StepMotorData[motorIndex].PulseRemain--;

					StepMotorData[motorIndex].FeedChangeCount++;
					if (StepMotorData[motorIndex].FeedChangeCount >= StepMotorData[motorIndex].FeedAccPulseCount)
					{
						if (StepMotorData[motorIndex].PulseRemain > StepMotorData[motorIndex].FeedAccPulseCount * 
							(StepMotorData[motorIndex].CurrentFeedAddVal - StepMotorData[motorIndex].MinFeedAddVal) / StepMotorData[motorIndex].FeedAccChangeVal)
//						if (StepMotorData[motorIndex].PulseRemain > StepMotorData[motorIndex].FeedAccPulseCount)
						{
							if (StepMotorData[motorIndex].CurrentFeedAddVal < StepMotorData[motorIndex].MaxFeedAddVal)
							{
								StepMotorData[motorIndex].CurrentFeedAddVal += StepMotorData[motorIndex].FeedAccChangeVal;
//								StepMotorData[motorIndex].CurrentFeedAddVal += (StepMotorData[motorIndex].MaxFeedAddVal - StepMotorData[motorIndex].MinFeedAddVal) / STEP_MOTOR_ACCELERATE_STEP;
							}
						}
						StepMotorData[motorIndex].FeedChangeCount = 0;
					}					
					//if (StepMotorData[motorIndex].PulseRemain < StepMotorData[motorIndex].FeedAccPulseCount * (StepMotorData[motorIndex].CurrentFeedAddVal - StepMotorData[motorIndex].MinFeedAddVal) / StepMotorData[motorIndex].MinFeedAddVal)

					if (StepMotorData[motorIndex].PulseRemain < StepMotorData[motorIndex].FeedAccPulseCount * 
						(StepMotorData[motorIndex].CurrentFeedAddVal - StepMotorData[motorIndex].MinFeedAddVal) / StepMotorData[motorIndex].FeedAccChangeVal)
					{
						if (StepMotorData[motorIndex].CurrentFeedAddVal > StepMotorData[motorIndex].MinFeedAddVal)
						{
							//StepMotorData[motorIndex].CurrentFeedAddVal -= (StepMotorData[motorIndex].CurrentFeedAddVal - StepMotorData[motorIndex].MinFeedAddVal) / STEP_MOTOR_ACCELERATE_STEP;
							StepMotorData[motorIndex].CurrentFeedAddVal -= StepMotorData[motorIndex].FeedAccChangeVal;
						}
					}					

					if (StepMotorData[motorIndex].CurrentFeedAddVal > StepMotorData[motorIndex].MaxFeedAddVal)	// check if MaxFeedAddVal was lowered
					{
						StepMotorData[motorIndex].CurrentFeedAddVal -= StepMotorData[motorIndex].FeedAccChangeVal;
						if (StepMotorData[motorIndex].CurrentFeedAddVal < StepMotorData[motorIndex].MaxFeedAddVal)
						{
							StepMotorData[motorIndex].CurrentFeedAddVal = StepMotorData[motorIndex].MaxFeedAddVal;
						}
					}
				}
				else
				{
					StepMotorData[motorIndex].Ctrl |= STEP_PULSE_BIT_MASK;
					*StepMotorData[motorIndex].ControlAddr = StepMotorData[motorIndex].Ctrl;
				}
				StepMotorData[motorIndex].FeedDelayCount -= StepMotorData[motorIndex].FeedDelay;
			}
		}
	}
}

void Step_Motor_1ms_Process(void)
{
	int motorIndex;
	short tempEncoderVal;
	char moveDirSign;
	char limitSensed = FALSE;

	// read encoder val
	for (motorIndex = 0; motorIndex < MAX_STEP_MOTOR_COUNT; motorIndex++)
	{
		StepMotorData[motorIndex].OldEncoderVal = StepMotorData[motorIndex].AbsEncoderVal;
		tempEncoderVal = EncoderValue[StepMotorData[motorIndex].EncoderIndex] & 0x007F;
		if (((tempEncoderVal - StepMotorData[motorIndex].TempEncoderVal) & 0x7F) < 0x40)
		{			
			StepMotorData[motorIndex].AbsEncoderVal += ((tempEncoderVal - StepMotorData[motorIndex].TempEncoderVal) & 0x7F);
		}
		else
		{
			StepMotorData[motorIndex].AbsEncoderVal -= ((StepMotorData[motorIndex].TempEncoderVal - tempEncoderVal) & 0x7F);
		}
		StepMotorData[motorIndex].TempEncoderVal = tempEncoderVal;
	}

	for (motorIndex = 0; motorIndex < MAX_STEP_MOTOR_COUNT; motorIndex++)
	{
		if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_IDLE)
		{
			continue;
		}
		else if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_BREAKING)
		{
			if (StepMotorData[motorIndex].PulseRemain == 0)
			{
				StepMotorData[motorIndex].CurrentFeedAddVal = StepMotorData[motorIndex].MinFeedAddVal;
				StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
				StepMotorData[motorIndex].GeneratedPulseCount = 0;
				if (motorIndex == BF_UP_DOWN_MOTOR_INDEX)
				{
					StepMotorData[motorIndex].MaxFeedAddVal = BF_UP_DOWN_MOTOR_HIGH_SPEED_VALUE;
				}
				StepMotorData[motorIndex].State = STEP_MOTOR_STATE_MOVING;
			}
			else
			{
				if (StepMotorData[motorIndex].Direction == StepMotorData[motorIndex].EncoderPlusDir)
				{
					moveDirSign = 1;
				}
				else
				{
					moveDirSign = -1;
				}

				if ((StepMotorData[motorIndex].GeneratedPulseCount - StepMotorData[motorIndex].PulseRemain)
					* StepMotorData[motorIndex].EncoderPulseCount / StepMotorData[motorIndex].MotorPulseCount > 
					(StepMotorData[motorIndex].AbsEncoderVal - StepMotorData[motorIndex].StartPos) * moveDirSign + STEP_MOTOR_STALL_ENCODER_DIFF)
				{
					Step_Motor_ClearPulse(motorIndex);	
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_STALL;
				}
			}
		}
		else if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_MOVING)
		{
			//if (StepMotorData[motorIndex].AbsEncoderVal < StepMotorData[motorIndex].TargetPos - 1) // move plus
			if (StepMotorData[motorIndex].AbsEncoderVal < StepMotorData[motorIndex].TargetPos) // move plus
			{
				moveDirSign = 1;
			}
			//else if (StepMotorData[motorIndex].AbsEncoderVal > StepMotorData[motorIndex].TargetPos + 1)	// move minus
			else if (StepMotorData[motorIndex].AbsEncoderVal > StepMotorData[motorIndex].TargetPos)	// move minus
			{
				moveDirSign = -1;
			}
			else	//reached at target pos
			{
				moveDirSign = 0;
			}

			if (moveDirSign != 0)		// not reached at target pos
			{
				if (moveDirSign == 1)	// move plus
				{
					if (StepMotorControlMode == STEP_MOTOR_CONTROL_MODE_INTERLOCK)
					{
						if (motorIndex == VG2_UP_DOWN_MOTOR_INDEX || motorIndex == BF_UP_DOWN_MOTOR_INDEX)
						{
							if ((StepMotorData[VG2_UP_DOWN_MOTOR_INDEX].AbsEncoderVal * 100 * NC_POS_PARAM_4_DIV / NC_POS_PARAM_4_MUL -
								StepMotorData[BF_UP_DOWN_MOTOR_INDEX].AbsEncoderVal * 100 * NC_POS_PARAM_1_DIV / NC_POS_PARAM_1_MUL)
								>= StepMotorInterlockAValue)
							{
								Step_Motor_ClearPulse(motorIndex);
								StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;
								continue;
							}
							
							if (motorIndex == BF_UP_DOWN_MOTOR_INDEX)
							{
								if ((StepMotorData[VG2_UP_DOWN_MOTOR_INDEX].AbsEncoderVal * 100 * NC_POS_PARAM_4_DIV / NC_POS_PARAM_4_MUL -
									StepMotorData[BF_UP_DOWN_MOTOR_INDEX].AbsEncoderVal * 100 * NC_POS_PARAM_1_DIV / NC_POS_PARAM_1_MUL)
									>= StepMotorInterlockAValue - 2000)	// 10mm
								{
									StepMotorData[BF_UP_DOWN_MOTOR_INDEX].MaxFeedAddVal = BF_UP_DOWN_MOTOR_SLOW_SPEED_VALUE;
								}
							}
						}
					}
					if (PORT_ReadInPort(StepMotorData[motorIndex].EncoderPlusLimitPortIndex))
					{
						limitSensed = TRUE;
					}
					else
					{
						limitSensed = FALSE;
						StepMotorData[motorIndex].Direction = StepMotorData[motorIndex].EncoderPlusDir;
					}
				}
				else if (moveDirSign == -1)	// move minus
				{
					if (PORT_ReadInPort(StepMotorData[motorIndex].EncoderMinusLimitPortIndex))
					{
						limitSensed = TRUE;
					}
					else
					{
						limitSensed = FALSE;
						StepMotorData[motorIndex].Direction = StepMotorData[motorIndex].EncoderMinusDir;
					}
				}

				if (limitSensed)
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;						
				}
				else
				{
					StepMotorData[motorIndex].GeneratedPulseCount += (
						((StepMotorData[motorIndex].TargetPos - StepMotorData[motorIndex].AbsEncoderVal) * moveDirSign *
						StepMotorData[motorIndex].MotorPulseCount / StepMotorData[motorIndex].EncoderPulseCount)
						- StepMotorData[motorIndex].PulseRemain);
					StepMotorData[motorIndex].PulseRemain = 
						(StepMotorData[motorIndex].TargetPos - StepMotorData[motorIndex].AbsEncoderVal) * moveDirSign *
						StepMotorData[motorIndex].MotorPulseCount / StepMotorData[motorIndex].EncoderPulseCount;

					if (StepMotorData[motorIndex].PulseRemain == 0)
					{
						Step_Motor_ClearPulse(motorIndex);
						StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;
					}
					else if ((StepMotorData[motorIndex].GeneratedPulseCount - StepMotorData[motorIndex].PulseRemain)
						* StepMotorData[motorIndex].EncoderPulseCount / StepMotorData[motorIndex].MotorPulseCount > 
						(StepMotorData[motorIndex].AbsEncoderVal - StepMotorData[motorIndex].StartPos) * moveDirSign + STEP_MOTOR_STALL_ENCODER_DIFF)
					{
						Step_Motor_ClearPulse(motorIndex);	
						StepMotorData[motorIndex].State = STEP_MOTOR_STATE_STALL;
					}
					else
					{
						StepMotorData[motorIndex].Ctrl = ((StepMotorData[motorIndex].Ctrl & (~STEP_DIR_BIT_MASK)) | (StepMotorData[motorIndex].Direction << STEP_DIR_BIT_SHIFT));
					}
				}
			}
			else
			{
				Step_Motor_ClearPulse(motorIndex);
				StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;
			}
		}
		else if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_HOME_FIRST_MOVE)
		{
			if (StepMotorData[motorIndex].HomeDir == STEP_MOTOR_PLUS_DIRECTION)
			{
				if (PORT_ReadInPort(StepMotorData[motorIndex].HomePortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].PulseCounter = 0;
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_HOME_SECOND_MOVE;
				}
				else if (PORT_ReadInPort(StepMotorData[motorIndex].PlusLimitPortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_HOME_REVERSE_MOVE;
				}
				else
				{
					StepMotorData[motorIndex].Direction = STEP_MOTOR_PLUS_DIRECTION;
					if (StepMotorData[motorIndex].Direction)
					{
						StepMotorData[motorIndex].Ctrl |= STEP_DIR_BIT_MASK;
					}
					else
					{
						StepMotorData[motorIndex].Ctrl &= (~STEP_DIR_BIT_MASK);
					}
					if (StepMotorData[motorIndex].EncoderPlusDir == StepMotorData[motorIndex].HomeDir)
					{
						moveDirSign = 1;
					} 
					else
					{
						moveDirSign = -1;
					}

					if ((StepMotorData[motorIndex].GeneratedPulseCount - StepMotorData[motorIndex].PulseRemain)
						* StepMotorData[motorIndex].EncoderPulseCount / StepMotorData[motorIndex].MotorPulseCount > 
						(StepMotorData[motorIndex].AbsEncoderVal - StepMotorData[motorIndex].StartPos) * moveDirSign + STEP_MOTOR_STALL_ENCODER_DIFF)
					{
						Step_Motor_ClearPulse(motorIndex);	
						StepMotorData[motorIndex].State = STEP_MOTOR_STATE_STALL;
					}
					else
					{
						//StepMotorData[motorIndex].PulseRemain = StepMotorData[motorIndex].FeedAcc * 10;
						StepMotorData[motorIndex].GeneratedPulseCount += (100000 - StepMotorData[motorIndex].PulseRemain);
						StepMotorData[motorIndex].PulseRemain = 100000;
					}
				}
			}
			else
			{
				if (PORT_ReadInPort(StepMotorData[motorIndex].HomePortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].PulseCounter = 0;
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_HOME_SECOND_MOVE;
				}
				else if (PORT_ReadInPort(StepMotorData[motorIndex].MinusLimitPortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_HOME_REVERSE_MOVE;
				}
				else
				{
					StepMotorData[motorIndex].Direction = STEP_MOTOR_MINUS_DIRECTION;
					if (StepMotorData[motorIndex].Direction)
					{
						StepMotorData[motorIndex].Ctrl |= STEP_DIR_BIT_MASK;
					}
					else
					{
						StepMotorData[motorIndex].Ctrl &= (~STEP_DIR_BIT_MASK);
					}

					if (StepMotorData[motorIndex].EncoderPlusDir == StepMotorData[motorIndex].HomeDir)
					{
						moveDirSign = 1;
					} 
					else
					{
						moveDirSign = -1;
					}

					if ((StepMotorData[motorIndex].GeneratedPulseCount - StepMotorData[motorIndex].PulseRemain)
						* StepMotorData[motorIndex].EncoderPulseCount / StepMotorData[motorIndex].MotorPulseCount > 
						(StepMotorData[motorIndex].AbsEncoderVal - StepMotorData[motorIndex].StartPos) * moveDirSign + STEP_MOTOR_STALL_ENCODER_DIFF)
					{
						Step_Motor_ClearPulse(motorIndex);	
						StepMotorData[motorIndex].State = STEP_MOTOR_STATE_STALL;
					}
					else
					{
						//StepMotorData[motorIndex].PulseRemain = StepMotorData[motorIndex].FeedAcc * 10;
						StepMotorData[motorIndex].GeneratedPulseCount += (100000 - StepMotorData[motorIndex].PulseRemain);
						StepMotorData[motorIndex].PulseRemain = 100000;
					}
				}

			}
		}
		else if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_HOME_SECOND_MOVE)
		{
			if (StepMotorData[motorIndex].HomeDir == STEP_MOTOR_PLUS_DIRECTION)
			{
				if (!PORT_ReadInPort(StepMotorData[motorIndex].HomePortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].HomeDetected = 1;
					StepMotorData[motorIndex].TempEncoderVal = EncoderValue[StepMotorData[motorIndex].EncoderIndex] & 0x007F;
					StepMotorData[motorIndex].AbsEncoderVal = 0;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;
				}
				else
				{	
					if (StepMotorData[motorIndex].PulseCounter == 0)
					{
						StepMotorData[motorIndex].Direction = 1;
						if (StepMotorData[motorIndex].Direction)
						{
							StepMotorData[motorIndex].Ctrl |= STEP_DIR_BIT_MASK;
						}
						else
						{
							StepMotorData[motorIndex].Ctrl &= (~STEP_DIR_BIT_MASK);
						}

						if (StepMotorData[motorIndex].EncoderPlusDir != StepMotorData[motorIndex].HomeDir)
						{
							moveDirSign = 1;
						} 
						else
						{
							moveDirSign = -1;
						}

						if ((StepMotorData[motorIndex].GeneratedPulseCount - StepMotorData[motorIndex].PulseRemain)
							* StepMotorData[motorIndex].EncoderPulseCount / StepMotorData[motorIndex].MotorPulseCount > 
							(StepMotorData[motorIndex].AbsEncoderVal - StepMotorData[motorIndex].StartPos) * moveDirSign + STEP_MOTOR_STALL_ENCODER_DIFF)
						{
							Step_Motor_ClearPulse(motorIndex);	
							StepMotorData[motorIndex].State = STEP_MOTOR_STATE_STALL;
						}
						else
						{
							StepMotorData[motorIndex].GeneratedPulseCount += (1 - StepMotorData[motorIndex].PulseRemain);
							StepMotorData[motorIndex].PulseRemain = 1;
							StepMotorData[motorIndex].PulseCounter = 19;
						}
					}
					else
					{
						StepMotorData[motorIndex].PulseCounter--;
					}
				}
			}
			else
			{
				if (!PORT_ReadInPort(StepMotorData[motorIndex].HomePortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].HomeDetected = 1;
					StepMotorData[motorIndex].TempEncoderVal = EncoderValue[StepMotorData[motorIndex].EncoderIndex] & 0x007F;
					StepMotorData[motorIndex].AbsEncoderVal = 0;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;

				}
				else
				{
					if (StepMotorData[motorIndex].PulseCounter == 0)
					{
						StepMotorData[motorIndex].Direction = 0;
						if (StepMotorData[motorIndex].Direction)
						{
							StepMotorData[motorIndex].Ctrl |= STEP_DIR_BIT_MASK;
						}
						else
						{
							StepMotorData[motorIndex].Ctrl &= (~STEP_DIR_BIT_MASK);
						}

						if (StepMotorData[motorIndex].EncoderPlusDir != StepMotorData[motorIndex].HomeDir)
						{
							moveDirSign = 1;
						} 
						else
						{
							moveDirSign = -1;
						}

						if ((StepMotorData[motorIndex].GeneratedPulseCount - StepMotorData[motorIndex].PulseRemain)
							* StepMotorData[motorIndex].EncoderPulseCount / StepMotorData[motorIndex].MotorPulseCount > 
							(StepMotorData[motorIndex].AbsEncoderVal - StepMotorData[motorIndex].StartPos) * moveDirSign + STEP_MOTOR_STALL_ENCODER_DIFF)
						{
							Step_Motor_ClearPulse(motorIndex);	
							StepMotorData[motorIndex].State = STEP_MOTOR_STATE_STALL;
						}
						else
						{
							StepMotorData[motorIndex].GeneratedPulseCount += (1 - StepMotorData[motorIndex].PulseRemain);
							StepMotorData[motorIndex].PulseRemain = 1;
							StepMotorData[motorIndex].PulseCounter = 19;
						}
					}
					else
					{
						StepMotorData[motorIndex].PulseCounter--;
					}
				}

			}
		}
		else if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_HOME_REVERSE_MOVE)
		{
			if (StepMotorData[motorIndex].HomeDir == STEP_MOTOR_PLUS_DIRECTION)
			{
				if (PORT_ReadInPort(StepMotorData[motorIndex].MinusLimitPortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_ERROR;
				}
				else if (PORT_ReadInPort(StepMotorData[motorIndex].HomePortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].PulseCounter = 0;
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_HOME_SECOND_MOVE;
				}
				else
				{
					StepMotorData[motorIndex].Direction = 1;
					if (StepMotorData[motorIndex].Direction)
					{
						StepMotorData[motorIndex].Ctrl |= STEP_DIR_BIT_MASK;
					}
					else
					{
						StepMotorData[motorIndex].Ctrl &= (~STEP_DIR_BIT_MASK);
					}

					if (StepMotorData[motorIndex].EncoderPlusDir != StepMotorData[motorIndex].HomeDir)
					{
						moveDirSign = 1;
					} 
					else
					{
						moveDirSign = -1;
					}

					if ((StepMotorData[motorIndex].GeneratedPulseCount - StepMotorData[motorIndex].PulseRemain)
						* StepMotorData[motorIndex].EncoderPulseCount / StepMotorData[motorIndex].MotorPulseCount > 
						(StepMotorData[motorIndex].AbsEncoderVal - StepMotorData[motorIndex].StartPos) * moveDirSign + STEP_MOTOR_STALL_ENCODER_DIFF)
					{
						Step_Motor_ClearPulse(motorIndex);	
						StepMotorData[motorIndex].State = STEP_MOTOR_STATE_STALL;
					}
					else
					{
						//StepMotorData[motorIndex].PulseRemain = StepMotorData[motorIndex].FeedAcc * 10;
						StepMotorData[motorIndex].GeneratedPulseCount += (100000 - StepMotorData[motorIndex].PulseRemain);
						StepMotorData[motorIndex].PulseRemain = 100000;
					}
				}
			}
			else
			{
				if (PORT_ReadInPort(StepMotorData[motorIndex].PlusLimitPortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_ERROR;
				}
				else if (PORT_ReadInPort(StepMotorData[motorIndex].HomePortIndex))
				{
					Step_Motor_ClearPulse(motorIndex);
					StepMotorData[motorIndex].PulseCounter = 0;
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_HOME_SECOND_MOVE;
				}
				else
				{
					StepMotorData[motorIndex].Direction = 0;
					if (StepMotorData[motorIndex].Direction)
					{
						StepMotorData[motorIndex].Ctrl |= STEP_DIR_BIT_MASK;
					}
					else
					{
						StepMotorData[motorIndex].Ctrl &= (~STEP_DIR_BIT_MASK);
					}

					if (StepMotorData[motorIndex].EncoderPlusDir != StepMotorData[motorIndex].HomeDir)
					{
						moveDirSign = 1;
					} 
					else
					{
						moveDirSign = -1;
					}

					if ((StepMotorData[motorIndex].GeneratedPulseCount - StepMotorData[motorIndex].PulseRemain)
						* StepMotorData[motorIndex].EncoderPulseCount / StepMotorData[motorIndex].MotorPulseCount > 
						(StepMotorData[motorIndex].AbsEncoderVal - StepMotorData[motorIndex].StartPos) * moveDirSign + STEP_MOTOR_STALL_ENCODER_DIFF)
					{
						Step_Motor_ClearPulse(motorIndex);	
						StepMotorData[motorIndex].State = STEP_MOTOR_STATE_STALL;
					}
					else
					{
						//StepMotorData[motorIndex].PulseRemain = StepMotorData[motorIndex].FeedAcc * 10;
						StepMotorData[motorIndex].GeneratedPulseCount += (100000 - StepMotorData[motorIndex].PulseRemain);
						StepMotorData[motorIndex].PulseRemain = 100000;
					}
				}

			}
		}
	}
}

void MC_StepMotor_EmergencyStop(void)
{
	int motorIndex;

	for (motorIndex = 0; motorIndex < MAX_STEP_MOTOR_COUNT; motorIndex++)
	{
		StepMotorData[motorIndex].Ctrl = 0;
		*StepMotorData[motorIndex].ControlAddr = 0;
		StepMotorData[motorIndex].PulseRemain = 0;
		StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;
	}
}
void MC_StepMotor_EmergencyReset(void)
{
	int motorIndex;

	for (motorIndex = 0; motorIndex < MAX_STEP_MOTOR_COUNT; motorIndex++)
	{
		if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_STALL)
		{
			StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;
		}
	}
}

void Step_Motor_Run(int motorIndex, int dir, int pulseCount)
{
	StepMotorData[motorIndex].Direction = dir;
	if (StepMotorData[motorIndex].Direction)
	{
		StepMotorData[motorIndex].Ctrl |= STEP_DIR_BIT_MASK;
	}
	else
	{
		StepMotorData[motorIndex].Ctrl &= (~STEP_DIR_BIT_MASK);
	}
//	StepMotorData[motorIndex].CurrentFeedDelay = StepMotorData[motorIndex].MaxFeedDelay;
	StepMotorData[motorIndex].CurrentFeedAddVal = StepMotorData[motorIndex].MinFeedAddVal;
	StepMotorData[motorIndex].FeedChangeCount = 0;
	StepMotorData[motorIndex].PulseRemain = pulseCount;
}


void Step_Motor_ClearPulse(int motorIndex)
{
	StepMotorData[motorIndex].Ctrl &= (~STEP_DIR_BIT_MASK);
	StepMotorData[motorIndex].PulseRemain = 0;
}


void Step_Motor_AddPulse(int motorIndex, int dir, int pulseCount)
{
	if (StepMotorData[motorIndex].PulseRemain == 0)
	{
		StepMotorData[motorIndex].Direction = dir;
		if (StepMotorData[motorIndex].Direction)
		{
			StepMotorData[motorIndex].Ctrl |= STEP_DIR_BIT_MASK;
		}
		else
		{
			StepMotorData[motorIndex].Ctrl &= (~STEP_DIR_BIT_MASK);
		}
//		StepMotorData[motorIndex].CurrentFeedDelay = StepMotorData[motorIndex].MaxFeedDelay;
		StepMotorData[motorIndex].CurrentFeedAddVal = StepMotorData[motorIndex].MinFeedAddVal;
		StepMotorData[motorIndex].FeedChangeCount = 0;
		StepMotorData[motorIndex].PulseRemain = pulseCount;
	}
	else
	{
		if (StepMotorData[motorIndex].Direction == dir)
		{
			StepMotorData[motorIndex].PulseRemain += pulseCount;
		}
		else
		{
			StepMotorData[motorIndex].PulseRemain -= pulseCount;
			if (StepMotorData[motorIndex].PulseRemain < 0)
			{
				StepMotorData[motorIndex].PulseRemain = 0;
			}
		}
	}
}

void Step_Motor_HomeSearch(int motorIndex)
{
	StepMotorData[motorIndex].HomeDetected = 0;
	StepMotorData[motorIndex].CurrentFeedAddVal = StepMotorData[motorIndex].MinFeedAddVal;
	StepMotorData[motorIndex].GeneratedPulseCount = 0;
	StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
	StepMotorData[motorIndex].State = STEP_MOTOR_STATE_HOME_FIRST_MOVE;
}


void Step_Motor_MovePos(int motorIndex, int pos)
{
	if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_STALL)
	{
		return;
	}
	else 
	{
		StepMotorData[motorIndex].TargetPos = pos;		
		if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_MOVING || StepMotorData[motorIndex].State == STEP_MOTOR_STATE_BREAKING)
		{
			if (StepMotorData[motorIndex].Direction == STEP_MOTOR_PLUS_DIRECTION)
			{
				if (StepMotorData[motorIndex].TargetPos < StepMotorData[motorIndex].AbsEncoderVal)
				{
					if (StepMotorData[motorIndex].PulseRemain > StepMotorData[motorIndex].MotorPulseCount)
					{
						StepMotorData[motorIndex].PulseRemain = StepMotorData[motorIndex].MotorPulseCount;
					}
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].GeneratedPulseCount = StepMotorData[motorIndex].PulseRemain;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_BREAKING;
				}
				else
				{
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					if (motorIndex == BF_UP_DOWN_MOTOR_INDEX)
					{
						StepMotorData[motorIndex].MaxFeedAddVal = BF_UP_DOWN_MOTOR_HIGH_SPEED_VALUE;
					}
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_MOVING;
				}
			}
			else
			{
				if (StepMotorData[motorIndex].TargetPos > StepMotorData[motorIndex].AbsEncoderVal)
				{
					if (StepMotorData[motorIndex].PulseRemain > StepMotorData[motorIndex].MotorPulseCount)
					{
						StepMotorData[motorIndex].PulseRemain = StepMotorData[motorIndex].MotorPulseCount;
					}
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].GeneratedPulseCount = StepMotorData[motorIndex].PulseRemain;
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_BREAKING;
				}
				else
				{
					StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
					StepMotorData[motorIndex].GeneratedPulseCount = 0;
					if (motorIndex == BF_UP_DOWN_MOTOR_INDEX)
					{
						StepMotorData[motorIndex].MaxFeedAddVal = BF_UP_DOWN_MOTOR_HIGH_SPEED_VALUE;
					}
					StepMotorData[motorIndex].State = STEP_MOTOR_STATE_MOVING;
				}
			}			
		}
		else
		{
			StepMotorData[motorIndex].CurrentFeedAddVal = StepMotorData[motorIndex].MinFeedAddVal;
			StepMotorData[motorIndex].StartPos = StepMotorData[motorIndex].AbsEncoderVal;
			StepMotorData[motorIndex].GeneratedPulseCount = 0;
			if (motorIndex == BF_UP_DOWN_MOTOR_INDEX)
			{
				StepMotorData[motorIndex].MaxFeedAddVal = BF_UP_DOWN_MOTOR_HIGH_SPEED_VALUE;
			}
			StepMotorData[motorIndex].State = STEP_MOTOR_STATE_MOVING;
		}
	}
}

void Step_Motor_Stop(int motorIndex)
{
	Step_Motor_ClearPulse(motorIndex);
	StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;
}

void Step_Motor_Alarm_Reset(void)
{
	int motorIndex;

	for (motorIndex = 0; motorIndex < MAX_STEP_MOTOR_COUNT; motorIndex++)
	{
		if (StepMotorData[motorIndex].State == STEP_MOTOR_STATE_STALL)
		{
			StepMotorData[motorIndex].State = STEP_MOTOR_STATE_IDLE;
		}
	}

}

void SetStepMotorInterlockValue(stepMotorInterlockAValue)
{
	StepMotorInterlockAValue = stepMotorInterlockAValue;
}
