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

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


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

CKinematicsBasicMapping::CKinematicsBasicMapping()
{

	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;

	if (XmapInc) { // **** clean up extra message boxes here after debug done
		if (AfxMessageBox("read X screw data file?", MB_YESNO) == IDYES) {
			test = ReadScrewData(".\\KMotion\\Data\\XscrewData_inchBasic.txt", &XmapNrows, &XmapInc, &XmapOffset, 'X', &XmapValid);
		}
		if (XmapValid) AfxMessageBox("X screw data read OK", MB_YESNO);
		else AfxMessageBox("Error in the read of X Screw Map File :(");
	}
	CString str1; //**** cleanup these message boxes,maybe debug option or leave in commented out?
	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(".\\KMotion\\Data\\YscrewData_inchBasic.txt", &YmapNrows, &YmapInc, &YmapOffset, 'Y', &YmapValid);
		}
		if (YmapValid) AfxMessageBox("Y screw data read OK", MB_YESNO);
		else AfxMessageBox("Error in the read of Y Screw Map File :(");
	}

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

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

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

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

	if (_MaxLinearLength == 0.0) _MaxLinearLength = 1.0;  // **** this is temp to test fault
	m_MotionParams.MaxLinearLength = _MaxLinearLength;  // limit the segment lengths for nonlinear systems
	m_MotionParams.MaxRapidFRO = 1.0;       // limit the increase in Rapid HW FRO
	m_MotionParams.UseOnlyLinearSegments = true;  // **** do i need this?
	
}


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 (x > 0.0) { 
			int xIncN = static_cast<int>(x / XmapInc); // find lower increment number of array measurement
			if (xIncN < XmapNrows - 1) {
				double xInMin = ((double)xIncN) * XmapInc;  // find in minimum 
				x1 = mapDouble(x, xInMin, xInMin + XmapInc, XscrewMap[xIncN], XscrewMap[xIncN + 1]);
			}
			else {// beyond mapping so return x from last mapped plus expected over that corrected position
				x1 = XscrewMap[XmapNrows - 1] + (x-(XmapNrows - 1)*XmapInc);
			}
		}
		// else return with x only without mapping since below range
	}

	if (YmapValid) {
		if (y > 0.0) {
			int yIncN = static_cast<int>(y / YmapInc); // find lower increment number of array measurement
			if (yIncN < YmapNrows - 1) {
				double yInMin = ((double)yIncN) * YmapInc;  // find in minimum 
				y1 = mapDouble(y, yInMin, yInMin + YmapInc, YscrewMap[yIncN], YscrewMap[yIncN + 1]);
			}
			else {// beyond mapping so return x from last mapped plus expected over that corrected position
				y1 = YscrewMap[YmapNrows - 1] + (y - (YmapNrows - 1)*YmapInc);
			}
		}
		// else return with x only without mapping since below range
	}

	//**** work out negative direction, probably from increment value in map and kinematic.txt file
	if (ZmapValid) {
		int zIncN = static_cast<int>(z / ZmapInc); // find lower increment number of array measurement
		double zInMin = ((double)zIncN) * ZmapInc;  // find in minimum 
		z1 = mapDouble(z, zInMin, zInMin + ZmapInc, ZscrewMap[zIncN], ZscrewMap[zIncN + 1]);
	}

	if (AmapValid) {
		int aIncN = static_cast<int>(a / AmapInc); // find lower increment number of array measurement
		double aInMin = ((double)aIncN) * AmapInc;  // find in minimum 
		a1 = mapDouble(a, aInMin, aInMin + AmapInc, AscrewMap[aIncN], AscrewMap[aIncN + 1]);
	}

	if (BmapValid) {
		int bIncN = static_cast<int>(b / BmapInc); // find lower increment number of array measurement
		double bInMin = ((double)bIncN) * BmapInc;  // find in minimum 
		b1 = mapDouble(b, bInMin, bInMin + BmapInc, BscrewMap[bIncN], BscrewMap[bIncN + 1]);
	}

	if (CmapValid) {
		int cIncN = static_cast<int>(c / CmapInc); // find lower increment number of array measurement
		double cInMin = ((double)cIncN) * CmapInc;  // find in minimum 
		c1 = mapDouble(c, cInMin, cInMin + CmapInc, CscrewMap[cIncN], CscrewMap[cIncN + 1]);
	}


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


// **** this is ignoring offset!!! assumes offset is always 0
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(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;  // **** temp for test
	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];

	// **** should offset be applied within here??
	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;
}