// 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()

	// promptLevel...
	// 0 for no prompts except error
	// 1 single request to map or not and confirmation msg mapping successful for list of axis
	// 2 request mapping of each axis with confirmation per
	// 3 same as 2 but shows values read for better debugging
{

	// read in values from Kinematics.txt
	// check for errors, and if found, send error message if value is not found or not valid and exit without mapping
	if (GetParameter("XmapInc", &XmapInc)) {
		AfxMessageBox("XmapInc not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	if (GetParameter("YmapInc", &YmapInc)) {
		AfxMessageBox("YmapInc not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	if (GetParameter("ZmapInc", &ZmapInc)) {
		AfxMessageBox("ZmapInc not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	if (GetParameter("AmapInc", &AmapInc)) {
		AfxMessageBox("AmapInc not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	if (GetParameter("BmapInc", &BmapInc)) {
		AfxMessageBox("BmapInc not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	if (GetParameter("CmapInc", &CmapInc)) {
		AfxMessageBox("CmapInc not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	if (GetParameter("MaxLinearLength", &_MaxLinearLength)) {
		AfxMessageBox("MaxLinearLength not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	if (GetParameter("WalkDistance", &walkDistance)) {
		AfxMessageBox("WalkDistance not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	double temp;
	if (GetParameter("WalkIterations", &temp)) {
		AfxMessageBox("WalkIterations not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	else walkIterations = static_cast<int>(temp);
	if (GetParameter("Prompts", &temp)) {
		AfxMessageBox("Prompts not found in Kinematics.txt file.\nScrew Mapping Aborted!");
		return 0;
	}
	else promptLevel = static_cast<int>(temp);

	bool mappingApplied =  0; // 0 index as indicator if any maps applied
	bool applyMapping = 1; // flag if mapping should be applied
	CString str1; // temp string used for message outputs
	CString strAxisMapped; // string used for output which axis were mapped

	// warning if no (axis)mapInc values were set to non zero in Kinematics.txt resulting in no screw mapping
	if (!XmapInc && !YmapInc && !ZmapInc && !AmapInc && !BmapInc && !CmapInc) {
		AfxMessageBox("Kinematics.txt file present without any mapping declared.");
	}
	else if(promptLevel){
		if (AfxMessageBox("Apply Screw Mapping?", MB_YESNO) == IDYES) {
		}
		else applyMapping = 0;
	}

	if (applyMapping) {
		bool test = 1;
		if (XmapInc) {
			if(promptLevel>1){
				if (AfxMessageBox("Read X screw data file?", MB_YESNO) == IDNO) test = 0;
			}
			if (test) {
				ReadScrewData("XscrewData_inchBasic.txt", &XmapNrows, &XmapInc, &XmapOffset, 'X', &XmapValid);
			}
			OutputMsgs(&mappingApplied, &strAxisMapped, XmapNrows, XmapInc, XmapOffset, 'X', XmapValid);
			test = 1;
		}

		if (YmapInc) {
			if (promptLevel>1) {
				if (AfxMessageBox("Read Y screw data file?", MB_YESNO) == IDNO) test = 0;
			}
			if (test) {
				ReadScrewData("YscrewData_inchBasic.txt", &YmapNrows, &YmapInc, &YmapOffset, 'Y', &YmapValid);
			}
			OutputMsgs(&mappingApplied, &strAxisMapped, YmapNrows, YmapInc, YmapOffset, 'Y', YmapValid);
			test = 1;
		}

		if (ZmapInc) {
			if (promptLevel>1) {
				if (AfxMessageBox("Read Z screw data file?", MB_YESNO) == IDNO) test = 0;
			}
			if (test) {
				ReadScrewData("ZscrewData_inchBasic.txt", &ZmapNrows, &ZmapInc, &ZmapOffset, 'Z', &ZmapValid);
			}
			OutputMsgs(&mappingApplied, &strAxisMapped, ZmapNrows, ZmapInc, ZmapOffset, 'Z', ZmapValid);
			test = 1;
		}

		if (AmapInc) {
			if (promptLevel>1) {
				if (AfxMessageBox("Read A screw data file?", MB_YESNO) == IDNO) test = 0;
			}
			if (test) {
				ReadScrewData("AscrewData_inchBasic.txt", &AmapNrows, &AmapInc, &AmapOffset, 'A', &AmapValid);
			}
			OutputMsgs(&mappingApplied, &strAxisMapped, AmapNrows, AmapInc, AmapOffset, 'A', AmapValid);
			test = 1;
		}

		if (BmapInc) {
			if (promptLevel>1) {
				if (AfxMessageBox("Read B screw data file?", MB_YESNO) == IDNO) test = 0;
			}
			if (test) {
				ReadScrewData("BscrewData_inchBasic.txt", &BmapNrows, &BmapInc, &BmapOffset, 'B', &BmapValid);
			}
			OutputMsgs(&mappingApplied, &strAxisMapped, BmapNrows, BmapInc, BmapOffset, 'B', BmapValid);
			test = 1;
		}

		if (CmapInc) {
			if (promptLevel>1) {
				if (AfxMessageBox("Read C screw data file?", MB_YESNO) == IDNO) test = 0;
			}
			if (test) {
				ReadScrewData("CscrewData_inchBasic.txt", &CmapNrows, &CmapInc, &CmapOffset, 'C', &CmapValid);
			}
			OutputMsgs(&mappingApplied, &strAxisMapped, CmapNrows, CmapInc, CmapOffset, 'C', CmapValid);
			test = 1;
		}
	}

	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. probably best to match the smallest increment size of mapped screws or shorter
	//m_MotionParams.MaxRapidFRO = 1.0;  // probably do not need this.  limit the increase in Rapid HW FRO 
	m_MotionParams.UseOnlyLinearSegments = true;  // probably good, but KmotionCNC config file may overwrite whatever is set here

	// if no mapping found output message
	if (!mappingApplied) {
		AfxMessageBox("NO Screw Mapping applied!");
	}
	else { // message which axis did have mapping applied successfully
		str1.Format("Screw Mapping applied!\n  Axis %s\n", strAxisMapped);
		if(promptLevel) AfxMessageBox(str1);
	}

	// if there is an error, return -1; // not sure what if any good error return does since testing did not stop any motion or give any warnings from calling function

	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) {
		CorrectScrewInch(&x1, XmapOffset, XmapInc, XmapNrows, XscrewMap);
	}

	if (YmapValid) {
		CorrectScrewInch(&y1, YmapOffset, YmapInc, YmapNrows, YscrewMap);
	}

	if (ZmapValid) {
		CorrectScrewInch(&z1, ZmapOffset, ZmapInc, ZmapNrows, ZscrewMap);
	}

	if (AmapValid) {
		CorrectScrewInch(&a1, AmapOffset, AmapInc, AmapNrows, AscrewMap);
	}

	if (BmapValid) {
		CorrectScrewInch(&b1, BmapOffset, BmapInc, BmapNrows, BscrewMap);
	}

	if (CmapValid) {
		CorrectScrewInch(&c1, CmapOffset, CmapInc, CmapNrows, CscrewMap);
	}

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


// map input double from input range to output range
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");

	if (!f)
	{
		AfxMessageBox((CString)"RSD - Unable to open Screw Map File : \n   " + 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);
		return 1;
	}

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

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

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

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

	if (result != 1)
	{
		fclose(f);
		AfxMessageBox((CString)"RSD - Invalid Screw Map File :\n   " + name + "\n  (offset bad value)");
		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  :\n   " + name + "\n  (invalid row or row data value) : ");
			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;
}


// this does the screw correction 
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; 
}


// output message prompts with message boxes per promptLevel
void CKinematicsBasicMapping::OutputMsgs(bool *mapApplied, CString *axisList, int screwMapNrows, double mapInc, double offset, char axis, bool ScrewMapValid) {
	
	CString str4;
	CString str5;
	
	if (ScrewMapValid) {	
		if (promptLevel == 2) {
			str4.Format("%c screw data read OK\n", axis);
			AfxMessageBox(str4);
		}

		if (promptLevel == 3) {
			str4.Format("%c axis map values\n", axis);
			str5 = str4;
			str4.Format("   %g\" = mapInc size\n", mapInc);
			str5 = str5 + str4;
			str4.Format("   %d = Number of Rows\n", screwMapNrows);
			str5 = str5 + str4;
			str4.Format("   %g\" = mapOffset (start of mapping)\n", offset);
			str5 = str5 + str4;
			double endInch = offset + (screwMapNrows - 1)*mapInc;
			str4.Format("   %g\" = end of mapping\n", endInch);
			str5 = str5 + str4;
			AfxMessageBox(str5);

		}
		if (*mapApplied) {
			*axisList = *axisList + (CString)", ";
		}
		else *mapApplied = 1;

		*axisList = *axisList + (CString)axis;
	}
	else {
		str4.Format("%c, NO Screw Mapping applied!\n", axis);
		AfxMessageBox(str4);
	}
}