#include "KMotionDef.h"
#define TMP 10					// which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"
// Defines axis 0, 1, 2 as simple step dir outputs
// enables them
// sets them as an xyz coordinate system for GCode

int main()
{
	ch0->InputMode = ENCODER_MODE;
	ch0->OutputMode = CL_STEP_DIR_MODE;
	ch0->Vel = 206375;
	ch0->Accel = 412750;
	ch0->Jerk = 4e+09;
	ch0->P = 0;
	ch0->I = 0.02;
	ch0->D = 0;
	ch0->FFAccel = 0;
	ch0->FFVel = 0;
	ch0->MaxI = 20000;
	ch0->MaxErr = 1e+09;
	ch0->MaxOutput = 20000;
	ch0->DeadBandGain = 1;
	ch0->DeadBandRange = 0;
	ch0->InputChan0 = 0;
	ch0->InputChan1 = 0;
	ch0->OutputChan0 = 52;
	ch0->OutputChan1 = 0;
	ch0->MasterAxis = -1;
	ch0->LimitSwitchOptions = 0x11f;
	ch0->LimitSwitchNegBit = 136;
	ch0->LimitSwitchPosBit = 137;
	ch0->SoftLimitPos = 1e+09;
	ch0->SoftLimitNeg = -1e+09;
	ch0->InputGain0 = 12.5;
	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 = 5e+13;
	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.000768809;
	ch0->iir[2].B1 = 0.00153762;
	ch0->iir[2].B2 = 0.000768809;
	ch0->iir[2].A1 = 1.92081;
	ch0->iir[2].A2 = -0.923885;
	EnableAxisDest(0, 0);

	ch1->InputMode = ENCODER_MODE;
	ch1->OutputMode = CL_STEP_DIR_MODE;
	ch1->Vel = 206375;
	ch1->Accel = 412750;
	ch1->Jerk = 4e+09;
	ch1->P = 0;
	ch1->I = 0.02;
	ch1->D = 0;
	ch1->FFAccel = 0;
	ch1->FFVel = 0;
	ch1->MaxI = 20000;
	ch1->MaxErr = 1e+06;
	ch1->MaxOutput = 20000;
	ch1->DeadBandGain = 1;
	ch1->DeadBandRange = 0;
	ch1->InputChan0 = 1;
	ch1->InputChan1 = 0;
	ch1->OutputChan0 = 53;
	ch1->OutputChan1 = 0;
	ch1->MasterAxis = -1;
	ch1->LimitSwitchOptions = 0x11f;
	ch1->LimitSwitchNegBit = 139;
	ch1->LimitSwitchPosBit = 138;
	ch1->SoftLimitPos = 1e+09;
	ch1->SoftLimitNeg = -1e+09;
	ch1->InputGain0 = -12.5;
	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 = 5e+13;
	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 = 400000;
	ch2->Accel = 400000;
	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 = 54;
	ch2->OutputChan1 = 0;
	ch2->MasterAxis = -1;
	ch2->LimitSwitchOptions = 0x11f;
	ch2->LimitSwitchNegBit = 141;
	ch2->LimitSwitchPosBit = 140;
	ch2->SoftLimitPos = 1e+09;
	ch2->SoftLimitNeg = -1e+09;
	ch2->InputGain0 = -12.5;
	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 = 500;
	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);

	DefineCoordSystem(0, 1, 2, -1);

	// Code for GAP control with GAP average voltage

	const int ADC_C = 0;		// Define ADC channel for monitoring 
	char s1036[80]; //for Voltage DRO
	char s1037[80]; //for FRO DRO
	double OC, SC, FAST, SLOW, REV, FRATE, C_RATE, UW, DW, SCW, RET_TIME; //define user parameters
	
	//start reading user parameters
	
		if (GetEditControlDouble(&OC, 1025, 1000))
		{
		printf("Error Reading Edit Control Var=1025\n"); 
		return 0;
		}
		if (GetEditControlDouble(&SC, 1026, 1010))
		{
		printf("Error Reading Edit Control Var=1026\n"); 
		return 0;
		}
		if (GetEditControlDouble(&FAST, 1027, 1020))
		{
		printf("Error Reading Edit Control Var=1027\n"); 
		return 0;
		}
		if (GetEditControlDouble(&SLOW, 1028, 1030))
		{
		printf("Error Reading Edit Control Var=1028\n"); 
		return 0;
		}
		if (GetEditControlDouble(&REV, 1029, 1040))
		{
		printf("Error Reading Edit Control Var=1029\n"); 
		return 0;
		}
		if (GetEditControlDouble(&FRATE, 1030, 1050))
		{
		printf("Error Reading Edit Control Var=1030\n"); 
		return 0;
		}
		if (GetEditControlDouble(&C_RATE, 1031, 1060))
		{
		printf("Error Reading Edit Control Var=1031\n"); 
		return 0;
		}
		if (GetEditControlDouble(&UW, 1032, 1070))
		{
		printf("Error Reading Edit Control Var=1032\n"); 
		return 0;
		}
		if (GetEditControlDouble(&DW, 1033, 1080))
		{
		printf("Error Reading Edit Control Var=1033\n"); 
		return 0;
		}
		if (GetEditControlDouble(&SCW, 1034, 1090))
		{
		printf("Error Reading Edit Control Var=1034\n"); 
		return 0;
		}
		if (GetEditControlDouble(&RET_TIME, 1035, 1100))
		{
		printf("Error Reading Edit Control Var=1035\n"); 
		return 0;
		}
		
	// stop reading user parameters
	
	double SCSCW = SC * SCW; 
	double OCUW = OC * UW; 
	double OCDW = OC * DW; 
	double VARC = 0;	//Define ARC Voltage variable
	double FRO = 1;	//Define FRO variable
	int NewBit1024 = 0; 
	int LastBit1024 = 0; 
	double T = 0;	//Define time variable
	float LastFROT = 0;	//Define last FRO time change
	
	//GAP CONTROL LOOP START
	
	  for (;;)		
		{
		T = WaitNextTimeSlice(); 
		VARC = KANALOG_CONVERT_ADC_TO_VOLTS(ADC(ADC_C)); 
		NewBit1024 = ReadBit(1024); 
		if (!LastBit1024 && NewBit1024)	//After Start of the program?
		{
			FRO = 1.0; 
			LastBit1024 = NewBit1024;
		}
		if (ReadBit(1024) && T > LastFROT + C_RATE) //Time to make changes?
		{
			if (VARC < SCSCW)	//Voltage lower than SC voltage with some safety limit?
			{
				SetFROwRate(REV, FRATE);	//GO reverse!
				LastFROT = T; Delay_sec(RET_TIME);	// Retraction Delay    
				SetFROwRate(1, FRATE);	//GO !
			}
			else if (VARC > OCUW)	//Voltage higher than Upper GAP window?
			{
				FRO = FRO + 0.01; 
				if (FRO > FAST) FRO = FAST; 
				SetFROwRate(FRO, FRATE);	//GO faster!
				LastFROT = T;
			}
			else if (VARC < OCDW && VARC > SCSCW)	//Voltage higher than SC but lower than Down GAP window?
			{
				FRO = FRO - 0.01; 
				if (FRO < SLOW) FRO = SLOW; 
				SetFROwRate(FRO, FRATE);	//GO slower!
				LastFROT = T;
			}
		}
		sprintf(s1036, "%g", VARC); 
		DROLabel(1000, 1036, s1036); //SHOW GAP Voltage
		sprintf(s1037, "%g", FRO); 
		DROLabel(1000, 1037, s1037); //SHOW FRO
		}
	
	//GAP CONTROL LOOP END
	
	return 0;
}