// 
// LSshowOffsets
// 02/02/2020
//
// Demonstrate conversion between machine and workspace positions

#include "KMotionDef.h"
#define TMP 30 // which spare persist to use to transfer data
#include "KflopToKMotionCNCFunctions.c"

main()
{
//Definitions 
	#define Xaxis 0
	#define Yaxis 1
	#define Zaxis 2

//Declarations	
	int FixtureIndex, Units, TWORD, HWORD, DWORD;		// current tool and fixture indices
	double OriginOffsetX, OriginOffsetY, OriginOffsetZ;  	// current fixture offset
	double AxisOffsetX, AxisOffsetY, AxisOffsetZ;		// current global offset
	double ToolOffsetX, ToolOffsetY, Length;		// current tool offsets
	double curXpos, curYpos, curZpos;	  		// current tool tip position (per DRO, gcode workspace)
	double machXpos, machYpos, machZpos, a, b, c;		// current machine position (without offsets)
	
	double compX, compY, compZ;			 	// position computed using offsets	
	
	int G43active;						// tool length only counts if G43 active

//Read all current offset values	
	GetMiscSettings(&Units, &TWORD, &HWORD, &DWORD);	// get index for current tool
	GetFixtureIndex(&FixtureIndex); 			// get index for current fixture

	GetOriginOffset(&OriginOffsetX, FixtureIndex, Xaxis); 	// get fixture offsets
	GetOriginOffset(&OriginOffsetY, FixtureIndex, Yaxis);
	GetOriginOffset(&OriginOffsetZ, FixtureIndex, Zaxis);

	GetAxisOffset(&AxisOffsetX,Xaxis); 			// get global offsets
	GetAxisOffset(&AxisOffsetY,Yaxis);
	GetAxisOffset(&AxisOffsetZ,Zaxis);

	GetToolOffsetX(TWORD,&ToolOffsetX);			// get tool offsets
	GetToolOffsetY(TWORD,&ToolOffsetY);
	GetToolLength(TWORD,&Length);

//Get current tip position
	GetDROs(&curXpos,&curYpos,&curZpos,&a,&b,&c);	//workspace (gcode) position
	GetMachine(&machXpos, &machYpos, &machZpos, &a,&b,&c);	// get current machine position

//Display current values
	printf("Tool Record Index= %d  Length Set to %.17g\n",TWORD,Length);
	printf("Fixture Offset X=%.4g Y=%.4g Z=%.4g\n",OriginOffsetX,OriginOffsetY,OriginOffsetZ);
	printf("Global Offset  X=%.4g Y=%.4g Z=%.4g\n",AxisOffsetX,AxisOffsetY,AxisOffsetZ);
	printf("Tool Offset    X=%.4g Y=%.4g Z=%.4g\n",ToolOffsetX,ToolOffsetY,Length);
	printf("Cur Position   X=%.4g Y=%.4g Z=%.4g\n",curXpos,curYpos,curZpos);	
	printf("Mach Position  X=%.4g Y=%.4g Z=%.4g\n\n",machXpos,machYpos,machZpos);

//Determine if tool length offset (G43) active
	G43active=(HWORD>=0);

//Compute workspace position from machine position
	compX=machXpos-OriginOffsetX-AxisOffsetX-ToolOffsetX;
	compY=machYpos-OriginOffsetY-AxisOffsetY-ToolOffsetY;
	compZ=machZpos-OriginOffsetZ-AxisOffsetZ-(G43active*Length);
	printf("Computed Workspace Position X=%.4g Y=%.4g Z=%.4g\n",compX,compY,compZ);

//Compute machine position from workspace position
	compX=curXpos+OriginOffsetX+AxisOffsetX+ToolOffsetX;
	compY=curYpos+OriginOffsetY+AxisOffsetY+ToolOffsetY;
	compZ=curZpos+OriginOffsetZ+AxisOffsetZ+(G43active*Length);
	printf("Computed Machine Position   X=%.4g Y=%.4g Z=%.4g\n\n",compX,compY,compZ);

}  //end main