#include "KMotionDef.h"
#include "PC-DSP.h"

int DoPC(int cmd);
int DoPCFloat(int cmd, float f);
int DoPCInt(int cmd, int i);
int MsgBox(char *s, int Flags);
int SetVars(int poff, int varoff, int n);
int GetVars(int varoff, int n, int poff);
int GetAxisOffset(double *AxisOffset, int Axis);
int GetOriginOffset(double *OriginOffset, int FixtureIndex, int Axis);
int GetMiscSettings();
int GetMachine();
int GetDROs(double *DROx, double *DROy, double *DROz, double *DROa, double *DROb, double *DROc);

void MoveNorth(double distance);
void MoveSouth(double distance);
void MoveWest(double distance);
void MoveEast(double distance);
void MoveUp(double distance);
void MoveDown(double distance);

void MoveXToDest(double destination);
void MoveYToDest(double destination);

void ProbeDown();
void ProbeNorth();
void ProbeSouth();
void ProbeWest();
void ProbeEast();

// Globals
double *pD = (double *)persist.UserData;
float xRes, yRes, zRes;

#define PROBEBIT 171                         // #110
#define PROBESPEED 500.0f                    // #111
#define XAXIS 0                              // #112
#define YAXIS 1                              // #113
#define ZAXIS 2                              // #114
#define UPSPEED 2000.0f                      // #115
#define DOWNSPEED (PROBESPEED * -1.0f)       // #116
#define WESTSPEED -1000.0f                   // #117
#define EASTSPEED 1000.0f                    // #118
#define NORTHSPEED 1000.0f                   // #119
#define SOUTHSPEED -1000.0f                  // #120
#define PLATETHICKNESS 0.621f                // #121
#define SAFEZ 3.0f                           // #122
#define PLATEDEPTH 3.0f                      // #123
#define PLATEWIDTH 3.0f                      // #124
#define BUFFERDISTANCE 0.1f                  // #125
#define PROBEDEPTH 0.6f                      // #126
#define RIGHTCORNEROFFSET 0.5f               // #127
#define MAXIMUMTOOLDIAMETER 0.5f             // #128
#define LEFTCORNEROFFSET 0.75f               // #129

