#include "KMotionDef.h"

#define ESTOP 1025
#define POWER 1024
#define PWRRDY  48
#define AXISENABLE 49
#define ZBRAKE 50
#define LUBE 52
#define xPos 1040
#define xNeg 1041
#define yPos 1043
#define yNeg 1044
#define zPos 1046
#define zNeg 1047
#define xRdy 1026
#define yRdy 1027
#define zRdy 1028
#define spRdy 1029


main()
{
	
	InitAux();
	AddKonnect_Aux0(0,&VirtualBits,VirtualBitsEx);
	
	SetBit(49);
	while(!ReadBit(xRdy) || !ReadBit(yRdy) || !ReadBit(zRdy) || !ReadBit(spRdy))
	{
		//do nothing until drives return ready
	}
	//x
	ch0->InputMode=ENCODER_MODE;
	ch0->OutputMode=DAC_SERVO_MODE;
	ch0->Vel=600000;
	ch0->Accel=3e+06;
	ch0->Jerk=7e+07;
	ch0->P=0.75;
	ch0->I=0.0085;
	ch0->D=38;
	ch0->FFAccel=1e-05;
	ch0->FFVel=0.0058;
	ch0->MaxI=1200;
	ch0->MaxErr=4e+07;
	ch0->MaxOutput=2046;
	ch0->DeadBandGain=1;
	ch0->DeadBandRange=0;
	ch0->InputChan0=0;
	ch0->InputChan1=0;
	ch0->OutputChan0=0;
	ch0->OutputChan1=1;
	ch0->MasterAxis=-1;
	ch0->LimitSwitchOptions=0x11c;
	ch0->LimitSwitchNegBit=1041;
	ch0->LimitSwitchPosBit=1040;
	ch0->SoftLimitPos=0;
	ch0->SoftLimitNeg=-3.32931e+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_LINEAR;
	ch0->BacklashAmount=83;
	ch0->BacklashRate=2000;
	ch0->invDistPerCycle=1;
	ch0->Lead=0;
	ch0->MaxFollowingError=400;
	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.92076;
	ch0->iir[2].A2=-0.923833;

	//y
	ch1->InputMode=ENCODER_MODE;
	ch1->OutputMode=DAC_SERVO_MODE;
	ch1->Vel=600000;
	ch1->Accel=3e+06;
	ch1->Jerk=7e+07;
	ch1->P=1.35;
	ch1->I=0.007;
	ch1->D=10;
	ch1->FFAccel=1e-06;
	ch1->FFVel=0.00372;
	ch1->MaxI=2046;
	ch1->MaxErr=40000;
	ch1->MaxOutput=2046;
	ch1->DeadBandGain=1;
	ch1->DeadBandRange=0;
	ch1->InputChan0=1;
	ch1->InputChan1=0;
	ch1->OutputChan0=1;
	ch1->OutputChan1=0;
	ch1->MasterAxis=-1;
	ch1->LimitSwitchOptions=0x11c;
	ch1->LimitSwitchNegBit=1044;
	ch1->LimitSwitchPosBit=1043;
	ch1->SoftLimitPos=0;
	ch1->SoftLimitNeg=-1.66465e+06;
	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=125;
	ch1->BacklashRate=3000;
	ch1->invDistPerCycle=1;
	ch1->Lead=0;
	ch1->MaxFollowingError=400;
	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=1;
	ch1->iir[2].B1=0;
	ch1->iir[2].B2=0;
	ch1->iir[2].A1=0;
	ch1->iir[2].A2=0;

	//z
	ch2->InputMode=ENCODER_MODE;
	ch2->OutputMode=DAC_SERVO_MODE;
	ch2->Vel=400000;
	ch2->Accel=1.5e+06;
	ch2->Jerk=3e+07;
	ch2->P=1.4;
	ch2->I=0.0055;
	ch2->D=15;
	ch2->FFAccel=0;
	ch2->FFVel=0.00425;
	ch2->MaxI=1200;
	ch2->MaxErr=400;
	ch2->MaxOutput=2046;
	ch2->DeadBandGain=1;
	ch2->DeadBandRange=0;
	ch2->InputChan0=2;
	ch2->InputChan1=2;
	ch2->OutputChan0=2;
	ch2->OutputChan1=5;
	ch2->MasterAxis=-1;
	ch2->LimitSwitchOptions=0x11c;
	ch2->LimitSwitchNegBit=1047;
	ch2->LimitSwitchPosBit=1046;
	ch2->SoftLimitPos=0;
	ch2->SoftLimitNeg=-1.66465e+06;
	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=166;
	ch2->BacklashRate=3300;
	ch2->invDistPerCycle=1;
	ch2->Lead=0;
	ch2->MaxFollowingError=400;
	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=0.027497;
	ch2->iir[1].B1=0.027497;
	ch2->iir[1].B2=0;
	ch2->iir[1].A1=0.945006;
	ch2->iir[1].A2=0;

	ch2->iir[2].B0=0.011007;
	ch2->iir[2].B1=0.022015;
	ch2->iir[2].B2=0.011007;
	ch2->iir[2].A1=1.68443;
	ch2->iir[2].A2=-0.728457;

	//Spindle
	ch3->InputMode=ENCODER_MODE;
	ch3->OutputMode=DAC_SERVO_MODE;
	ch3->Vel=300000;
	ch3->Accel=300000;
	ch3->Jerk=600000;
	ch3->P=0.05;
	ch3->I=0;
	ch3->D=0.5;
	ch3->FFAccel=0;
	ch3->FFVel=0.002;
	ch3->MaxI=800;
	ch3->MaxErr=1e+06;
	ch3->MaxOutput=2046;
	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=1e30;
	ch3->SoftLimitNeg=-1e30;
	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=1000000;
	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=1;
	ch3->iir[2].B1=0;
	ch3->iir[2].B2=0;
	ch3->iir[2].A1=0;
	ch3->iir[2].A2=0;





	
	EnableAxis(0);
	EnableAxis(1);
	EnableAxis(2);
	EnableAxis(3);
	SetBit(ZBRAKE);
	
	double Tau = 0.006; // seconds for Low Pass Filter Time Constant
	KLP = exp(-TIMEBASE/Tau);
	printf("Tau=%f KLP=%f\n",Tau,KLP);
	
	//EnableAxisDest(0,0);
	//EnableAxisDest(0,1);
	//EnableAxis(2);

    
	
	DefineCoordSystem(0,1,2,-1);
	
	
	
	int result;
	
	 for (;;) // loop forever
    {
        WaitNextTimeSlice(); // execute loop once every time slice
		
		
        HandleDriveReady();
        HandleIOErrors();
		

		// Handle ESTOP
		
		if  (!ReadBit(ESTOP))
		{
			if (ch0->Enable) DisableAxis(0);  // axis still enabled?  - Disable it
			if (ch1->Enable) DisableAxis(1);  // axis still enabled?  - Disable it
			if (ch2->Enable) DisableAxis(2);  // axis still enabled?  - Disable it
			
			//Kill servo power
			ClearBit(PWRRDY);
			//Disable Physical Drive
			ClearBit(AXISENABLE);
			//Clear Z brake on
			ClearBit(ZBRAKE);
			//Turn lube off
			ClearBit(LUBE);
			
			
		}
		
		if  (ReadBit(POWER)) 
		{
			if(!ReadBit(PWRRDY))
			{
				//Turn on power to servos
				SetBit(PWRRDY);
			}
				//enable physical axis, turn off z brake
				SetBit(AXISENABLE);
				SetBit(ZBRAKE);
				SetBit(LUBE);
				//enable kflop axis
				EnableAxis(0);
				EnableAxis(1);
				EnableAxis(2);
				EnableAxis(3);
			
			
		}

		limitKillPower();
        
    }   // end of forever loop

}


