// KinematicsBasicMapping.cpp: implementation of the CKinematicsBasicMapping class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "KinematicsBasicMapping.h"


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CKinematicsBasicMapping::CKinematicsBasicMapping()
{
	XscrewMap = NULL; // clear array pointer initial values
	YscrewMap = NULL;
	ZscrewMap = NULL;
	AscrewMap = NULL;
	BscrewMap = NULL;
	CscrewMap = NULL;
}


int CKinematicsBasicMapping::Initialize()
{

	GetParameter("XmapInc", &XmapInc);
	GetParameter("YmapInc", &YmapInc);
	GetParameter("ZmapInc", &ZmapInc);
	GetParameter("AmapInc", &AmapInc);
	GetParameter("BmapInc", &BmapInc);
	GetParameter("CmapInc", &CmapInc);
	GetParameter("MaxLinearLength", &_MaxLinearLength);
	GetParameter("WalkDistance", &walkDistance); 
	double temp;
	GetParameter("WalkIterations", &temp); 
	walkIterations = static_cast<int>(temp);
	
	int test=-1;

	if (XmapInc) { 
		if (AfxMessageBox("read X screw data file?", MB_YESNO) == IDYES) {
			test = ReadScrewData("XscrewData_inchBasic.txt", &XmapNrows, &XmapInc, &XmapOffset, 'X', &XmapValid);
		}
		if (XmapValid) AfxMessageBox("X screw data read OK");
		else AfxMessageBox("Error in the read of X Screw Map File :(");
	}

	/*
	CString str1; //extra value messages for debug only
	str1.Format("XmapInc = %g \n", XmapInc);
	AfxMessageBox(str1);

	CString str2;
	str2.Format("readScrewData result = %d \n", test);
	AfxMessageBox(str2);
	*/

	if (YmapInc) {
		if (AfxMessageBox("read Y screw data file?", MB_YESNO) == IDYES) {
			ReadScrewData("YscrewData_inchBasic.txt", &YmapNrows, &YmapInc, &YmapOffset, 'Y', &YmapValid);
		}
		if (YmapValid) AfxMessageBox("Y screw data read OK");
		else AfxMessageBox("Error in the read of Y Screw Map File :(");
	}

	if (ZmapInc) {
		if (AfxMessageBox("read Z screw data file?", MB_YESNO) == IDYES) {
			ReadScrewData("ZscrewData_inchBasic.txt", &ZmapNrows, &ZmapInc, &ZmapOffset, 'Z', &ZmapValid);
		}
		if (ZmapValid) AfxMessageBox("Z screw data read OK");
		else AfxMessageBox("Error in the read of Z Screw Map File :(");
	}

	if (AmapInc) {
		if (AfxMessageBox("read A screw data file?", MB_YESNO) == IDYES) {
			ReadScrewData("AscrewData_inchBasic.txt", &AmapNrows, &AmapInc, &AmapOffset, 'A', &AmapValid);
		}
		if (AmapValid) AfxMessageBox("A screw data read OK");
		else AfxMessageBox("Error in the read of A Screw Map File :(");
	}

	if (BmapInc) {
		if (AfxMessageBox("read B screw data file?", MB_YESNO) == IDYES) {
			ReadScrewData("BscrewData_inchBasic.txt", &BmapNrows, &BmapInc, &BmapOffset, 'B', &BmapValid);
		}
		if (BmapValid) AfxMessageBox("B screw data read OK");
		else AfxMessageBox("Error in the read of B Screw Map File :(");
	}

	if (CmapInc) {
		if (AfxMessageBox("read C screw data file?", MB_YESNO) == IDYES) {
			ReadScrewData("CscrewData_inchBasic.txt", &CmapNrows, &CmapInc, &CmapOffset, 'C', &CmapValid);
		}
		if (CmapValid) AfxMessageBox("C screw data read OK");
		else AfxMessageBox("Error in the read of C Screw Map File :(");
	}

	if (_MaxLinearLength == 0.0) {
		_MaxLinearLength = 1.0;  // this is temp to allow full loading with warning
		AfxMessageBox((CString)"Zero Value for MaxLinearLength in Kinematics.txt file.  Temp reset to 1.0 to allow finish loading.  Fix file and reload!");
	}
	m_MotionParams.MaxLinearLength = _MaxLinearLength;  // limit the segment lengths for nonlinear systems
	m_MotionParams.MaxRapidFRO = 1.0;       // limit the increase in Rapid HW FRO ****Does this get overwritten by KmotionCNC config file?
	m_MotionParams.UseOnlyLinearSegments = true;  // **** do i need this?  Does this get overwritten by KmotionCNC config file?
	
	// if there is an error, return -1;

	return 0;
}