main()
{
	printf("\n\nStarting main\n");

	pD[10] = PROBEBIT;
	pD[11] = PROBESPEED;
	pD[12] = XAXIS;
	pD[13] = YAXIS;
	pD[14] = ZAXIS;
	pD[15] = UPSPEED;
	pD[16] = DOWNSPEED;
	pD[17] = WESTSPEED;
	pD[18] = EASTSPEED;
	pD[19] = NORTHSPEED;
	pD[20] = SOUTHSPEED;
	pD[21] = PLATETHICKNESS;
	pD[22] = SAFEZ;
	pD[23] = PLATEDEPTH;
	pD[24] = PLATEWIDTH;
	pD[25] = BUFFERDISTANCE;
	pD[26] = PROBEDEPTH;
	pD[27] = RIGHTCORNEROFFSET;
	pD[28] = MAXIMUMTOOLDIAMETER;
	pD[29] = LEFTCORNEROFFSET;
	
	
	printf("Setting vars\n");
	SetVars(110, 20, 10);

	double nTouch, sTouch, wTouch, eTouch;
	double nHole, sHole, wHole, eHole;
	double approxXAxisHoleZero, approxYAxisHoleZero;
	double toolWidth, toolDepth;

	printf("Getting Axis Resolution\n");
	DoPCInt(PC_COMM_GETAXISRES, 50);

	xRes = *(float *)&persist.UserData[50];
	yRes = *(float *)&persist.UserData[51];
	zRes = *(float *)&persist.UserData[52];

	printf("Axis Resolutions Found:\n\tx: %f\n\ty: %f\n\tz: %f\n", xRes, yRes, zRes);

	// Find Z Height
	ProbeDown();
	printf("Setting Z DRO to plate thickness: %f\n", PLATETHICKNESS);
	DoPCFloat(PC_COMM_SET_Z, PLATETHICKNESS);

	// Find N side of plate
	MoveUp(BUFFERDISTANCE);
	MoveNorth(PLATEDEPTH - LEFTCORNEROFFSET + BUFFERDISTANCE);
	MoveDown(PROBEDEPTH);
	ProbeSouth();
	
	printf("Setting nTouch to ch1->Dest: %f\n", ch1->Dest);
	nTouch = ch1->Dest;
	
	// Find E side of plate
	MoveNorth(BUFFERDISTANCE);
	MoveUp(PROBEDEPTH);
	MoveSouth(BUFFERDISTANCE + RIGHTCORNEROFFSET);
	MoveEast(PLATEWIDTH - LEFTCORNEROFFSET + BUFFERDISTANCE);
	MoveDown(PROBEDEPTH);
	ProbeWest();
	
	printf("Setting eTouch to ch0->Dest: %f\n", ch0->Dest);
	eTouch = ch0->Dest;
	
	// Find W side of plate
	MoveEast(BUFFERDISTANCE);
	MoveUp(PROBEDEPTH);
	MoveWest(BUFFERDISTANCE + PLATEWIDTH + BUFFERDISTANCE + MAXIMUMTOOLDIAMETER);
	MoveDown(PROBEDEPTH);
	ProbeEast();
	
	printf("Setting wTouch to ch0->Dest: %f\n", ch0->Dest);
	wTouch = ch0->Dest;

	// Calculate Tool Width
	toolWidth = (eTouch - wTouch) / xRes - PLATEWIDTH;
	printf("toolWidth calculated as %f\n", toolWidth);
	
	// Find S side of plate
	MoveWest(BUFFERDISTANCE);
	MoveUp(PROBEDEPTH);
	MoveEast(BUFFERDISTANCE + PLATEWIDTH - RIGHTCORNEROFFSET + (toolWidth / 2.0));
	MoveSouth(PLATEDEPTH + MAXIMUMTOOLDIAMETER);
	MoveDown(PROBEDEPTH);
	ProbeNorth();
	
	printf("Setting sTouch to ch1->Dest: %f\n", ch1->Dest);
	sTouch = ch1->Dest;
	
	toolDepth = (nTouch - sTouch) / yRes - PLATEDEPTH;
	printf("toolDepth calculated as %f\n", toolDepth);
	
	// Move to Hole Center
	MoveSouth(BUFFERDISTANCE);
	MoveUp(PROBEDEPTH);
	MoveNorth(BUFFERDISTANCE + LEFTCORNEROFFSET + (toolDepth / 2.0));
	MoveWest(PLATEWIDTH - RIGHTCORNEROFFSET - LEFTCORNEROFFSET);
	MoveDown(PROBEDEPTH);
	
	printf("Setting approxXAxisHoleZero to %f\n", ch0->Dest);
    approxXAxisHoleZero = ch0->Dest;
	printf("Setting approxYAxisHoleZero to %f\n", ch1->Dest);
	approxYAxisHoleZero = ch1->Dest;
	
	// Find Exact Hole Center
	ProbeWest();
	wHole = ch0->Dest;
	MoveXToDest(approxXAxisHoleZero);
	ProbeEast();
	eHole = ch0->Dest;
	MoveWest(((eHole - wHole) / 2.0) / xRes);

	printf("Setting X DRO to Zero\n");
	DoPCFloat(PC_COMM_SET_X, 0.0f);
    
	ProbeNorth();
	nHole = ch1->Dest;
	MoveYToDest(approxYAxisHoleZero);
	ProbeSouth();
	sHole = ch1->Dest;
	MoveNorth(((nHole - sHole) / 2.0) / yRes);

	printf("Setting Y DRO to Zero\n");
	DoPCFloat(PC_COMM_SET_Y, 0.0f);
	
	MoveUp(SAFEZ);

	printf("Done\n");
}

