/**********************************************************************
	Copyright (C) 2018  MisfitTech LLC,  All rights reserved.

 	MisfitTech uses a dual license model that allows the software to be used under
	a standard GPL open source license, or a commercial license.  The standard GPL
	license  requires that all software statically linked with MisfitTec Code is
	also distributed under the same GPL V2 license terms.  Details of both license
	options follow:

	- Open source licensing -
	MisfitTech is a free download and may be used, modified, evaluated and
	distributed without charge provided the user adheres to version two of the GNU
	General Public License (GPL) and does not remove the copyright notice or this
	text.  The GPL V2 text is available on the gnu.org web site

	- Commercial licensing -
	Businesses and individuals that for commercial or other reasons cannot comply
	with the terms of the GPL V2 license must obtain a low cost commercial license
	before incorporating MisfitTech code into proprietary software for distribution in
	any form.  Commercial licenses can be purchased from www.misfittech.net
	and do not require any source files to be changed.


	This code is distributed in the hope that it will be useful.  You cannot
	use MisfitTech's code unless you agree that you use the software 'as is'.
	MisfitTech's code is provided WITHOUT ANY WARRANTY; without even the implied
	warranties of NON-INFRINGEMENT, MERCHANTABILITY or FITNESS FOR A PARTICULAR
	PURPOSE. MisfitTech LLC disclaims all conditions and terms, be they
	implied, expressed, or statutory.


    Written by Trampas Stern for MisfitTech.

    Misfit Tech invests time and resources providing this open source code,
    please support MisfitTech and open-source hardware by purchasing
	products from MisfitTech, www.misifittech.net!
 *********************************************************************/
#include "calibration.h"
#include "Flash.h"
#include "nonvolatile.h"
#include "board.h" //for divide with rounding macro
#include "utils.h"


static uint16_t getTableIndex(uint16_t value)
{
	int32_t x;

	x=((int32_t)value*CALIBRATION_TABLE_SIZE)/CALIBRATION_STEPS; //the divide is a floor not a round which is what we want
	return (uint16_t)x;

}
static uint16_t interp(Angle x1, Angle y1, Angle x2, Angle y2, Angle x)
{
	int32_t dx,dy,dx2,y;
	dx=x2-x1;
	dy=y2-y1;
	dx2=x-x1;
	y=(int32_t)y1+DIVIDE_WITH_ROUND((dx2*dy),dx);
	if (y<0)
	{
		y=y+CALIBRATION_STEPS;
	}
	return (uint16_t)y;
}

static void printData(int32_t *data, int32_t n)
{
	int32_t i;
	Serial.print("\n\r");
	for (i=0; i<n; i++)
	{
		Serial.print(data[i]);
		if (i!=(n-1))
		{
			Serial.print(",");
		}
	}
	Serial.print("\n\r");
}
bool CalibrationTable::updateTableValue(int32_t index, int32_t value)
{

	table[index].value=value;
	table[index].error=CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE; //or error is roughly like variance, so set it to span to next calibration value.
	return true;

}
void CalibrationTable::printCalTable(void)
{
	int i;
	Serial.print("\n\r");
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
	{
		Serial.print((uint16_t)table[i].value);
		Serial.print(",");
	}
	Serial.print("\n\r");
}

Angle CalibrationTable::fastReverseLookup(Angle encoderAngle)
{
#ifdef NZS_FAST_CAL
	//assume calibration is good
	if (fastCalVaild)
	{
		uint16_t x;
		x=((uint16_t)encoderAngle)/4;  //we only have 16384 values in table

		return (Angle)NVM->FastCal.angle[x];
	}else
	{
		return reverseLookup(encoderAngle);
	}
#else
	return reverseLookup(encoderAngle)
#endif
}

