//V3 Added physical buttons for CycleStart,FeedHold,
//V3 Added physical buttons for MainINIT/PowerOn, SpindleCW,CCW,Off.
//V5 Added Feed,Rapid and Spindle potentiometers overrides.
//V5.1 Code executes a FeedHold when override pots are set to minimum.
//V5.2 Tom added TOOL_CHANGE_REQUEST 1056. M6 program set a Virtual Bit to request that the Init Program perform the KMotionCNC commands. That would result in only one Thread sending commands.
//V5.3 Added CSS code

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


#define INITPOWERONBIT 1026
#define CYCLESTARTBIT 1024
#define FEEDHOLDBIT 1025
#define ESTOP 137
#define FLOODCOOLANTBIT 157
#define VFDERRORLINEBIT 138
#define XSDERRORLINEBIT 139
#define ZSDERRORLINEBIT 141
#define SPINDLESTOPBIT 1028//NormallyClosed Button
#define SPINDLECWBIT 1027
#define SPINDLECCWBIT 1029
#define TOOL_CHANGE_REQUEST 1056

//#define XMINUSLIMITBIT 1025
//#define XPLUSLIMITBIT 1024
//#define ZMINUSLIMITBIT 1029
//#define ZPLUSLIMITBIT 1028

#define X 0
#define Z 2

#define MAX_JOG_SPEED_X 300.0 // ipm
#define MAX_JOG_SPEED_Z 300.0 // ipm

// From SetFROwithpot.c and SetSSOwithpot.c
#define CHANGE_TOL 0.02  // only update if change is more than this
#define CHANGE_TIME 0.05  // don't update more often then this time
double LastFRO=-1;
double LastFROTime=0;
double LastSSO=-1;
double LastSSOTime=0;
// end set FRO and SSO

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

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


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

// state variables for switch debouncing
int inplast=0,inplastsolid=-1,inpcount=0;
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 xsdelast=0,xsdelastsolid=-1,xsdecount=0;
int ysdelast=0,ysdelastsolid=-1,ysdecount=0;
int zsdelast=0,zsdelastsolid=-1,zsdecount=0;
int vfdelast=0,vfdelastsolid=-1,vfdecount=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 xpjblast=0,xpjblastsolid=-1,xpjbcount=0;



//int Debounce(int n, int *cnt, int *last, int *lastsolid);