void MoveNorth(double distance)
{
	printf("Moving North by %f\n", distance);
	MoveRel(YAXIS, distance * yRes);
	while (!CheckDone(YAXIS));
}

void MoveSouth(double distance)
{
	printf("Moving South by %f\n", distance);
	MoveRel(YAXIS, -1 * distance * yRes);
	while (!CheckDone(YAXIS));
}

void MoveWest(double distance)
{
	printf("Moving West by %f\n", distance);
	MoveRel(XAXIS, -1 * distance * xRes);
	while (!CheckDone(XAXIS));
}

void MoveEast(double distance)
{
	printf("Moving East by %f\n", distance);
	MoveRel(XAXIS, distance * xRes);
	while (!CheckDone(XAXIS));
}

void MoveUp(double distance)
{
	printf("Moving Up by %f\n", distance);
	MoveRel(ZAXIS, distance * zRes);
	while (!CheckDone(ZAXIS));
}

void MoveDown(double distance)
{
	printf("Moving Down by %f\n", distance);
	MoveRel(ZAXIS, -1 * distance * zRes);
	while (!CheckDone(ZAXIS));
}

void MoveXToDest(double destination)
{
	printf("Moving XAXIS to %f\n", destination);
	Move(XAXIS, destination);
	while (!CheckDone(XAXIS));
}

void MoveYToDest(double destination)
{
	printf("Moving YAXIS to %f\n", destination);
	Move(YAXIS, destination);
	while (!CheckDone(YAXIS));
}

void ProbeDown()
{
	printf("Jogging Down at speed:%f\n", DOWNSPEED);
    Jog(ZAXIS, DOWNSPEED);
	printf("Waiting for probe to read high\n");
	while (!ReadBit(PROBEBIT));   		
	printf("Stopping\n");
    Jog(ZAXIS, 0);
	printf("Waiting for Z motion to stop\n");
	while (!CheckDone(ZAXIS));
}

void ProbeSouth()
{
	printf("Jogging South at speed:%f\n", SOUTHSPEED);
	Jog(YAXIS, SOUTHSPEED);
	printf("Waiting for probe to read high\n");
	while (!ReadBit(PROBEBIT));   		
	printf("Stopping\n");
    Jog(YAXIS, 0);
	printf("Waiting for Y motion to stop\n");
	while (!CheckDone(YAXIS));
}

void ProbeNorth()
{
	printf("Jogging North at speed:%f\n", NORTHSPEED);
	Jog(YAXIS, NORTHSPEED);
	printf("Waiting for probe to read high\n");
	while (!ReadBit(PROBEBIT));   		
	printf("Stopping\n");
    Jog(YAXIS, 0);
	printf("Waiting for Y motion to stop\n");
	while (!CheckDone(YAXIS));
}

void ProbeWest()
{
	printf("Jogging West at speed:%f\n", WESTSPEED);
	Jog(XAXIS, WESTSPEED);
	printf("Waiting for probe to read high\n");
	while (!ReadBit(PROBEBIT));   		
	printf("Stopping\n");
    Jog(XAXIS, 0);
	printf("Waiting for X motion to stop\n");
	while (!CheckDone(XAXIS));
}

void ProbeEast()
{
	printf("Jogging East at speed:%f\n", EASTSPEED);
	Jog(XAXIS, EASTSPEED);
	printf("Waiting for probe to read high\n");
	while (!ReadBit(PROBEBIT));   		
	printf("Stopping\n");
    Jog(XAXIS, 0);
	printf("Waiting for X motion to stop\n");
	while (!CheckDone(XAXIS));
}

