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

#define CYCLESTARTBIT 16
#define FEEDHOLDBIT 17
#define ESTOP 136
#define COOLANTBIT 18
#define SPINDLESTOPBIT 21
#define SPINDLECWBIT 19
#define SPINDLECCWBIT 20
#define SDERRORLINEBIT 143
#define ZMINUSLIMITBIT 141
#define ZPLUSLIMITBIT 142
#define YMINUSLIMITBIT 140
#define YPLUSLIMITBIT 139
#define XMINUSLIMITBIT 138
#define XPLUSLIMITBIT 137

#define X 0
#define Y 1
#define Z 2
//#define A 3

#define MAX_JOG_SPEED_X 240.0 // ipm
#define MAX_JOG_SPEED_Y 240.0 // ipm
#define MAX_JOG_SPEED_Z 240.0 // ipm

void UpdateJogSpeeds(void);
void DoLimitJog(double rate, int cmd);

void ServiceTimerSequence(void); 
void ServiceEStop(void);



// 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 ccwlast=0,ccwlastsolid=-1,ccwcount=0;
int cwlast=0,cwlastsolid=-1,cwcount=0;
int colast=0,colastsolid=-1,cocount=0;
int solast=0,solastsolid=-1,socount=0;
int sdelast=0,sdelastsolid=-1,sdecount=0;
int zmlast=0,zmlastsolid=-1,zmcount=0;
int zplast=0,zplastsolid=-1,zpcount=0;
int ymlast=0,ymlastsolid=-1,ymcount=0;
int yplast=0,yplastsolid=-1,ypcount=0;
int xmlast=0,xmlastsolid=-1,xmcount=0;
int xplast=0,xplastsolid=-1,xpcount=0;
int clast=0,clastsolid=-1,ccount=0;
int elast=0,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 Debounce(int n, int *cnt, int *last, int *lastsolid);

main()
{
	
    #define DROIN 40
    double *pin  = (double *)&persist.UserData[(DROIN -1)*2];
    
	int result;
	int Answer;

	ch0->InputMode=NO_INPUT_MODE;
	ch0->OutputMode=STEP_DIR_MODE;
	ch0->Vel=21000;
	ch0->Accel=300000;
	ch0->Jerk=2.5e+006;
	ch0->P=1;
	ch0->I=1;
	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=0;
	ch0->InputChan1=0;
	ch0->OutputChan0=12;
	ch0->OutputChan1=0;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x113;
	ch0->LimitSwitchNegBit=138;
	ch0->LimitSwitchPosBit=137;
	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=1;
	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.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;
 

	ch1->InputMode=NO_INPUT_MODE;
	ch1->OutputMode=STEP_DIR_MODE;
	ch1->Vel=21000;
	ch1->Accel=250000;
	ch1->Jerk=2.5e+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=0;
	ch1->InputChan1=0;
	ch1->OutputChan0=13;
	ch1->OutputChan1=0;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x113;
	ch1->LimitSwitchNegBit=140;
	ch1->LimitSwitchPosBit=139;
	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=10.;
	ch1->BacklashRate=100;
	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;
  

	ch2->InputMode=NO_INPUT_MODE;
	ch2->OutputMode=STEP_DIR_MODE;
	ch2->Vel=21000;
	ch2->Accel=250000;
	ch2->Jerk=2.5e+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=0;
	ch2->InputChan1=0;
	ch2->OutputChan0=14;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x11f;
	ch2->LimitSwitchNegBit=141;
	ch2->LimitSwitchPosBit=142;
	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_LINEAR;
	ch2->BacklashAmount=5;
	ch2->BacklashRate=100;
	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(0,ch0->Position);
	EnableAxisDest(0,ch0->Dest);
	//EnableAxisDest(1,ch1->Position);
	EnableAxisDest(1,ch1->Dest);
	//EnableAxisDest(2,ch2->Position);
	EnableAxisDest(2,ch2->Dest);
	
for (;;) // loop forever
    {
        WaitNextTimeSlice(); // execute loop once every time slice
        ServiceTimerSequence();  // service the timer sequencing
        UpdateJogSpeeds();
    }   // end of forever loop
}

 //set Rate within Limits
void DoLimitJog(double rate, int cmd)
{
	if (rate < 0.0) rate=0.0;
	if (rate > 1.0) rate=1.0;
	
	DoPCFloat(cmd, rate);
}

void UpdateJogSpeeds(void)
{
	int Units, TWORD, HWORD, DWORD;
	double rate;
	
	// Read double from a KMotionCNC Edit Control
	// Persist Var identifies the Control and contents specifies 
	// where the string data should be placed in the 
	// Gather Buffer as an offset in words
	if (GetEditControlDouble(&rate, 170, 1000)) return;  // exit if value is invalid
	
	GetMiscSettings(&Units, &TWORD, &HWORD, &DWORD); // check Units
	if (Units == CANON_UNITS_MM) rate = rate/25.4; // convert screen value to ipm	
	DoLimitJog(rate/MAX_JOG_SPEED_X, PC_COMM_SET_JOG_OVERRIDE_X);
	DoLimitJog(rate/MAX_JOG_SPEED_Y, PC_COMM_SET_JOG_OVERRIDE_Y);
	DoLimitJog(rate/MAX_JOG_SPEED_Z, PC_COMM_SET_JOG_OVERRIDE_Z);

}

 
// sequence an IO Bit Based on Time in a non-blocking manner WAY LUBE PUMP
 