Angle CalibrationTable::reverseLookup(Angle encoderAngle)
{

	int32_t i=0;
	int32_t a1,a2;
	int32_t x;
	int16_t y;
	int32_t min,max;
	min=(uint16_t)table[0].value;
	max=min;



	for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
	{
		x=(uint16_t)table[i].value;
		if (x<min)
		{
			min=x;
		}
		if (x>max)
		{
			max=x;
		}
	}


	x=(uint16_t)encoderAngle;
	if (x<min)
	{
		x=x+CALIBRATION_STEPS;
	}

	i=0;

	while (i<CALIBRATION_TABLE_SIZE)
	{
		a1=(uint16_t)table[i].value;

		//handle index wrap around
		if (i==(CALIBRATION_TABLE_SIZE-1))
		{
			a2=(uint16_t)table[0].value;
		}else
		{
			a2=(uint16_t)table[i+1].value;
		}

		//wrap
		if (abs(a1-a2)>CALIBRATION_STEPS/2)
		{
			if (a1<a2)
			{
				a1=a1+CALIBRATION_STEPS;
			}else
			{
				a2=a2+CALIBRATION_STEPS;
			}

			//LOG("xxxx %d %d %d",a1,a2,x);
		}

		//finding matching location
		if ( (x>=a1 && x<=a2) ||
				(x>=a2 && x<=a1) )
		{
			//LOG("%d", i);
			// inerpolate results and return
			//LOG("%d %d %d",a1,a2,x);
			//LOG("%d,%d",(i*CALIBRATION_MAX)/CALIBRATION_TABLE_SIZE,((i+2)*CALIBRATION_MAX)/CALIBRATION_TABLE_SIZE);

			y=interp(a1, DIVIDE_WITH_ROUND((i*CALIBRATION_STEPS),CALIBRATION_TABLE_SIZE), a2, DIVIDE_WITH_ROUND( ((i+1)*CALIBRATION_STEPS),CALIBRATION_TABLE_SIZE), x);

			return y;
		}
		i++;
	}
	ERROR("WE did some thing wrong");




}


void CalibrationTable::smoothTable(void)
{
	uint16_t b[]={1,2,4,5,4,2,1};
	uint16_t sum_b=19;  //sum of b filter

	int32_t data[CALIBRATION_TABLE_SIZE];
	int32_t table2[CALIBRATION_TABLE_SIZE];

	int32_t i;
	int32_t offset=0;
	int32_t startNum;

	//first lets handle the wrap around in the table
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
	{
		if (i>0 && offset==0)
		{
			if(((uint16_t)table[i-1].value-(uint16_t)table[i].value) <-32768)
			{
				offset=-65536;
			}

			if (((uint16_t)table[i-1].value-(uint16_t)table[i].value) > 32768)
			{
				offset=65536;
			}
		}
		table2[i]=(int32_t)((uint16_t)table[i].value)+offset;
	}

	//Serial.print("after wrap\n\r");
	//printData(table2,CALIBRATION_TABLE_SIZE);

	//remove the starting offset and compensate table for index
	startNum=table2[0];
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
	{
		table2[i]=table2[i]-startNum - (i*65536)/CALIBRATION_TABLE_SIZE;
	}

	//Serial.print("after phase comp\n\r");
	//printData(table2,CALIBRATION_TABLE_SIZE);

	//filter the data
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
	{
		int j,ix,ib;;
		int32_t sum=0;

		ib=0;
		for (j=i-3; j<i+4; j++)
		{
			ix=j;
			if (ix<0)
			{
				ix=ix+CALIBRATION_TABLE_SIZE;
			}
			if (ix>=CALIBRATION_TABLE_SIZE)
			{
				ix=ix-CALIBRATION_TABLE_SIZE;
			}
			if (i==0)
			{
				LOG("index %d",ix);
			}
			sum=sum+table2[ix]*b[ib];
			ib++;
		}
		sum=DIVIDE_WITH_ROUND(sum,sum_b);
		data[i]=sum;
	}

	//Serial.print("after filter\n\r");
	//printData(data,CALIBRATION_TABLE_SIZE);

	//add in offset and the phase compenstation
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
	{
		data[i]=data[i]+startNum + (i*65536)/CALIBRATION_TABLE_SIZE;
	}

	//Serial.print("after phase comp added\n\r");
	//printData(data,CALIBRATION_TABLE_SIZE);

	//remove the uint16_t wrap
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
	{
		if (data[i]>=65536)
		{
			data[i]=data[i]-65536;
		}
	}

	//Serial.print("after wrap added\n\r");
	//printData(data,CALIBRATION_TABLE_SIZE);

	//save new table
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
	{
		table[i].value=data[i];
	}
}

