#include "KMotionDef.h"
#define TMP 10
#include "KflopToKMotionCNCFunctions.c"
#include "SCMDefs.c"

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

int flast = 0, flastsolid = -1, fcount = 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;


void main()
{
	InitAux();
	AddKonnect(0, &VirtualBits, VirtualBitsEx);
	AddKonnect(1, VirtualBitsEx + 1, VirtualBitsEx + 2);
	AddKonnect(2, VirtualBitsEx + 3, VirtualBitsEx + 4);

	double Tau = 0.02;			// seconds for Low Pass Filter Time Constant
	KLP = exp(-TIMEBASE / Tau);
	printf("Tau=%f KLP=%f\n", Tau, KLP);
	

	ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=DAC_SERVO_MODE;
	ch0->Vel=360000;
	ch0->Accel=990000;
	ch0->Jerk=3.5e+06;
	ch0->P=0.6;
	ch0->I=0.0008;
	ch0->D=4;
	ch0->FFAccel=0;
	ch0->FFVel=0.0042;
	ch0->MaxI=700;
	ch0->MaxErr=2500;
	ch0->MaxOutput=2047;
	ch0->DeadBandGain=0;
	ch0->DeadBandRange=0;
	ch0->InputChan0=0;
	ch0->InputChan1=1;
	ch0->OutputChan0=0;
	ch0->OutputChan1=1;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x10f;
	ch0->LimitSwitchNegBit=143;
	ch0->LimitSwitchPosBit=143;
	ch0->SoftLimitPos=1.8526e+06;
	ch0->SoftLimitNeg=-1.8526e+06;
	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=10;
	ch0->BacklashRate=1;
	ch0->invDistPerCycle=1;
	ch0->Lead=1;
	ch0->MaxFollowingError=100;
	ch0->StepperAmplitude=250;

	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.0162306;
	ch0->iir[2].B1=0.0324612;
	ch0->iir[2].B2=0.0162306;
	ch0->iir[2].A1=1.57014;
	ch0->iir[2].A2=-0.635065;

	ch1->InputMode=ENCODER_MODE;
	ch1->OutputMode=DAC_SERVO_MODE;
	ch1->Vel=250000;
	ch1->Accel=990000;
	ch1->Jerk=6e+06;
	ch1->P=0.6;
	ch1->I=0.0025;
	ch1->D=18;
	ch1->FFAccel=0;
	ch1->FFVel=0.0056;
	ch1->MaxI=800;
	ch1->MaxErr=2500;
	ch1->MaxOutput=2047;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=1;
	ch1->InputChan0=1;
	ch1->InputChan1=0;
	ch1->OutputChan0=1;
	ch1->OutputChan1=1;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x10f;
	ch1->LimitSwitchNegBit=143;
	ch1->LimitSwitchPosBit=143;
	ch1->SoftLimitPos=1.8426e+08;
	ch1->SoftLimitNeg=-1.8426e+08;
	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=1;
	ch1->MaxFollowingError=100;
	ch1->StepperAmplitude=20;

	ch1->iir[0].B0=0.0274969;
	ch1->iir[0].B1=0.0274969;
	ch1->iir[0].B2=0;
	ch1->iir[0].A1=0.945006;
	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=1;
	ch1->iir[2].B1=0;
	ch1->iir[2].B2=0;
	ch1->iir[2].A1=0;
	ch1->iir[2].A2=0;

	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=DAC_SERVO_MODE;
	ch2->Vel=120000;
	ch2->Accel=600000;
	ch2->Jerk=3e+07;
	ch2->P=0.4;
	ch2->I=0.002;
	ch2->D=20;
	ch2->FFAccel=0;
	ch2->FFVel=0.0056;
	ch2->MaxI=400;
	ch2->MaxErr=1000;
	ch2->MaxOutput=2047;
	ch2->DeadBandGain=0;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=0;
	ch2->OutputChan0=2;
	ch2->OutputChan1=0;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x10f;
	ch2->LimitSwitchNegBit=143;
	ch2->LimitSwitchPosBit=143;
	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=10;
	ch2->MaxFollowingError=100;
	ch2->StepperAmplitude=20;

	ch2->iir[0].B0=0.123861;
	ch2->iir[0].B1=0.123861;
	ch2->iir[0].B2=0;
	ch2->iir[0].A1=0.752278;
	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.099244;
	ch2->iir[2].B1=0.198488;
	ch2->iir[2].B2=0.099244;
	ch2->iir[2].A1=0.93807;
	ch2->iir[2].A2=-0.335047;


	EnableAxis(0);
	EnableAxis(1);
	EnableAxis(2);

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

	//extern float CS0_LastFRO;
	//printf("%2.2f\n", CS0_LastFRO);

	if (!ReadBit(AIR_PRESSURE_CHK))			//checks if there are air pressure
	{
		DoPC(PC_COMM_ESTOP);
		MsgBox("Air Supply Low", MB_OK | MB_ICONEXCLAMATION);
		disablemachine();

	} else if(!ReadBit(LUBRIFICATION_CHK))
	{
		DoPC(PC_COMM_ESTOP);
		int Answer;
		Answer = MsgBox("Machine Not Greased. Do you want to Grease?", MB_YESNO | MB_ICONEXCLAMATION);
			if (Answer == IDYES)
			{
				disablemachine();
				greasemachine();
				disablemachine();
			}
			else
				disablemachine();
		
	}
		
				for (;;)					// loop forever
	{
		WaitNextTimeSlice();	// execute loop once every time slice
		buttonvacuum();			// Loop to change vacuum stats
		disablemachine();		// Loop to disable Machine
		ServiceJobTimer();

		if (ch0->Enable && ch1->Enable && ch2->Enable)
		{
			int time;
			
			if (time > 1000)time = 1;
			else time++;	
			if(time == 999) spindlespeed();			// Loop do feedback spindle speed
			
		}
		
	}


}

