//with MPG OK for jogging

#include "KMotionDef.h"
#define TMP 10 // which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"

//MPG Definition
#define MPG_INPUT_AXIS 7

#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 SELECTX 34
#define SELECTY 33
#define SELECTZ 32
#define SELECTA 31

#define Fro 30
#define Rro 29
#define Sso 28

// Buttons definition
#define FEEDHOLDBIT 27
#define CYCLESTARTBIT 26
#define ESTOP 45
#define HALTBIT 27
#define RESTARTBIT 26
#define ALARMBIT 1

//MPG Direction
int DirMPG;

// function prototypes for compiler
int DoPC(int cmd);
int DoPCFloat(int cmd, float f);
int Debounce(int n, int *cnt, int *last, int *lastsolid);

// state variables for switch debouncing
int flast=0,flastsolid=-1,fcount=0;
int clast=0,clastsolid=-1,ccount=0;
int elast=1,elastsolid=-1,ecount=0;
int hlast=0,hlastsolid=-1,hcount=0;
int rlast=0,rlastsolid=-1,rcount=0;
int zlast=0,zlastsolid=-1,zcount=0;
int alast=0,alastsolid=-1,acount=0;

//Define PUMP Relay IO
#define RelayPump 16

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


long value0;
long value1;
long value2;
long value3;
long value4;