void CalibrationTable::saveToFlash(void)
{
	FlashCalData_t data;
	int i;
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++ )
	{
		data.table[i]=(uint16_t)table[i].value;
	}
	data.status=true;

	LOG("Writting Calbiration to Flash");
	nvmWriteCalTable(&data,sizeof(data));

	memset(&data,0,sizeof(data));
	memcpy(&data, &NVM->CalibrationTable,sizeof(data));
	createFastCal();

	LOG("after writting status is %d",data.status);
	loadFromFlash();

}

void CalibrationTable::loadFromFlash(void)
{
	FlashCalData_t data;
	int i;
	LOG("Reading Calbiration to Flash");
	memcpy(&data, &NVM->CalibrationTable,sizeof(data));
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++ )
	{
		table[i].value=Angle(data.table[i]);
		table[i].error=CALIBRATION_MIN_ERROR;
	}
	data.status=true;
}

bool CalibrationTable::flashGood(void)
{
	LOG("calibration table status is: %d",NVM->CalibrationTable.status);
	return NVM->CalibrationTable.status;
}


void CalibrationTable::createFastCal(void)
{
#ifdef NZS_FAST_CAL
	int32_t i;
	uint16_t cs=0;
	uint16_t data[256];
	int32_t j;
	j=0;
	cs=0;
	LOG("setting fast calibration");
	for (i=0; i<16384; i++)
	{

		uint16_t x;
		x=reverseLookup(i*4);
		data[j]=x;
		j++;
		if (j>=256)
		{
			flashWrite(&NVM->FastCal.angle[i-255],data,256*sizeof(uint16_t));
			//LOG("Wrote fastcal at index %d-%d", i-255, i);
			j=0;
		}
		cs+=x;
	}
	//update the checksum
	flashWrite(&NVM->FastCal.checkSum,&cs,sizeof(uint16_t));
	fastCalVaild=true;

	//this is a quick test
	/*
			for (i=0; i<16384; i++)
			{
				LOG("fast Cal %d,%d,%d",i,NVM->FastCal.angle[i],(uint32_t)reverseLookup(i*4));
			}
	 */
#endif
}
void CalibrationTable::updateFastCal(void)
{
#ifdef NZS_FAST_CAL
	int32_t i;
	uint16_t cs=0;
	uint16_t data[256];
	int32_t j;
	bool NonZero=false;
	for (i=0; i<16384; i++)
	{
		cs+=NVM->FastCal.angle[i];
		if (cs != 0)
		{
			NonZero=true;
		}
	}
	if (cs!=NVM->FastCal.checkSum || NonZero==false)
	{
		createFastCal();
	}
	else
	{
		LOG("fast cal is valid");
		fastCalVaild=true;
	}
#endif
}

void CalibrationTable::init(void)
{
	int i;

	if (true == flashGood())
	{
		loadFromFlash();
		updateFastCal();
	}else
	{
		for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
		{
			table[i].value=0;
			table[i].error=CALIBRATION_ERROR_NOT_SET;
		}
	}
	return;
}

#if 0 
//This code was removed because with micro stepping we can not assume
// our actualAngle is correct. 
void CalibrationTable::updateTable(Angle actualAngle, Angle encoderValue);
{
	static int32_t lastAngle=-1;
	static uint16_t lastEncoderValue=0;

	if (last != -1)
	{
		int32_t dist;

		//hopefull we can use the current point and last point to interpolate and set a value or two in table.
		dist=abs((int32_t)actualAngle-(int32_t)lastAngle); //distance between the two angles

		//since our angles wrap the shortest distance will be one less than 32768
		if (dist>CALIBRATION_STEPS/2)
		{
			dist=dist-CALIBRATION_STEPS;
		}

		//if our distance is larger than size between calibration points in table we will ignore this sample
		if (dist>CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE)
		{
			//spans two or more table calibration points for this implementation we will not use
			lastIndex=(int32_t)index;
			lastValue=value;
			return;
		}

		//now lets see if the values are above and below a table calibration point
		dist= abs(getTableIndex(lastAngle)-getTableIndex(actualAngle));
		if (dist != 0) //if the two indexs into table are not the same it spans a calibration point in table.
		{
			//the two span a set calibation table point.
			uint16_t newValue;
			newValue=interp(lastAngle, lastEncoderValue, actualAngle, encoderValue, getTableIndex(actualAngle)*(CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE))
    						  //this new value is our best guess as to the correct calibration value.
    						  updateTableValue(getTableIndex(actualAngle),newValue);
		} else
		{
			//we should calibate the table value for the point the closest
		}





	}
	lastAngle=(int32_t)actualAngle;
	lastEncoderValue=encoderValue;

}
#endif