#define TIME_ON 300.0//seconds
#define CYCLE_TIME 3600.0 //seconds
#define OUTPUT_BIT 155 // which IO bit to drive
void ServiceTimerSequence(void)
{
    static double T0=0.0;  // remember the last time we turned on
    double T=Time_sec(); // get current Time_sec
     
    if (T0==0.0 || T > T0 + CYCLE_TIME) T0=T;  // set start time of cycle
     
    if (T < T0 + TIME_ON)  // are we within the TIME_ON section of the cycle?
        SetBit(OUTPUT_BIT);  //yes
    else
        ClearBit(OUTPUT_BIT);  //no
        
        
        
        
        
        int result;
        
// 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 Coolant Pump
		result = Debounce(ReadBit(COOLANTBIT),&cocount,&colast,&colastsolid);
		if  (result == 1)
		{
			if (ReadBit(154) == 1)
				ClearBit(154);
			else
				SetBit(154);
		}

		// Handle Spindle CW
		result = Debounce(ReadBit(SPINDLECWBIT),&cwcount,&cwlast,&cwlastsolid);
		if  (result == 1)
		{
			SetBit(152);
		}
		// Handle Spindle CCW
		result = Debounce(ReadBit(SPINDLECCWBIT),&ccwcount,&ccwlast,&ccwlastsolid);
		if  (result == 1)
		{
			SetBit(153);
		}
		// Handle Spindle OFF
		result = Debounce(ReadBit(SPINDLESTOPBIT),&socount,&solast,&solastsolid);
		if  (result == 1)
		{
			ClearBit(152);
			ClearBit(153);
		}
		// Handle ESTOP
		result = Debounce(ReadBit(ESTOP),&ecount,&elast,&elastsolid);
		if  (result == 0)
		{
			DoPC(PC_COMM_ESTOP);
			ClearBit(152);
			ClearBit(153);
			ClearBit(154);
			MsgBox("E-STOP Pressed",MB_OK|MB_ICONEXCLAMATION);
		}
		// Handle Servo Drives Error Line
		result = Debounce(ReadBit(SDERRORLINEBIT),&sdecount,&sdelast,&sdelastsolid);
		if  (result == 0)
		{
			DoPC(PC_COMM_ESTOP);
			ClearBit(152);
			ClearBit(153);
			ClearBit(154);
			MsgBox("ServoDrive Fault",MB_OK|MB_ICONEXCLAMATION);	
		}
		
		// Handle axis Z minus limit switch
		result = Debounce(ReadBit(ZMINUSLIMITBIT),&zmcount,&zmlast,&zmlastsolid);
		if  (result == 0)
		{
			ClearBit(152);
			ClearBit(153);
			ClearBit(154);
			MsgBox("Z - Limit. Jog opposite direction.",MB_OK|MB_ICONEXCLAMATION);	
		}
		// Handle axis Z plus limit switch
		result = Debounce(ReadBit(ZPLUSLIMITBIT),&zpcount,&zplast,&zplastsolid);
		if  (result == 0)
		{
			ClearBit(152);
			ClearBit(153);
			ClearBit(154);
			MsgBox("Z + Limit. Jog opposite direction.",MB_OK|MB_ICONEXCLAMATION);	
		}
		// Handle axis Y minus limit switch
		result = Debounce(ReadBit(YMINUSLIMITBIT),&ymcount,&ymlast,&ymlastsolid);
		if  (result == 1)
		{
			ClearBit(152);
			ClearBit(153);
			ClearBit(154);
			MsgBox("Y - Limit. Jog opposite direction.",MB_OK|MB_ICONEXCLAMATION);	
		}
		// Handle axis Y plus limit switch
		result = Debounce(ReadBit(YPLUSLIMITBIT),&ypcount,&yplast,&yplastsolid);
		if  (result == 1)
		{
			ClearBit(152);
			ClearBit(153);
			ClearBit(154);
			MsgBox("Y + Limit. Jog opposite direction.",MB_OK|MB_ICONEXCLAMATION);	
		}
		// Handle axis X minus limit switch
		result = Debounce(ReadBit(XMINUSLIMITBIT),&xmcount,&xmlast,&xmlastsolid);
		if  (result == 1)
		{
			ClearBit(152);
			ClearBit(153);
			ClearBit(154);
			MsgBox("X - Limit. Jog opposite direction.",MB_OK|MB_ICONEXCLAMATION);	
		}
		// Handle axis X plus limit switch
		result = Debounce(ReadBit(XPLUSLIMITBIT),&xpcount,&xplast,&xplastsolid);
		if  (result == 1)
		{
			ClearBit(152);
			ClearBit(153);
			ClearBit(154);
			MsgBox("X + Limit. Jog opposite direction.",MB_OK|MB_ICONEXCLAMATION);	
		}
		// Handle HALT
		//result = Debounce(ReadBit(HALTBIT),&hcount,&hlast,&hlastsolid);
		//if  (result == 1)
		{
			
		}
	}

		// 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 300

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;


}