CKinematicsBasicMapping::~CKinematicsBasicMapping()
{

}


int CKinematicsBasicMapping::TransformCADtoActuators(double x, double y, double z, double a, double b, double c, double *Acts, bool NoGeo)
{

	double x1 = x;
	double y1 = y;
	double z1 = z;
	double a1 = a;
	double b1 = b;
	double c1 = c;

	if (XmapValid) {
		if (CorrectScrewInch(&x1, XmapOffset, XmapInc, XmapNrows, XscrewMap)) {
			// if returns 1 then error
			AfxMessageBox((CString)"Error in X correcting screw inch");
		}
	}

	if (YmapValid) {
		if (CorrectScrewInch(&y1, YmapOffset, YmapInc, YmapNrows, YscrewMap)) {
			// if returns 1 then error
			AfxMessageBox((CString)"Error in Y correcting screw inch");
		}
	}

	if (ZmapValid) {
		if (CorrectScrewInch(&z1, ZmapOffset, ZmapInc, ZmapNrows, ZscrewMap)) {
			// if returns 1 then error
			AfxMessageBox((CString)"Error in Z correcting screw inch");
		}
	}

	if (AmapValid) {
		if (CorrectScrewInch(&a1, AmapOffset, AmapInc, AmapNrows, AscrewMap)) {
			// if returns 1 then error
			AfxMessageBox((CString)"Error in A correcting screw inch");
		}
	}

	if (BmapValid) {
		if (CorrectScrewInch(&b1, BmapOffset, BmapInc, BmapNrows, BscrewMap)) {
			// if returns 1 then error
			AfxMessageBox((CString)"Error in B correcting screw inch");
		}
	}

	if (CmapValid) {
		if (CorrectScrewInch(&c1, CmapOffset, CmapInc, CmapNrows, CscrewMap)) {
			// if returns 1 then error
			AfxMessageBox((CString)"Error in C correcting screw inch");
		}
	}


	Acts[0] = x1*m_MotionParams.CountsPerInchX;
	Acts[1] = y1*m_MotionParams.CountsPerInchY;
	Acts[2] = z1*m_MotionParams.CountsPerInchZ;

	Acts[3] = a1*m_MotionParams.CountsPerInchA;
	Acts[4] = b1*m_MotionParams.CountsPerInchB;
	Acts[5] = c1*m_MotionParams.CountsPerInchC;

	return 0;
}


// perform Inversion to go the other way

int CKinematicsBasicMapping::TransformActuatorstoCAD(double *Acts, double *xr, double *yr, double *zr, double *ar, double *br, double *cr, bool NoGeo)
{
	return InvertTransformCADtoActuators(Acts, xr, yr, zr, ar, br, cr, NoGeo);
}