//when we are microstepping and are in between steps the probability the stepper motor did not move
// is high. That is the actualAngle will be correct but the encoderValue will be behind due to not having enough torque to move motor. 
// Therefore we only want to update the calibration on whole steps where we have highest probability of things being correct. 
void CalibrationTable::updateTable(Angle actualAngle, Angle encoderValue)
{
	int32_t dist, index;
	Angle tableAngle;

	index = getTableIndex((uint32_t)actualAngle+CALIBRATION_STEPS/CALIBRATION_TABLE_SIZE/2);  //add half of distance to next entry to round to closest table index

	tableAngle=(index*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE; //calculate the angle for this index

	dist=tableAngle-actualAngle;  //distance to calibration table angle

	//LOG("Dist is %d",dist);
	if (abs(dist)<CALIBRATION_MIN_ERROR) //if we are with in our minimal error we can calibrate
	{
		updateTableValue(index,(int32_t)encoderValue);
	}
}

bool CalibrationTable::calValid(void)
{
	uint32_t i;
	for (i=0; i<CALIBRATION_TABLE_SIZE; i++)
	{
		if (table[i].error == CALIBRATION_ERROR_NOT_SET)
		{
			return false;
		}
	}
	if (false == flashGood())
	{
		saveToFlash();
	}
	return true;
}
//We want to linearly interpolate between calibration table angle
int CalibrationTable::getValue(Angle actualAngle, CalData_t *ptrData)
{
	int32_t indexLow,indexHigh;
	int32_t angleLow,angleHigh;
	uint16_t value;
	int32_t y1,y2;
	int16_t err;

	indexLow=getTableIndex((uint16_t)actualAngle);
	// LOG("index %d, actual %u",indexLow, (uint16_t)actualAngle);
	indexHigh=indexLow+1;

	angleLow=(indexLow*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE;
	angleHigh=(indexHigh*CALIBRATION_STEPS)/CALIBRATION_TABLE_SIZE;

	if (indexHigh>=CALIBRATION_TABLE_SIZE)
	{
		indexHigh -= CALIBRATION_TABLE_SIZE;
	}

	//LOG("AngleLow %d, AngleHigh %d",angleLow,angleHigh);
	//LOG("TableLow %u, TableHigh %d",(uint16_t)table[indexLow].value,(uint16_t)table[indexHigh].value);
	y1=table[indexLow].value;
	y2=table[indexHigh].value;

	//handle the wrap condition
	if (abs(y2-y1)>CALIBRATION_STEPS/2)
	{
		if (y2<y1)
		{
			y2=y2+CALIBRATION_STEPS;
		}else
		{
			y1=y1+CALIBRATION_STEPS;
		}
	}

	value=interp(angleLow, y1, angleHigh, y2,actualAngle);

	//handle the wrap condition
	if (value>=CALIBRATION_STEPS)
	{
		value=value-CALIBRATION_STEPS;
	}

	err=table[indexLow].error;
	if (table[indexHigh].error > err)
	{
		err=table[indexHigh].error;
	}

	if (table[indexLow].error == CALIBRATION_ERROR_NOT_SET ||
			table[indexHigh].error == CALIBRATION_ERROR_NOT_SET)
	{
		err=CALIBRATION_ERROR_NOT_SET;
	}
	ptrData->value=value;
	ptrData->error=err;

	return 0;

}

Angle CalibrationTable::getCal(Angle actualAngle)
{
	CalData_t data;
	getValue(actualAngle, &data);
	return data.value;
}