void greasemachine()
{
				ClearBit(LUBRIFICATION_PULSE);
				SetBit(LUBRIFICATION_PULSE);
				Delay_sec(3);
				ClearBit(LUBRIFICATION_PULSE);
				Delay_sec(3);
				SetBit(LUBRIFICATION_PULSE);
				Delay_sec(3);
				ClearBit(LUBRIFICATION_PULSE);	
				Delay_sec(3);
				SetBit(LUBRIFICATION_PULSE);
				Delay_sec(3);
				ClearBit(LUBRIFICATION_PULSE);	
}

void buttonvacuum()
{
	int LastState = persist.UserData[80];
	int LastState2 = persist.UserData[81];
	int result, result2;

	result = Debounce(ReadBit(VACUUM_1_ON_BUTTON), &fcount, &flast, &flastsolid);

	if (result == 1)
	{	
		if (LastState == 0)		// If Vacuum was off, now turn on
		{
			ClearBit(VACUUM_1_ON);
			SetBit(VACUUM_1_OFF);
			LastState = 1;		//Saves Vacuum State
		}
		else if (LastState == 1)
		{
			ClearBit(VACUUM_1_OFF);
			SetBit(VACUUM_1_ON);
			LastState = 0;
		}
	}

	result2 = Debounce(ReadBit(VACUUM_2_ON_BUTTON), &ccount, &clast, &clastsolid);
	
	if (result2 == 1)
	{	
		if (LastState2 == 0)		// If Vacuum was off, now turn on
		{
			ClearBit(VACUUM_2_ON);
			SetBit(VACUUM_2_OFF);
			LastState2 = 1;		//Saves Vacuum State
		}
		else if (LastState2 == 1)
		{
			ClearBit(VACUUM_2_OFF);
			SetBit(VACUUM_2_ON);
			LastState2 = 0;
		}
	}

	persist.UserData[80] = LastState;	//Store Vacuum State in global
	persist.UserData[81] = LastState2;	//Store Vacuum State in global

}