// taken from Kinematics.cpp and added external file option to change walkIterations and more importantly waldDistance
int CKinematicsBasicMapping::InvertTransformCADtoActuators(double *Acts, double *xr, double *yr, double *zr, double *ar, double *br, double *cr, bool NoGeo)
{
	double Tol = 1e-6;
	double d = 0.1;					// should be linear over this range
	double x = 0, y = 0, z = 0, a = 0, b = 0, c = 0; // initial guess at answer
	double Acts0[MAX_ACTUATORS], ActsX[MAX_ACTUATORS], ActsY[MAX_ACTUATORS];
	double ActsZ[MAX_ACTUATORS], ActsA[MAX_ACTUATORS], ActsB[MAX_ACTUATORS], ActsC[MAX_ACTUATORS];
	double A[3 * 4];

	double ex, ey, ez, ea, eb, ec;

	for (int i = 0; i<walkIterations; i++)  // i<100 default
	{
		// measure sensitivity
		// 
		// assume majority is simply x -> Act0, y -> Act1, etc..

		TransformCADtoActuators(x, y, z, a, b, c, Acts0);
		TransformCADtoActuators(x + d, y, z, a, b, c, ActsX);
		TransformCADtoActuators(x, y + d, z, a, b, c, ActsY);
		TransformCADtoActuators(x, y, z + d, a, b, c, ActsZ);
		TransformCADtoActuators(x, y, z, a + d, b, c, ActsA);
		TransformCADtoActuators(x, y, z, a, b + d, c, ActsB);
		TransformCADtoActuators(x, y, z, a, b, c + d, ActsC);


		//   | x | | RX0  RY0  RZ0 |    | R0 |
		//   | y | | RX1  RY1  RZ1 | =  | R1 |
		//   | z | | RX2  RY2  RZ2 |    | R2 |


		A[0 * 4 + 0] = (ActsX[0] - Acts0[0]) / d;  // changes due to x
		A[1 * 4 + 0] = (ActsX[1] - Acts0[1]) / d;
		A[2 * 4 + 0] = (ActsX[2] - Acts0[2]) / d;

		A[0 * 4 + 1] = (ActsY[0] - Acts0[0]) / d;  // changes due to y
		A[1 * 4 + 1] = (ActsY[1] - Acts0[1]) / d;
		A[2 * 4 + 1] = (ActsY[2] - Acts0[2]) / d;

		A[0 * 4 + 2] = (ActsZ[0] - Acts0[0]) / d;  // changes due to z
		A[1 * 4 + 2] = (ActsZ[1] - Acts0[1]) / d;
		A[2 * 4 + 2] = (ActsZ[2] - Acts0[2]) / d;

		A[0 * 4 + 3] = Acts[0] - Acts0[0];   // desired changes
		A[1 * 4 + 3] = Acts[1] - Acts0[1];
		A[2 * 4 + 3] = Acts[2] - Acts0[2];

		Solve(A, 3);  // solve simultaneous eqs

					  // corrections in CAD space

		ex = A[0 * 4 + 3];
		ey = A[1 * 4 + 3];
		ez = A[2 * 4 + 3];
		ea = d*(Acts[3] - Acts0[3]) / (ActsA[3] - Acts0[3]);
		eb = d*(Acts[4] - Acts0[4]) / (ActsB[4] - Acts0[4]);
		ec = d*(Acts[5] - Acts0[5]) / (ActsC[5] - Acts0[5]);

		// Done if all within Tolerance

		if (fabs(ex) < Tol &&
			fabs(ey) < Tol &&
			fabs(ez) < Tol &&
			fabs(ea) < Tol &&
			fabs(eb) < Tol &&
			fabs(ec) < Tol)
		{
			*xr = x;
			*yr = y;
			*zr = z;
			*ar = a;
			*br = b;
			*cr = c;

			return 0;
		}

		// make a correction

		// limit corrections to small values

		if (ex > walkDistance) ex = walkDistance;
		if (ex < -walkDistance) ex = -walkDistance;
		if (ey > walkDistance) ey = walkDistance;
		if (ey < -walkDistance) ey = -walkDistance;

		x += ex;
		y += ey;
		z += ez;
		a += ea;
		b += eb;
		c += ec;
	}

	// it never converges, return whatever we have

	*xr = x;
	*yr = y;
	*zr = z;
	*ar = a;
	*br = b;
	*cr = c;

	return 1;
}

double CKinematicsBasicMapping::mapDouble(double startVal, double in_min, double in_max, double out_min, double out_max) {
	return (((startVal - in_min) * (out_max - out_min) / (in_max - in_min)) + out_min);
}


