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

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


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

CKinematicsBasicMapping::CKinematicsBasicMapping()
{
	
	// Check if parameters exist in Kinematics.txt, if so update them and ask for confirmation value window yes/no, no abort
	// **** maybe add checks per return of getParameter ==2 and others along the way

	
	GetParameter("XmapInc", &XmapInc);
	GetParameter("YmapInc", &YmapInc);
	GetParameter("ZmapInc", &ZmapInc);
	GetParameter("AmapInc", &AmapInc);
	GetParameter("BmapInc", &BmapInc);
	GetParameter("CmapInc", &CmapInc);
	GetParameter("MaxLinearLength", &_MaxLinearLength);
	
	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, XscrewMap, &XmapValid); 
		}
		if (XmapValid) AfxMessageBox("X screw data read OK", MB_YESNO);
		else AfxMessageBox("Error in the read of X Screw Map File :(");
	}
	CString str1;
	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, YscrewMap, &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, ZscrewMap, &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, AscrewMap, &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, BscrewMap, &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, CscrewMap, &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)
{
	// find lengths of each actuator

	GeoCorrect(x,y,z,&x,&y,&z);

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

	if (XmapValid) {
		int xIncN = x / XmapInc; // find lower increment number of array measurement
		double xInMin = ((double)xIncN) * XmapInc;  // find in minimum 
		x1 = mapDouble(x, xInMin, xInMin + XmapInc, XscrewMap[xIncN], XscrewMap[xIncN + 1]); 
	}

	if (YmapValid) {
		int yIncN = y / YmapInc; // find lower increment number of array measurement
		double yInMin = ((double)yIncN) * YmapInc;  // find in minimum 
		y1 = mapDouble(y, yInMin, yInMin + YmapInc, YscrewMap[yIncN], YscrewMap[yIncN + 1]);
	}

	if (ZmapValid) {
		int zIncN = 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 = 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 = 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 = 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);
}

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, double *screwMap, bool *ScrewMapValid) // built from readgeotable()
{
	double screwInch;
	int row;
	
	*ScrewMapValid = false;

	if (name[0] == 0) return 0; // passing in no file exits without completion

	//FILE *f = fopen("\\KMotion\\Data\\" name, "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;
	}

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


	// **** what to do with array def???
	//if (GeoTable) delete[] GeoTable;
	//GeoTable = new CPT3D[screwMapNrows*NCols];
	//****

	// **** does this work without losing global name already made in .h?
	if (screwMap) delete[] screwMap;  // clear array
	screwMap = 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
		screwMap[row] = screwInch;

		//GeoTable[row*NCols + col].x = X;
		//GeoTable[row*NCols + col].y = Y;
		//GeoTable[row*NCols + col].z = Z;
	}

	fclose(f);

	*ScrewMapValid = true;
	return 0;
}