void disablemachine()
{

	if (ch0->Enable && ch1->Enable && ch2->Enable)
	{
		SetBit(POWER_ON);				// CONTATORA DE POTENCIA
	}
	else
	{
		ClearBit(POWER_ON);
		ClearBit(SPINDLE_ON);
		ClearBit(Z_BRAKE);
	}
	if (ReadBit(POWER_ON_CHK))
	{
		SetBit(Z_BRAKE);				//BIT DO FREIO DO EIXO Z
	}
	else
	{
		ClearBit(Z_BRAKE);			//BIT DO FREIO DO EIXO Z
	}
	
	if (!ReadBit(AIR_PRESSURE_CHK))			//checks if there are air pressure
	{
		DoPC(PC_COMM_ESTOP);
		//MsgBox("Air Supply Low", MB_OK | MB_ICONEXCLAMATION);
	}
}


#define CHANGE_TOL 3  // only update if change is more than this
double LastXPR;
double LastYPR;
double LastZPR;
double LastIPR;

void spindlespeed()
{
	char s[80];
	//------------------------ SPINDLE LOAD
	double IPR = KANALOG_CONVERT_ADC_TO_VOLTS(ADC(7)) * 5;	// Voltage return from VFD
	if (IPR < 0.8) IPR = 0;
	
	if ( IPR > LastIPR + 0.1 || IPR < LastIPR + 0.1)
	{
		sprintf(s, "%2.2f A\n", IPR);
		DROLabel(1000, 162, s);
		WriteVarFloat(163, IPR / 15);
		LastIPR = IPR;
	}

	//------------------------ X AXIS LOAD
	double XPR = KANALOG_CONVERT_ADC_TO_VOLTS(ADC(6));	// Voltage return from VFD

	if (XPR < 0.00)
		XPR = XPR * -1;

	XPR = (XPR * 100) / 3;

	if ((XPR > LastXPR+CHANGE_TOL) || (XPR < LastXPR-CHANGE_TOL))
		{
			sprintf(s, "%2.0f \n", XPR);
			DROLabel(1200, 164, s);
			WriteVarFloat(165, XPR / 100);
			LastXPR=XPR;
		}

	//------------------------ Y AXIS LOAD

	double YPR = KANALOG_CONVERT_ADC_TO_VOLTS(ADC(5));	// Voltage return from VFD

	if (YPR < 0.00)
		YPR = YPR * -1;
	YPR = (YPR * 100) / 3;

	if ((YPR > LastYPR+CHANGE_TOL) || (YPR < LastYPR-CHANGE_TOL))
	{
		sprintf(s, "%2.0f \n", YPR);
		DROLabel(1300, 166, s);
		WriteVarFloat(167, YPR / 100);
		LastYPR=YPR;
	}

	//------------------------ Z AXIS LOAD
	double ZPR = KANALOG_CONVERT_ADC_TO_VOLTS(ADC(4));	// Voltage return from VFD;

	if (ZPR < 0.00)
		ZPR = ZPR * -1;
	ZPR = (ZPR * 100) / 3;
	if ((ZPR > LastZPR+CHANGE_TOL) || (ZPR < LastZPR-CHANGE_TOL))
	{
	sprintf(s, "%2.0f \n", ZPR);
	DROLabel(1400, 168, s);
	WriteVarFloat(169, ZPR / 100);
	LastZPR=ZPR;
	}
}


#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;
}

#define MACHINING_TIME 170

void ServiceJobTimer(void)
{
    static BOOL WasActive = FALSE;
    static double StartTime;
    double dt, Seconds;
    int Hours, Minutes;
    char s[80];

    if (JOB_ACTIVE && !WasActive)    // Started ??
    {
        StartTime = Time_sec();
        WasActive = TRUE;
    }
    if (!JOB_ACTIVE && WasActive)    // Ended ??
    {
        WasActive = FALSE;

        dt = Time_sec() - StartTime;
        Hours = (int)(dt / 3600.0);
        Minutes = (int)((dt - Hours * 3600.0) / 60.0);
        Seconds = dt - Hours * 3600.0 - Minutes * 60.0;

        if (Hours > 0)
            sprintf(s, "Last Run Time %d hours %d min %5.1f sec", Hours, Minutes, Seconds);
        else if (Minutes > 0)
            sprintf(s, "Last Run Time %d min %5.1f sec", Minutes, Seconds);
        else
            sprintf(s, "Last Run Time %5.1f sec", Seconds);

        // Put it onto the Screen using Persist
        DROLabel(1500, MACHINING_TIME, s);
    }
}