// read screw map data and store in map array
int CKinematicsBasicMapping::ReadScrewData(const char *name, int *screwMapNrows, double *mapInc, double *offset, char axis, bool *ScrewMapValid)
{
	double screwInch;
	int row;

	*ScrewMapValid = false;

	if (name[0] == 0) return 0; // passing in no file exits without completion
	
	//FILE *f = fopen((CString)MainPath + "\\Data\\Kinematics.txt", "rt");
	FILE *f = fopen((CString)MainPath + "\\Data\\" + name, "rt");

	//FILE *f = fopen(name, "rt");

	if (!f)
	{
		AfxMessageBox((CString)"RSD - Unable to open Screw Map File : " + name);
		return 1;
	}


	int result = fscanf(f, "%d", screwMapNrows);

	if (result != 1 || *screwMapNrows < 2 || *screwMapNrows > 4000)
	{
		fclose(f);
		CString str3;
		str3.Format("RSD - Invalid Screw Map File (screwMapNrows bad value) : %d \n", *screwMapNrows);
		AfxMessageBox(str3);
		//AfxMessageBox((CString)"RSD - Invalid Screw Map File (screwMapNrows bad value) : " + name);
		return 1;
	}
	/*
	CString str3;  // for debug 
	str3.Format("RSD - Screw Map File screwMapNrows = %d \n", *screwMapNrows);
	AfxMessageBox(str3);
	*/

	double tempInc = 0.0;
	result = fscanf(f, "%lf", &tempInc);

	if (result != 1)
	{
		fclose(f);
		AfxMessageBox((CString)"RSD - Invalid Screw Map File (increment bad value) : " + name);
		return 1;
	}

	if (tempInc != *mapInc)
	{
		fclose(f);
		AfxMessageBox((CString)"RSD - Invalid Screw Map File (increment does not match kinematics.txt) : " + name);
		return 1;
	}

	result = fscanf(f, "%lf", offset);

	if (result != 1)
	{
		fclose(f);
		AfxMessageBox((CString)"RSD - Invalid Screw Map File (offset bad value) : " + name);
		return 1;
	}


	double* tempArray;		// create a new array pointer

	tempArray = new double[*screwMapNrows];

	for (int i = 0; i<*screwMapNrows; i++)
	{
		result = fscanf(f, "%d,%lf", &row, &screwInch);

		if (result != 2 || row < 0 || row >= *screwMapNrows)
		{
			fclose(f);
			AfxMessageBox((CString)"RSD - Invalid Screw Map File (invalid row or row data value) : " + name);
			return 1;
		}

		// array update
		tempArray[row] = screwInch;
	}

	if (axis == 'X') {
		if (XscrewMap) delete[] XscrewMap;  // clear array from memory
		XscrewMap = tempArray;
	}
	else if (axis == 'Y') {
		if (YscrewMap) delete[] YscrewMap;  // clear array from memory
		YscrewMap = tempArray;
	}
	else if (axis == 'Z') {
		if (ZscrewMap) delete[] ZscrewMap;  // clear array from memory
		ZscrewMap = tempArray;
	}
	else if (axis == 'A') {
		if (AscrewMap) delete[] AscrewMap;  // clear array from memory
		AscrewMap = tempArray;
	}
	else if (axis == 'B') {
		if (BscrewMap) delete[] BscrewMap;  // clear array from memory
		BscrewMap = tempArray;
	}
	else if (axis == 'C') {
		if (CscrewMap) delete[] CscrewMap;  // clear array from memory
		CscrewMap = tempArray;
	}


	fclose(f);

	*ScrewMapValid = true;
	return 0;
}

// currently no error checking
int CKinematicsBasicMapping::CorrectScrewInch(double *screwInchPos, double mapOffset, double mapInc, int mapNrows, double *screwMap)
{
	if (((mapInc > 0.0) && (*screwInchPos > mapOffset)) || ((mapInc < 0.0) && (*screwInchPos < mapOffset))) { // positive or negative mapping and beyond start point
		int screwIncN = abs(static_cast<int>((*screwInchPos - mapOffset) / mapInc)); // find lower increment number of array measurement
		if (screwIncN < mapNrows - 1) {
			double screwRangeMin = (((double)screwIncN) * mapInc) + mapOffset;  // find in minimum 
			*screwInchPos = mapDouble(*screwInchPos, screwRangeMin, screwRangeMin + mapInc, screwMap[screwIncN], screwMap[screwIncN + 1]);
		}
		else { // beyond mapping so return screwInchPos from last mapped position plus expected over that corrected position
			*screwInchPos = screwMap[mapNrows - 1] + (*screwInchPos - ((mapNrows - 1)*mapInc + mapOffset));
		}
	}
	// else leave with no change to input since below start point of mapping

	return 0; // 0 for no error although no current error detecting performed
}