int main() 
{

	
	int Change1, Change2, NewPos, Pos, Factor;
	int InMotion=FALSE,Axis,LastAxis=-1;
	double LastChangeTime=0,Target,Factor=0, LastF, LastRr, LastS;
	SetBitDirection(16,1);

	//Y
	ch0->InputMode=NO_INPUT_MODE;
	ch0->OutputMode=STEP_DIR_MODE;
	ch0->Vel=90000;
	ch0->Accel=800000;
	ch0->Jerk=3e+06;
	ch0->P=0;
	ch0->I=0.01;
	ch0->D=0;
	ch0->FFAccel=0;
	ch0->FFVel=0;
	ch0->MaxI=200;
	ch0->MaxErr=1e+06;
	ch0->MaxOutput=200;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=0;
	ch0->InputChan1=0;
	ch0->OutputChan0=0;
	ch0->OutputChan1=0;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x100;
	ch0->LimitSwitchNegBit=0;
	ch0->LimitSwitchPosBit=0;
	ch0->SoftLimitPos=10e999;
	ch0->SoftLimitNeg=-10e999;
	ch0->InputGain0=1;
	ch0->InputGain1=1;
	ch0->InputOffset0=0;
	ch0->InputOffset1=0;
	ch0->OutputGain=1;
	ch0->OutputOffset=0;
	ch0->SlaveGain=1;
	ch0->BacklashMode=BACKLASH_OFF;
	ch0->BacklashAmount=0;
	ch0->BacklashRate=0;
	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.000768788;
	ch0->iir[2].B1=0.00153758;
	ch0->iir[2].B2=0.000768788;
	ch0->iir[2].A1=1.92076;
	ch0->iir[2].A2=-0.923833;
	ch0->Position = ch0->Dest;
	EnableAxis(0);

	//X
	ch1->InputMode=NO_INPUT_MODE;
	ch1->OutputMode=STEP_DIR_MODE;
	ch1->Vel=90000;
	ch1->Accel=800000;
	ch1->Jerk=3e+06;
	ch1->P=0;
	ch1->I=0.01;
	ch1->D=0;
	ch1->FFAccel=0;
	ch1->FFVel=0;
	ch1->MaxI=200;
	ch1->MaxErr=1e+06;
	ch1->MaxOutput=200;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=1;
	ch1->InputChan1=0;
	ch1->OutputChan0=1;
	ch1->OutputChan1=0;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x100;
	ch1->LimitSwitchNegBit=0;
	ch1->LimitSwitchPosBit=0;
	ch1->SoftLimitPos=10e999;
	ch1->SoftLimitNeg=-10e999;
	ch1->InputGain0=1;
	ch1->InputGain1=1;
	ch1->InputOffset0=0;
	ch1->InputOffset1=0;
	ch1->OutputGain=1;
	ch1->OutputOffset=0;
	ch1->SlaveGain=1;
	ch1->BacklashMode=BACKLASH_OFF;
	ch1->BacklashAmount=0;
	ch1->BacklashRate=0;
	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;
	ch1->Position = ch1->Dest;
    EnableAxis(1);

	//Z
	ch2->InputMode=NO_INPUT_MODE;
	ch2->OutputMode=STEP_DIR_MODE;
	ch2->Vel=240000;
	ch2->Accel=4000000;
	ch2->Jerk=4e+06;
	ch2->P=0;
	ch2->I=0.01;
	ch2->D=0;
	ch2->FFAccel=0;
	ch2->FFVel=0;
	ch2->MaxI=200;
	ch2->MaxErr=1e+06;
	ch2->MaxOutput=200;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=0;
	ch2->OutputChan0=2;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x100;
	ch2->LimitSwitchNegBit=0;
	ch2->LimitSwitchPosBit=0;
	ch2->SoftLimitPos=10e999;
	ch2->SoftLimitNeg=-10e999;
	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;
	ch2->Position = ch2->Dest;
    EnableAxis(2);


	//A
	ch3->InputMode=NO_INPUT_MODE;
	ch3->OutputMode=STEP_DIR_MODE;
	ch3->Vel=40000;
	ch3->Accel=400000;
	ch3->Jerk=4e+06;
	ch3->P=0;
	ch3->I=0.01;
	ch3->D=0;
	ch3->FFAccel=0;
	ch3->FFVel=0;
	ch3->MaxI=200;
	ch3->MaxErr=1e+06;
	ch3->MaxOutput=200;
	ch3->DeadBandGain=1;
	ch3->DeadBandRange=0;
	ch3->InputChan0=3;
	ch3->InputChan1=0;
	ch3->OutputChan0=3;
	ch3->OutputChan1=0;
	ch3->MasterAxis=-1;
	ch3->LimitSwitchOptions=0x100;
	ch3->LimitSwitchNegBit=0;
	ch3->LimitSwitchPosBit=0;
	ch3->SoftLimitPos=1e+09;
	ch3->SoftLimitNeg=-1e+09;
	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;
	ch3->Position = ch3->Dest;
    EnableAxis(3);


	//Spindle
	ch4->InputMode=ENCODER_MODE;
	ch4->OutputMode=STEP_DIR_MODE;
	ch4->Vel=4000000;
	ch4->Accel=500000; //100000
	ch4->Jerk=4e+10;
	ch4->P=0.0; //1.5
	ch4->I=0.0000;  //0.001
	ch4->D=0;
	ch4->FFAccel=0;
	ch4->FFVel=0;
	ch4->MaxI=200000;
	ch4->MaxErr=4000;
	ch4->MaxOutput=00000;
	ch4->DeadBandGain=0;
	ch4->DeadBandRange=0;
	ch4->InputChan0=1;
	ch4->InputChan1=1;
	ch4->OutputChan0=21;
	ch4->OutputChan1=1;
	ch4->MasterAxis=-1;
	ch4->LimitSwitchOptions=0x100;
	ch4->LimitSwitchNegBit=0;
	ch4->LimitSwitchPosBit=0;
	ch4->SoftLimitPos=1e+999;
	ch4->SoftLimitNeg=-1e+999;
	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=4e+100;
	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;
	ch4->Position = ch4->Dest;
    EnableAxis(4);
	
	
	//MPG
	ch7->InputMode=ENCODER_MODE;
	ch7->OutputMode=NO_OUTPUT_MODE;
	ch7->Vel=40000;
	ch7->Accel=400000;
	ch7->Jerk=4e+06;
	ch7->P=0.2;
	ch7->I=0;
	ch7->D=0;
	ch7->FFAccel=0;
	ch7->FFVel=0;
	ch7->MaxI=200;
	ch7->MaxErr=200;
	ch7->MaxOutput=200;
	ch7->DeadBandGain=1;
	ch7->DeadBandRange=0;
	ch7->InputChan0=2;
	ch7->InputChan1=1;
	ch7->OutputChan0=7;
	ch7->OutputChan1=1;
	ch7->MasterAxis=-1;
	ch7->LimitSwitchOptions=0x100;
	ch7->LimitSwitchNegBit=0;
	ch7->LimitSwitchPosBit=0;
	ch7->SoftLimitPos=1e+09;
	ch7->SoftLimitNeg=-1e+09;
	ch7->InputGain0=1;
	ch7->InputGain1=1;
	ch7->InputOffset0=0;
	ch7->InputOffset1=0;
	ch7->OutputGain=1;
	ch7->OutputOffset=0;
	ch7->SlaveGain=1;
	ch7->BacklashMode=BACKLASH_OFF;
	ch7->BacklashAmount=0;
	ch7->BacklashRate=0;
	ch7->invDistPerCycle=1;
	ch7->Lead=0;
	ch7->MaxFollowingError=10000000;
	ch7->StepperAmplitude=250;

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

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

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

	//ConfigSpindle(1,4,0.1,0.1,10000);
	DefineCoordSystem(1,0,2,3);
	
	//Muxing of encoders
	//#define ENC_0_3_JP4 0x200
	//FPGAW(ENC_NOISE_FILTER_ADD) = ENC_0_3_JP4 + 50;
	FPGAW(ENC_NOISE_FILTER_ADD)=25;
	
	//Setting length of the step/dir
	FPGA(STEP_PULSE_LENGTH_ADD)=32;
	
	//SetFRO(1); //Set hardware FRO to 1, under 100% immeditate effect
	
		
    //printf("Init performed");
	int result;
	
	Pos = chan[MPG_INPUT_AXIS].Position;

	for (;;) // loop forever for buttons
	{
		WaitNextTimeSlice();
		
		// Handle FeedHold/Resume
		result = Debounce(ReadBit(FEEDHOLDBIT),&fcount,&flast,&flastsolid);
		if  (result == 1)
		{
			if (CS0_StoppingState == 0)
				StopCoordinatedMotion();
			else
				ResumeCoordinatedMotion();
		}

		// Handle Cycle Start
		result = Debounce(ReadBit(CYCLESTARTBIT),&ccount,&clast,&clastsolid);
		if  (result == 1)
		{
			DoPC(PC_COMM_EXECUTE);
		}

		// Handle ESTOP
		result = Debounce(ReadBit(ESTOP),&ecount,&elast,&elastsolid);
		//if  (result == 0)
		if (!ReadBit(ESTOP))
		if (!ReadBit(ESTOP))
		if (!ReadBit(ESTOP))
		if (!ReadBit(ESTOP))
		if (!ReadBit(ESTOP))
		if (!ReadBit(ESTOP))
		if (!ReadBit(ESTOP))
		if (!ReadBit(ESTOP))
		if (!ReadBit(ESTOP))
		if (!ReadBit(ESTOP))
		{
			printf("Estop");
			//Delay_sec(1);
			DoPC(PC_COMM_ESTOP);
		}
		
		// Handle HALT
		//result = Debounce(ReadBit(HALTBIT),&hcount,&hlast,&hlastsolid);
		//if  (result == 1)
		//{
		//	DoPC(PC_COMM_HALT);
		//}
		
		// Handle RESTART
		//result = Debounce(ReadBit(RESTARTBIT),&rcount,&rlast,&rlastsolid);
		//if  (result == 1)
		//{
		//	DoPC(PC_COMM_RESTART);
		//}
		
		
		// Handle ALARM
		result = Debounce(ReadBit(ALARMBIT),&acount,&alast,&alastsolid);
		if  (result == 1)
		{
			printf("Estop - ALARM");
			DoPC(PC_COMM_ESTOP);
		}
		
		NewPos = chan[MPG_INPUT_AXIS].Position;
		Change1 = NewPos - Pos;
		Change2 = NewPos - Pos;
		Pos = NewPos;     
		Factor = 10*LastF;

		if (ReadBit(SELECTX))  // is X selected?
		{
			Axis=1;
			DirMPG=-1;
		}
		else if (ReadBit(SELECTY))  // is Y selected?
		{
			Axis=0;
			DirMPG=-1;
		}
		else if (ReadBit(SELECTZ))  // is Z selected?
		{
			Axis=2;
			DirMPG=1;
		}
		else if (ReadBit(SELECTA))  // is A selected?
		{
			Axis=3;
			DirMPG=1;
		}
		else //if (ReadBit(Fro)) // FRO select
			Change1=0;
			
		// 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,Target);  //finalize any motion
	
			LastAxis = Axis;
			InMotion = FALSE;
		}
		
		if (Change1) // did we move?
		{
			if (!InMotion) Target = chan[Axis].Dest;
			Target += Change1 * Factor * DirMPG;
			MoveExp(Axis,Target,TAU);  // note: contains a WaitNextTimeSlice
			LastChangeTime = Time_sec();
			InMotion=TRUE;
		}
		else
		{
			WaitNextTimeSlice();
		}		
	
		if (ReadBit(Fro))  // is FRO selected? RRO SSO
		{
			//ScreenScript("WinMsg:DlgName;IDC_Feed;BM_CLICK;");
			if (ScreenScript("WinMsg:DlgName;IDC_Feed;BM_CLICK;"))
				printf("Screen Script Failed\n");
			else
				printf("Screen Script Success\n");
				
			LastF=LastF+0.0025*Change2;
		
			if (LastF<0)
			{
				LastF=0;
			}
			if (LastF>2)
			{
				LastF=2;
			}
			DoPCFloat(PC_COMM_SET_FRO,LastF);
		}

		if (ReadBit(Rro))  // is RRO selected? RRO SSO
		{
			ScreenScript("WinMsg:DlgName;IDC_Rapid;BM_CLICK;");
			LastRr=LastRr+0.0025*Change2;
			if (LastRr<0)
			{
				LastRr=0;
			}
			if (LastRr>1.1)
			{
				LastRr=1.1;
			}		
			DoPCFloat(PC_COMM_SET_RRO,LastRr);

		}	
		
			if (ReadBit(Sso))  // is RRO selected? RRO SSO
		{
			LastS=LastS+0.0025*Change2;		
			if (LastS<0.7)
			{
				LastS=0.7;
			}
			if (LastS>1.3)
			{
				LastS=1.3;
			}
			DoPCFloat(PC_COMM_SET_SSO,LastS);

		}
		
	}

	

    return 0;
    
}


// Put a Float as a parameter and pass the command to the App
int DoPCFloat(int cmd, float f)
{
	int result;
	persist.UserData[PC_COMM_PERSIST+1] = *(int*)&f;
	return DoPC(cmd);
}


// Pass a command to the PC and wait for it to handshake
// that it was received by either clearing the command
// or changing it to a negative error code
int DoPC(int cmd)
{
	int result;
	
	persist.UserData[PC_COMM_PERSIST]=cmd;
	
	do
	{
		WaitNextTimeSlice();	
	}while (result=persist.UserData[PC_COMM_PERSIST]>0);
	
	//printf("Result = %d\n",result);

	return result;
}

// Debounce a bit
//
// return 1 one time when first debounced high 
// return 0 one time when first debounced low 
// return -1 otherwise 
#define DBTIME 100

int Debounce(int n, int *cnt, int *last, int *lastsolid)
{
	int v = -1;
	
	if (n == *last)  // same as last time?
	{
		if (*cnt == DBTIME-1)
		{
			if (n != *lastsolid)
			{
				v = *lastsolid = n;  // return debounced value
			}
		}
		if (*cnt < DBTIME)	(*cnt)++;
	}
	else
	{
		*cnt = 0;  // reset count
	}
	*last = n;
	return v;
}