main()
{
	//From SetFROwithpot.c and SetSSOwithpot.c
	double Pot1,Pot2,Pot3,FRO,SSO,RAPIDO,T;
	//end set FRO and SSO
	InitAux();
	AddKonnect(0,&VirtualBits,VirtualBitsEx);
	int BitA,Change1=0,Change2=0, DiffX2;
	int PosNoWrap, NewPos, Pos=0, wraps;
	int InMotion=FALSE,Axis,LastAxis=-1;
	double LastChangeTime=0,Target,Factor=0;


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

	int result;
	int Answer;

	ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=DAC_SERVO_MODE;
	ch0->Vel=340000;
	ch0->Accel=3e+06;
	ch0->Jerk=3e+07;
	ch0->P=17;
	ch0->I=0.015;
	ch0->D=0;
	ch0->FFAccel=0.0002;
	ch0->FFVel=0.0002;
	ch0->MaxI=2000;
	ch0->MaxErr=1000;
	ch0->MaxOutput=2000;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=0;
	ch0->InputChan1=1;
	ch0->OutputChan0=0;
	ch0->OutputChan1=1;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x100;
	ch0->LimitSwitchNegBit=0;
	ch0->LimitSwitchPosBit=0;
	ch0->SoftLimitPos=1e+09;
	ch0->SoftLimitNeg=-1e+09;
	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=100;
	ch0->StepperAmplitude=250;

	ch0->iir[0].B0=14.8496;
	ch0->iir[0].B1=-28.9467;
	ch0->iir[0].B2=14.1066;
	ch0->iir[0].A1=1.8047;
	ch0->iir[0].A2=-0.814233;

	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.016609;
	ch0->iir[2].B1=0.033219;
	ch0->iir[2].B2=0.016609;
	ch0->iir[2].A1=1.60679;
	ch0->iir[2].A2=-0.673229;


	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=DAC_SERVO_MODE;
	ch2->Vel=340000;
	ch2->Accel=3.5e+06;
	ch2->Jerk=2.5e+07;
	ch2->P=19;
	ch2->I=0.017;
	ch2->D=0;
	ch2->FFAccel=0.0003;
	ch2->FFVel=0.0007;
	ch2->MaxI=2000;
	ch2->MaxErr=1000;
	ch2->MaxOutput=2000;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=0;
	ch2->OutputChan0=2;
	ch2->OutputChan1=1;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x100;
	ch2->LimitSwitchNegBit=0;
	ch2->LimitSwitchPosBit=0;
	ch2->SoftLimitPos=1e+09;
	ch2->SoftLimitNeg=-1e+09;
	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=100;
	ch2->StepperAmplitude=250;

	ch2->iir[0].B0=14.8496;
	ch2->iir[0].B1=-28.9467;
	ch2->iir[0].B2=14.1066;
	ch2->iir[0].A1=1.8047;
	ch2->iir[0].A2=-0.814233;

	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.016609;
	ch2->iir[2].B1=0.033219;
	ch2->iir[2].B2=0.016609;
	ch2->iir[2].A1=1.60679;
	ch2->iir[2].A2=-0.673229;


	EnableAxisDest(0,ch0->Position);
	EnableAxisDest(2,ch2->Position);
	
		ch7->InputMode=ENCODER_MODE;
	ch7->OutputMode=DAC_SERVO_MODE;
	ch7->Vel=0.5;
	ch7->Accel=1;
	ch7->Jerk=5;
	ch7->P=0;
	ch7->I=0;
	ch7->D=0;
	ch7->FFAccel=0;
	ch7->FFVel=2047;
	ch7->MaxI=2047;
	ch7->MaxErr=1e+10;
	ch7->MaxOutput=2047;
	ch7->DeadBandGain=1;
	ch7->DeadBandRange=0;
	ch7->InputChan0=3;
	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;


	SetBit(144);//Enables Main Contactor Relay1 for servo drives with SWE kanalog output.
	// once the RESET button is clicked.
	SetBit(145);//Enables Relay2 for enable/disable of servo drives and VFDwith SWE 
	// kanalog output. And controled by Estop switch.
	DefineCoordSystem(0,-1,2,-1);//DefineXZ(Y,A not used)
	SetBit(147); // turn off spindle clockwise
	SetBit(148); // turn off spindle counterclockwise

	BOOL PotSetFeedhold=FALSE;

	ClearBit(TOOL_CHANGE_REQUEST);

	for (;;) // loop forever
	{		
ServiceCSS();

		//From SetFROwithpot.c and SetSSOwithpot.c
		T = WaitNextTimeSlice();		
		Pot1 = (double)KANALOG_CONVERT_ADC_TO_VOLTS(ADC(0));
		Pot2 = (double)KANALOG_CONVERT_ADC_TO_VOLTS(ADC(1));
		Pot3 = (double)KANALOG_CONVERT_ADC_TO_VOLTS(ADC(2));
		FRO = Pot3 * 0.217;
		RAPIDO = Pot2 * 0.117;
		SSO = Pot1 * 0.17;

		if (FRO <= .1||RAPIDO <= .1)
		{
			StopCoordinatedMotion();
			PotSetFeedhold = TRUE;
		}
		else if (PotSetFeedhold)
		{
			ResumeCoordinatedMotion();
			PotSetFeedhold = FALSE;
		}

		if (FRO > 2.0) FRO = 2.0; // limit FRO to 2.0 max
		if (FRO > 0.95 && FRO < 1.05 ) FRO = 1.0; // Deadband around 100% 
		if (SSO > 1.5) SSO = 1.5; // limit SSO to 1.5 max
		if (SSO > 0.95 && SSO < 1.05 ) SSO = 1.0; // Deadband around 100%	
		if (RAPIDO > 1.00) RAPIDO = 1.00; // Limit rapid override to 1.00

		SetRapidFRO (RAPIDO);
		// send message to KMotionCNC if the pot changed significantly
		// and it has been a while since the last message
		if ((FRO > LastFRO+CHANGE_TOL || FRO < LastFRO-CHANGE_TOL) && 
			T > LastFROTime+CHANGE_TIME)
		{
			DoPCFloat(PC_COMM_SET_FRO,FRO);
			LastFRO=FRO;
			LastFROTime=T;
		}		
		if ((SSO > LastSSO+CHANGE_TOL || SSO < LastSSO-CHANGE_TOL) && 
			T > LastSSOTime+CHANGE_TIME)
		{
			DoPCFloat(PC_COMM_SET_SSO,SSO);
			LastSSO=SSO;
			LastSSOTime=T;
		}

		WaitNextTimeSlice(); // execute loop once every time slice
		// check if all axes are enabled
		if (ch0->Enable && ch2->Enable)
		{
			SetBit(145);// yes they are, enable the amplifier
			SetBit(157);//Enables waylube relay
			// once the RESET button is clicked.
		}
		else
		{
			ClearBit(145);  // no at least one is disabled, disable the amplifier
			ClearBit(157); // Waylube relay
		}
		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)
{
	static LastChangeCount=-1;
	int Units, TWORD, HWORD, DWORD;
	double rate;

	// don't bother if nothing changed
	if (EditChangeCount == LastChangeCount) return;

	// remember count before updating
	LastChangeCount = EditChangeCount;

	// 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_Z, PC_COMM_SET_JOG_OVERRIDE_Z);


}
void ServiceTimerSequence(void)
{    
	int result;
	/// Handle FeedHold/Resume
	result = Debounce(ReadBit(FEEDHOLDBIT),&fcount,&flast,&flastsolid);
	if  (result == 1)
	{
		if (CS0_StoppingState == 0)
			StopCoordinatedMotion();
	}

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

	/// Handle Main INIT Power On
	result = Debounce(ReadBit(INITPOWERONBIT),&inpcount,&inplast,&inplastsolid);
	if  (result == 1)
	{
		DoPCInt(PC_COMM_USER_BUTTON,0);
	}


	/// Handle Spindle on CW
	result = Debounce(ReadBit(SPINDLECWBIT),&cwcount,&cwlast,&cwlastsolid);
	if  (result == 1)  // If spindle CW button is pushed
	{
		int Answer=IDYES; // assume ok to turn on
		if (!ReadBit(48))// is the virtual bit not set indicating the Operator hasn't already answered yes? 
			Answer = MsgBox("Is 3phase Converter On?",MB_YESNO|MB_ICONEXCLAMATION);

		if (Answer == IDYES)  // phase converter is on
		{
			//printf("Answer is Yes\n");
			SetBit(48);  // set Virtual bit to not ask again
			ClearBit(148); //Spindle CCW bit
			Delay_sec(.2);
			SetBit(147); // turn on spindle clockwise
		}
		else       // phase converter is off
		{
			//printf("Answer is No\n");
			SetBit(147); // turn off spindle clockwise
			SetBit(148); // turn off spindle counterclockwise

			// if running a Job and Spindle isn't to be turned on Halt the Job
			if (JOB_ACTIVE) DoPC(PC_COMM_HALT);
		}
	}


	/// Handle Spindle on CCW
	result = Debounce(ReadBit(SPINDLECCWBIT),&ccwcount,&ccwlast,&ccwlastsolid);
	if  (result == 1)
	{
		int	Answer=IDYES; // assume ok to turn on
		if (!ReadBit(48))// is the virtual bit not set indicating the Operator hasn't already answered yes? 
			Answer = MsgBox("Is 3phase Converter On?",MB_YESNO|MB_ICONEXCLAMATION);

		if (Answer == IDYES)
		{
			//printf("Answer is Yes\n");
			SetBit(48);  // set Virtual bit to not ask again
			ClearBit(147); //Spindle CW bit
			Delay_sec(.2);
			SetBit(148); // turn on spindle CCW
		}
		else
		{
			//printf("Answer is No\n");
			SetBit(147); // turn off spindle clockwise
			SetBit(148); // turn off spindle counterclockwise

			// if running a Job and Spindle isn't to be turned on Halt the Job
			if (JOB_ACTIVE)
				DoPC(PC_COMM_HALT);
		}
	}

	/// Handle Spindle on OFF
	result = Debounce(ReadBit(SPINDLESTOPBIT),&socount,&solast,&solastsolid);
	if  (result == 0)
	{
		SetBit(147); // turn off spindle clockwise
		SetBit(148); // turn off spindle counterclockwise
	}

	/// Handle ESTOP
	result = Debounce(ReadBit(ESTOP),&ecount,&elast,&elastsolid);
	if  (result == 0)
	{
		DoPC(PC_COMM_ESTOP);
		ClearBit(145);//Disable Servo Drives & vfd
		ClearBit(159);//Turn off FloodCoolant
		ClearBit(157);//Tur off WayLubeRelay
		SetBit(147); // turn off spindle clockwise
		SetBit(148); // turn off spindle counterclockwise
		MsgBox("E-STOP Condition",MB_OK|MB_ICONEXCLAMATION);
	}

	// Handle X Axis Servo Drives Error Line
	result = Debounce(ReadBit(XSDERRORLINEBIT),&xsdecount,&xsdelast,&xsdelastsolid);
	if  (result == 1)
	{
		DoPC(PC_COMM_ESTOP);
		ClearBit(145);//Disable Servo Drives & vfd
		SetBit(147); // turn off spindle clockwise
		SetBit(148); // turn off spindle counterclockwise
		ClearBit(159);//Turn off FloodCoolant		
		MsgBox("X ServoDrive Fault",MB_OK|MB_ICONEXCLAMATION);	
		ClearBit(144);//Enable/Disable MainContactorRelay for servo drive main & logic power.
	}

	// Handle Z Axis Servo Drives Error Line
	result = Debounce(ReadBit(ZSDERRORLINEBIT),&zsdecount,&zsdelast,&zsdelastsolid);
	if  (result == 1)
	{
		DoPC(PC_COMM_ESTOP);
		ClearBit(145);//Disable Servo Drives & vfd
		SetBit(147); // turn off spindle clockwise
		SetBit(148); // turn off spindle counterclockwise
		ClearBit(159);//Turn off FloodCoolant
		MsgBox("Z ServoDrive Fault",MB_OK|MB_ICONEXCLAMATION);
		ClearBit(144);//Enable/Disable MainContactorRelay for servo drive main & logic power.
	}

	/// Handle VFD Error Output
	result = Debounce(ReadBit(VFDERRORLINEBIT),&vfdecount,&vfdelast,&vfdelastsolid);
	if  (result == 1)
	{
		DoPC(PC_COMM_ESTOP);
		ClearBit(145);//Disable Servo Drives & vfd
		SetBit(147); // turn off spindle clockwise
		SetBit(148); // turn off spindle counterclockwise
		ClearBit(159);//Turn off FloodCoolant		
		MsgBox("VFD Fault",MB_OK|MB_ICONEXCLAMATION);	
	}
	
	
	/// Handle Tool Change
	if  (ReadBit(TOOL_CHANGE_REQUEST))
	{
		DoPC(PC_COMM_HALT_NEXT_LINE);
		MsgBox("!!!Change Tool!!!",MB_OK|MB_ICONEXCLAMATION);
		ClearBit(TOOL_CHANGE_REQUEST);
	}

	// Handle axis X minus limit switch
	//result = Debounce(ReadBit(XMINUSLIMITBIT),&xmcount,&xmlast,&xmlastsolid);
	//if  (result == 1)
	//{
	//ClearBit(152);
	//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);	
	//MsgBox("X + Limit. Jog opposite direction.",MB_OK|MB_ICONEXCLAMATION);
	//}

	// Handle axis Z minus limit switch
	//result = Debounce(ReadBit(ZMINUSLIMITBIT),&zmcount,&zmlast,&zmlastsolid);
	//if  (result == 1)
	//{
	//ClearBit(152);
	//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 == 1)
	//{
	//ClearBit(152);
	//MsgBox("Z + 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;

}