int GetDROs(double *DROx, double *DROy, double *DROz, double *DROa, double *DROb, double *DROc)
{
	int TMP = 60;
	if (DoPCInt(PC_COMM_GET_DROS,TMP)) return 1;       // Var index and Cmd
	*DROx=pD[TMP];
	*DROy=pD[TMP+1];
	*DROz=pD[TMP+2];
	*DROa=pD[TMP+3];
	*DROb=pD[TMP+4];
	*DROc=pD[TMP+5];
}

int GetMachine(double *Machinex, double *Machiney, double *Machinez, double *Machinea, double *Machineb, double *Machinec)
{
	int TMP = 66;
	if (DoPCInt(PC_COMM_GET_MACHINE_COORDS,TMP)) return 1;       // Var index and Cmd
	*Machinex=pD[TMP];
	*Machiney=pD[TMP+1];
	*Machinez=pD[TMP+2];
	*Machinea=pD[TMP+3];
	*Machineb=pD[TMP+4];
	*Machinec=pD[TMP+5];
}

int GetMiscSettings(int *Units, int *TWORD, int *HWORD, int *DWORD)
{
	int TMP = 72;
	if (DoPCInt(PC_COMM_GET_MISC_SETTINGS,TMP)) return 1; 
	
	*Units = persist.UserData[TMP];
	*TWORD = persist.UserData[TMP+1];
	*HWORD = persist.UserData[TMP+2];
	*DWORD = persist.UserData[TMP+3];
}

int GetOriginOffset(double *OriginOffset, int FixtureIndex, int Axis)
{
	int TMP = 77;
	if (GetVars(5200+FixtureIndex*20+Axis+1,1,TMP)) return 1;  // Download to persist TMP
	*OriginOffset=pD[TMP];
	return 0;
}

int GetAxisOffset(double *AxisOffset, int Axis)
{
	int TMP = 78;
	if (GetVars(5200+Axis+11,1,TMP)) return 1;  // Download to persist TMP
	*AxisOffset=pD[TMP];
	return 0;
}

int SetVars(int varoff, int n, int poff)
{
	persist.UserData[PC_COMM_PERSIST+2] = n;       // number of elements
	persist.UserData[PC_COMM_PERSIST+3] = poff;    // persist offset (doubles)
	return DoPCInt(PC_COMM_SET_VARS,varoff);       // Var index and Cmd
}

int GetVars(int varoff, int n, int poff)
{
	persist.UserData[PC_COMM_PERSIST+2] = n;       // number of elements
	persist.UserData[PC_COMM_PERSIST+3] = poff;    // persist offset (doubles)
	return DoPCInt(PC_COMM_GET_VARS,varoff);       // Var index and Cmd
}


#define GATH_OFF 0  // define the offset into the Gather buffer where strings are passed

// Trigger a message box on the PC to be displayed
// defines for MS Windows message box styles and Operator
// response IDs are defined in the KMotionDef.h file 
int MsgBox(char *s, int Flags)
{
	char *p=(char *)gather_buffer+GATH_OFF*sizeof(int);
	int i;
	
	do // copy to gather buffer w offset 0
	{
		*p++ = *s++;
	}while (s[-1]);
	
	persist.UserData[PC_COMM_PERSIST+2] = Flags;  // set options
	DoPCInt(PC_COMM_MSG,GATH_OFF);
	return persist.UserData[PC_COMM_PERSIST+3];
}

// put the MDI string (Manual Data Input - GCode) in the 
// gather buffer and tell the App where it is
int MDI(char *s)
{
	char *p=(char *)gather_buffer+GATH_OFF*sizeof(int);
	int i;
	
	do // copy to gather buffer w offset 0
	{
		*p++ = *s++;
	}while (s[-1]);
	
	// issue the command an wait till it is complete
	// (or an error - such as busy)
	return DoPCInt(PC_COMM_MDI,GATH_OFF);
}

// 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);
}

// Put an integer as a parameter and pass the command to the App
int DoPCInt(int cmd, int i)
{
	int result;
	persist.UserData[PC_COMM_PERSIST+1] = i;
	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;
}