// limits to emulate A2100 limit switch process for now
// A2100 performs same process as e-stop, hold main power button and jog off limits
// to clear.
void limitKillPower()
{
	int xLPos = ReadBit(xPos);
    int xLNeg = ReadBit(xNeg);
    int yLPos = ReadBit(yPos);
    int yLNeg = ReadBit(yNeg);
    int zLPos = ReadBit(zPos);
    int zLNeg = ReadBit(zNeg);

	if((!xLPos || !xLNeg || !yLPos || !yLNeg || !zLPos || !zLNeg) && !ReadBit(POWER)) // || yLPos || yLNeg || zLPos || zLNeg)
    {
			ClearBit(PWRRDY);
			//Disable Physical Drive
			ClearBit(AXISENABLE);
			//Clear Z brake on
			ClearBit(ZBRAKE);
			//Turn lube off
			ClearBit(LUBE);
			DisableAxis(0);
			DisableAxis(1);
			DisableAxis(2);
			DisableAxis(3);
        
    }
}

void HandleDriveReady
{
	//turn off drive ready signal if kmotion channel is disabled
	if (!ch0->Enable || !ch1->Enable || !ch2->Enable && ReadBit(49))
	{
	clearBit(49);
	}

}

void HandleIOErrors
{
		int CoolantOL = ReadBit(1049);
		int PowerFault = ReadBit(1050);
		int SpindleTemp = ReadBit(1051);
		int SpindleDrive = ReadBit(1052);
		int RegenOL = ReadBit(1053);
		
		/*if(CoolantOL)
		{
			limitKillPower();
			printf("Coolant Overload");
			
		}
		if(PowerFault)
		{
			limitKillPower();
			printf("Power Fault");
			
		}
		if(SpindleTemp)
		{
			limitKillPower();
			printf("Spindle Motor Over Temp");
			
		}
		if(SpindleDrive)
		{
			limitKillPower();
			printf("Spindle Drive Over Temp");
			
		}
		if(RegenOL)
		{
			limitKillPower();
			printf("Regen Overload");
			
		} */
		
	
}