#include "KMotionDef.h"

// Defines axis 0, 1, 2 as simple step dir outputs
// enables them
// sets them as an xyz coordinate system for GCode

#define DISABLE_DELAY 5.0		// seconds after host stops to disable the thing

	//MPG HANDLER

	// Example Function as "smooth" MPG motion example
	// which makes use of the exponential motion command.
	// Additionally double Filtered to be very smoot with limited Jerk


#define TAU  0.01				// smoothness factor (Low Pass Time constant seconds for MoveExp)
#define TAU1 0.015				// smoothness factor (Low Pass Time constant seconds for pre filter 1)
#define TAU2 0.015				// smoothness factor (Low Pass Time constant seconds for pre filter 2)
#define FINAL_TIME 1.0			// Set final dest after this amount of time with no change



#define increment 7
#define machAxis 8


#define XSTEPS 10240			// X Axis Steps Per Inch
#define YSTEPS 10240			// Y Axis Steps Per Inch
#define ZSTEPS 10000			// Z Axis Steps Per Inch
#define ASTEPS 46574.81			// A Axis Steps Per Degree
#define BSTEPS 1000				// B Axis Steps Per Degree
#define CSTEPS 1500				// C Axis Steps Per Degree

void ServiceMPG(void);

main()
{
	InitAux();
	AddKonnect(0, &VirtualBits, VirtualBitsEx);

	ch0->InputMode = NO_INPUT_MODE;
	ch0->OutputMode = STEP_DIR_MODE;
	ch0->Vel = 40000;
	ch0->Accel = 400000;
	ch0->Jerk = 4e+006;
	ch0->P = 0;
	ch0->I = 0.01;
	ch0->D = 0;
	ch0->FFAccel = 0;
	ch0->FFVel = 0;
	ch0->MaxI = 200;
	ch0->MaxErr = 1e+006;
	ch0->MaxOutput = 200;
	ch0->DeadBandGain = 1;
	ch0->DeadBandRange = 0;
	ch0->InputChan0 = 1;
	ch0->InputChan1 = 0;
	ch0->OutputChan0 = 8;
	ch0->OutputChan1 = 0;
	ch0->MasterAxis = -1;
	ch0->LimitSwitchOptions = 0x11f;
	ch0->LimitSwitchNegBit = 1027;
	ch0->LimitSwitchPosBit = 1027;
	ch0->SoftLimitPos = 1e+009;
	ch0->SoftLimitNeg = -1e+009;
	ch0->InputGain0 = 1;
	ch0->InputGain1 = 1;
	ch0->InputOffset0 = 0;
	ch0->InputOffset1 = 0;
	ch0->OutputGain = 1;
	ch0->OutputOffset = 0;
	ch0->SlaveGain = 1;
	ch0->BacklashMode = BACKLASH_LINEAR;
	ch0->BacklashAmount = 20;
	ch0->BacklashRate = 200000;
	ch0->invDistPerCycle = 1;
	ch0->Lead = 0;
	ch0->MaxFollowingError = 1000000000;
	ch0->StepperAmplitude = 20;

	ch0->iir[0].B0 = 1;
	ch0->iir[0].B1 = 0;
	ch0->iir[0].B2 = 0;
	ch0->iir[0].A1 = 0;
	ch0->iir[0].A2 = 0;

	ch0->iir[1].B0 = 1;
	ch0->iir[1].B1 = 0;
	ch0->iir[1].B2 = 0;
	ch0->iir[1].A1 = 0;
	ch0->iir[1].A2 = 0;

	ch0->iir[2].B0 = 0.000769;
	ch0->iir[2].B1 = 0.001538;
	ch0->iir[2].B2 = 0.000769;
	ch0->iir[2].A1 = 1.92081;
	ch0->iir[2].A2 = -0.923885;
	EnableAxisDest(0, 0);

	ch1->InputMode = NO_INPUT_MODE;
	ch1->OutputMode = STEP_DIR_MODE;
	ch1->Vel = 40000;
	ch1->Accel = 400000;
	ch1->Jerk = 4e+006;
	ch1->P = 0;
	ch1->I = 0.01;
	ch1->D = 0;
	ch1->FFAccel = 0;
	ch1->FFVel = 0;
	ch1->MaxI = 200;
	ch1->MaxErr = 1e+006;
	ch1->MaxOutput = 200;
	ch1->DeadBandGain = 1;
	ch1->DeadBandRange = 0;
	ch1->InputChan0 = 1;
	ch1->InputChan1 = 0;
	ch1->OutputChan0 = 9;
	ch1->OutputChan1 = 0;
	ch1->MasterAxis = -1;
	ch1->LimitSwitchOptions = 0x11f;
	ch1->LimitSwitchNegBit = 1028;
	ch1->LimitSwitchPosBit = 1028;
	ch1->SoftLimitPos = 1e+009;
	ch1->SoftLimitNeg = -1e+009;
	ch1->InputGain0 = 1;
	ch1->InputGain1 = 1;
	ch1->InputOffset0 = 0;
	ch1->InputOffset1 = 0;
	ch1->OutputGain = 1;
	ch1->OutputOffset = 0;
	ch1->SlaveGain = 1;
	ch1->BacklashMode = BACKLASH_LINEAR;
	ch1->BacklashAmount = 37;
	ch1->BacklashRate = 200000;
	ch1->invDistPerCycle = 1;
	ch1->Lead = 0;
	ch1->MaxFollowingError = 1000000000;
	ch1->StepperAmplitude = 20;

	ch1->iir[0].B0 = 1;
	ch1->iir[0].B1 = 0;
	ch1->iir[0].B2 = 0;
	ch1->iir[0].A1 = 0;
	ch1->iir[0].A2 = 0;

	ch1->iir[1].B0 = 1;
	ch1->iir[1].B1 = 0;
	ch1->iir[1].B2 = 0;
	ch1->iir[1].A1 = 0;
	ch1->iir[1].A2 = 0;

	ch1->iir[2].B0 = 0.000769;
	ch1->iir[2].B1 = 0.001538;
	ch1->iir[2].B2 = 0.000769;
	ch1->iir[2].A1 = 1.92081;
	ch1->iir[2].A2 = -0.923885;
	EnableAxisDest(1, 0);

	ch2->InputMode = NO_INPUT_MODE;
	ch2->OutputMode = STEP_DIR_MODE;
	ch2->Vel = 40000;
	ch2->Accel = 400000;
	ch2->Jerk = 4e+006;
	ch2->P = 0;
	ch2->I = 0.01;
	ch2->D = 0;
	ch2->FFAccel = 0;
	ch2->FFVel = 0;
	ch2->MaxI = 200;
	ch2->MaxErr = 1e+006;
	ch2->MaxOutput = 200;
	ch2->DeadBandGain = 1;
	ch2->DeadBandRange = 0;
	ch2->InputChan0 = 2;
	ch2->InputChan1 = 0;
	ch2->OutputChan0 = 10;
	ch2->OutputChan1 = 0;
	ch2->MasterAxis = -1;
	ch2->LimitSwitchOptions = 0x11f;
	ch2->LimitSwitchNegBit = 1029;
	ch2->LimitSwitchPosBit = 1029;
	ch2->SoftLimitPos = 1e+009;
	ch2->SoftLimitNeg = -1e+009;
	ch2->InputGain0 = 1;
	ch2->InputGain1 = 1;
	ch2->InputOffset0 = 0;
	ch2->InputOffset1 = 0;
	ch2->OutputGain = 1;
	ch2->OutputOffset = 0;
	ch2->SlaveGain = 1;
	ch2->BacklashMode = BACKLASH_OFF;
	ch2->BacklashAmount = 0;
	ch2->BacklashRate = 0;
	ch2->invDistPerCycle = 1;
	ch2->Lead = 0;
	ch2->MaxFollowingError = 1000000000;
	ch2->StepperAmplitude = 20;

	ch2->iir[0].B0 = 1;
	ch2->iir[0].B1 = 0;
	ch2->iir[0].B2 = 0;
	ch2->iir[0].A1 = 0;
	ch2->iir[0].A2 = 0;

	ch2->iir[1].B0 = 1;
	ch2->iir[1].B1 = 0;
	ch2->iir[1].B2 = 0;
	ch2->iir[1].A1 = 0;
	ch2->iir[1].A2 = 0;

	ch2->iir[2].B0 = 0.000769;
	ch2->iir[2].B1 = 0.001538;
	ch2->iir[2].B2 = 0.000769;
	ch2->iir[2].A1 = 1.92081;
	ch2->iir[2].A2 = -0.923885;
	EnableAxisDest(2, 0);

	ch3->InputMode = NO_INPUT_MODE;
	ch3->OutputMode = STEP_DIR_MODE;
	ch3->Vel = 70000;
	ch3->Accel = 30000;
	ch3->Jerk = 4e+006;
	ch3->P = 0;
	ch3->I = 0.01;
	ch3->D = 0;
	ch3->FFAccel = 0;
	ch3->FFVel = 0;
	ch3->MaxI = 200;
	ch3->MaxErr = 1e+006;
	ch3->MaxOutput = 200;
	ch3->DeadBandGain = 1;
	ch3->DeadBandRange = 0;
	ch3->InputChan0 = 8;
	ch3->InputChan1 = 0;
	ch3->OutputChan0 = 11;
	ch3->OutputChan1 = 0;
	ch3->MasterAxis = -1;
	ch3->LimitSwitchOptions = 0x100;
	ch3->LimitSwitchNegBit = 0;
	ch3->LimitSwitchPosBit = 0;
	ch3->SoftLimitPos = 1e+009;
	ch3->SoftLimitNeg = -1e+009;
	ch3->InputGain0 = 1;
	ch3->InputGain1 = 1;
	ch3->InputOffset0 = 0;
	ch3->InputOffset1 = 0;
	ch3->OutputGain = -1;
	ch3->OutputOffset = 0;
	ch3->SlaveGain = 1;
	ch3->BacklashMode = BACKLASH_OFF;
	ch3->BacklashAmount = 0;
	ch3->BacklashRate = 0;
	ch3->invDistPerCycle = 1;
	ch3->Lead = 0;
	ch3->MaxFollowingError = 1000000000;
	ch3->StepperAmplitude = 20;

	ch3->iir[0].B0 = 1;
	ch3->iir[0].B1 = 0;
	ch3->iir[0].B2 = 0;
	ch3->iir[0].A1 = 0;
	ch3->iir[0].A2 = 0;

	ch3->iir[1].B0 = 1;
	ch3->iir[1].B1 = 0;
	ch3->iir[1].B2 = 0;
	ch3->iir[1].A1 = 0;
	ch3->iir[1].A2 = 0;

	ch3->iir[2].B0 = 0.000769;
	ch3->iir[2].B1 = 0.001538;
	ch3->iir[2].B2 = 0.000769;
	ch3->iir[2].A1 = 1.92081;
	ch3->iir[2].A2 = -0.923885;
	EnableAxisDest(3, 0);

	ch4->InputMode = NO_INPUT_MODE;
	ch4->OutputMode = STEP_DIR_MODE;
	ch4->Vel = 40000;
	ch4->Accel = 400000;
	ch4->Jerk = 4e+006;
	ch4->P = 0.2;
	ch4->I = 0;
	ch4->D = 0;
	ch4->FFAccel = 0;
	ch4->FFVel = 0;
	ch4->MaxI = 200;
	ch4->MaxErr = 200;
	ch4->MaxOutput = 200;
	ch4->DeadBandGain = 1;
	ch4->DeadBandRange = 0;
	ch4->InputChan0 = 4;
	ch4->InputChan1 = 1;
	ch4->OutputChan0 = 4;
	ch4->OutputChan1 = 1;
	ch4->MasterAxis = -1;
	ch4->LimitSwitchOptions = 0x100;
	ch4->LimitSwitchNegBit = 0;
	ch4->LimitSwitchPosBit = 0;
	ch4->SoftLimitPos = 1e+009;
	ch4->SoftLimitNeg = -1e+009;
	ch4->InputGain0 = 1;
	ch4->InputGain1 = 1;
	ch4->InputOffset0 = 0;
	ch4->InputOffset1 = 0;
	ch4->OutputGain = 1;
	ch4->OutputOffset = 0;
	ch4->SlaveGain = 1;
	ch4->BacklashMode = BACKLASH_OFF;
	ch4->BacklashAmount = 0;
	ch4->BacklashRate = 0;
	ch4->invDistPerCycle = 1;
	ch4->Lead = 0;
	ch4->MaxFollowingError = 10000000;
	ch4->StepperAmplitude = 250;

	ch4->iir[0].B0 = 1;
	ch4->iir[0].B1 = 0;
	ch4->iir[0].B2 = 0;
	ch4->iir[0].A1 = 0;
	ch4->iir[0].A2 = 0;

	ch4->iir[1].B0 = 1;
	ch4->iir[1].B1 = 0;
	ch4->iir[1].B2 = 0;
	ch4->iir[1].A1 = 0;
	ch4->iir[1].A2 = 0;

	ch4->iir[2].B0 = 1;
	ch4->iir[2].B1 = 0;
	ch4->iir[2].B2 = 0;
	ch4->iir[2].A1 = 0;
	ch4->iir[2].A2 = 0;
	EnableAxisDest(4, 0);

	ch5->InputMode = ENCODER_MODE;
	ch5->OutputMode = NO_OUTPUT_MODE;
	ch5->Vel = 40000;
	ch5->Accel = 400000;
	ch5->Jerk = 4e+006;
	ch5->P = 0.2;
	ch5->I = 0;
	ch5->D = 0;
	ch5->FFAccel = 0;
	ch5->FFVel = 0;
	ch5->MaxI = 200;
	ch5->MaxErr = 200;
	ch5->MaxOutput = 200;
	ch5->DeadBandGain = 1;
	ch5->DeadBandRange = 0;
	ch5->InputChan0 = 0;
	ch5->InputChan1 = 1;
	ch5->OutputChan0 = 5;
	ch5->OutputChan1 = 1;
	ch5->MasterAxis = -1;
	ch5->LimitSwitchOptions = 0x100;
	ch5->LimitSwitchNegBit = 0;
	ch5->LimitSwitchPosBit = 0;
	ch5->SoftLimitPos = 1e+009;
	ch5->SoftLimitNeg = -1e+009;
	ch5->InputGain0 = 1;
	ch5->InputGain1 = 1;
	ch5->InputOffset0 = 0;
	ch5->InputOffset1 = 0;
	ch5->OutputGain = 1;
	ch5->OutputOffset = 0;
	ch5->SlaveGain = 1;
	ch5->BacklashMode = BACKLASH_OFF;
	ch5->BacklashAmount = 0;
	ch5->BacklashRate = 0;
	ch5->invDistPerCycle = 1;
	ch5->Lead = 0;
	ch5->MaxFollowingError = 10000000;
	ch5->StepperAmplitude = 250;

	ch5->iir[0].B0 = 1;
	ch5->iir[0].B1 = 0;
	ch5->iir[0].B2 = 0;
	ch5->iir[0].A1 = 0;
	ch5->iir[0].A2 = 0;

	ch5->iir[1].B0 = 1;
	ch5->iir[1].B1 = 0;
	ch5->iir[1].B2 = 0;
	ch5->iir[1].A1 = 0;
	ch5->iir[1].A2 = 0;

	ch5->iir[2].B0 = 1;
	ch5->iir[2].B1 = 0;
	ch5->iir[2].B2 = 0;
	ch5->iir[2].A1 = 0;
	ch5->iir[2].A2 = 0;



	DefineCoordSystem(0, 1, 2, 4);

	for (;;)
	{							//MPG HANDLER

		// Example Function as "smooth" MPG motion example
		// which makes use of the exponential motion command.
		// Additionally double Filtered to be very smoot with limited Jerk
		void ServiceMPG(void)
		{
			double IncrementValue = *(double *)&persist.UserData[(increment - 1) * 2]	// Get increment value
			double AxisSteps static int Pos, FirstTime = TRUE;
			static int InMotion = FALSE, Axis, LastAxis = -1;
			static double LastChangeTime = 0, Target, Factor = 0;
			static double Target1, Target2, K1, K2, K1M, K2M;

			int Change1, NewPos;

			if (FirstTime)
			{
				Pos = chan[MPG_INPUT_AXIS].Position;
				K1 = exp(-2 * TIMEBASE / TAU1);	// filter coefficients
				K2 = exp(-2 * TIMEBASE / TAU2);
				K1M = 1.0 - K1;
				K2M = 1.0 - K2;
				FirstTime = FALSE;
			}

			NewPos = chan[MPG_INPUT_AXIS].Position;
			Change1 = NewPos - Pos;
			Pos = NewPos;

			Axis = *(double *)&persist.UserData[(machAxis - 1) * 2];

			if (Axis < 6 || JOB_ACTIVE)	// if not button pressed or Job Active ignore the encoder.
				Change1 = 0;



			if (Axis == 0)		// Select X Steps per unit
				AxisSteps = XSTEPS;
			else if (Axis == 1)	// Select Y Steps per unit
				AxisSteps = YSTEPS;
			else if (Axis == 2)	// Select Z Steps per unit
				AxisSteps = ZSTEPS;
			else if (Axis == 4)	// Select A Steps per unit
				AxisSteps = ASTEPS;
			else if (Axis == 5)	// Select B Steps per unit
				AxisSteps = BSTEPS;
			else if (Axis == 3)	// Select C Steps per unit
				AxisSteps = CSTEPS;


			Factor = AxisSteps * IncrementValue;


			// Feedhold fully stopped ??
			if (CS0_StoppingState == 4 && InMotion)
			{
				Change1 = 0;	// ignore any MPG change
				Jog(LastAxis, 0);
				InMotion = FALSE;
			}

			// check if the Axis just changed or we have been 
			// converging to the target for a long time
			if (Axis != LastAxis || (InMotion && Time_sec() > LastChangeTime + FINAL_TIME))
			{
				if (InMotion)
					Move(LastAxis, Target1);	//finalize any motion

				LastAxis = Axis;
				InMotion = FALSE;
			}

			if (Change1)		// did we move?
			{
				if (!InMotion)
					Target = Target1 = Target2 = chan[Axis].Dest;
				Target1 += Change1 * Factor;
				LastChangeTime = Time_sec();
				InMotion = TRUE;
			}

			if (InMotion)		// If moving
			{
				if (Target1 > chan[LastAxis].SoftLimitPos)
					Target1 = chan[LastAxis].SoftLimitPos;
				if (Target1 < chan[LastAxis].SoftLimitNeg)
					Target1 = chan[LastAxis].SoftLimitNeg;
				Target2 = Target2 * K1 + Target1 * K1M;
				Target = Target * K2 + Target2 * K2M;
				MoveExp(Axis, Target, TAU);
			}
		}
		WaitNextTimeSlice();
		static int PrevStatusRequestCounter = -1;	// TOFIX: what about overflow?
		static double LastRequestTime = 0.;
		SetBitDirection(60, 1);
		SetBitDirection(63, 1);
		if (StatusRequestCounter != PrevStatusRequestCounter)
		{						// if there has been a request
			LastRequestTime = Time_sec();	// remember time of this change

			SetBit(63);			// ENABLE / Set HERE // NB: setting a bit that's already set *may* cause problems, if so then make this conditional upon the current status

			// remember the StatusRequestCounter for the next time around
			PrevStatusRequestCounter = StatusRequestCounter;
		}
		else
		{						// there hasn't been a request
			// has it been long enough since the last request?
			if ((Time_sec() - LastRequestTime) > DISABLE_DELAY)
			{
				ClearBit(60);	// Trigger servo estop
				Delay_sec(2);	// Wait for servos
				ClearBit(63);	// Shut down main power
			}
		}





	}


}