// PSDCalibrate.c by Omar Anwar, June 2017
// PSD (Position Sensitive Device) Callibration Program
// This is a program to automate the callibratation process of PSDs for LabBots
// in Robotics Lab, UWA
// The output of this program will be a text file containing the 128 values of table for 
// the sensor selected at the beggining. This table can be directly copied to hdt.txt and used.
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include "eyebot.h"
#include "types.h"

int main (){
	LCDSetPrintf(0, 0,"PSD Calibration Program");
	LCDSetPrintf(1, 0, "This program will genrate a text file containing");
	LCDSetPrintf(2, 0, "the PSD table used for calibration of PSDs.");
	LCDSetPrintf(3, 0, "This table can be copied to HDT file for use.");
	LCDSetPrintf(6, 0, "Choose the PSD you want to Calibrate");
	LCDMenu("Left (1)", "Front (2)", "Right (3)", "EXIT");
	int k = -1, PSD = 0, i = 0, j=0, temp =0, x=0, y=0;
	int RAWvalue = 0;
	int PrevRAWvalue = 0;
	int RAWvalueVect[7];
	int PSDtable[128];
	int dist = 300;
	int previous = 0;
	FILE *f;

	k = KEYGet(); //keyread for KEY4
	if (k != KEY4)
	{
		LCDClear();
		if (k == KEY1)
		{
			LCDSetPrintf(1, 0, "Calibrating Left Sensor i.e. PSD-1 ...");
			LCDSetPrintf(3, 0, "The robot must be at specified dist between left sensor and maze wall");
			LCDSetPrintf(5, 0, "Press START to begin the Calibration");
			PSD = 1;
			f = fopen("PSD_Left.txt", "w");
			if (f == NULL)
			{
				LCDSetPrintf(5, 0, "Error opening file!");
				return 0;
			}
			fprintf(f, "%s", "TABLE PSD_TableLeft\n");
		}
		if (k == KEY2)
		{
			LCDSetPrintf(1, 0, "Calibrating Front Sensor i.e. PSD-2 ...");
			LCDSetPrintf(3, 0, "The robot must be at specified distance between front sensor and the maze wall");
			LCDSetPrintf(5, 0, "Press START to begin the Calibration");
			PSD = 2;
			f = fopen("PSD_Front.txt", "w");
			if (f == NULL)
			{
				LCDSetPrintf(5, 0, "Error opening file!");
				return 0;
			}
			fprintf(f, "%s", "TABLE PSD_TableFront\n");
		}
		if (k == KEY3)
		{
			LCDSetPrintf(1, 0, "Calibrating Right Sensor i.e. PSD-3 ...");
			LCDSetPrintf(3, 0, "The robot must be at specified distance between right sensor and the maze wall");
			LCDSetPrintf(5, 0, "Press START to begin the Calibration");
			PSD = 3;
			f = fopen("PSD_Right.txt", "w");
			if (f == NULL)
			{
				LCDSetPrintf(5, 0, "Error opening file!");
				return 0;
			}
			fprintf(f, "%s", "TABLE PSD_TableRight\n");
		}
	}
	else
		return 0;

	LCDMenu("Start", "", "", "EXIT");
	k = KEYGet();
	if (k == KEY4) // EXIT
	{
		return 0;
	}
	else // Start Calibration
	{
		for (i = 0; i < 128; i++)
			PSDtable[i] = -1;
		dist = 300;
		/*RAWvalue = 0;
		RAWvalueVect[0] = PSDGetRaw(PSD); // acquire 7 samples
		RAWvalueVect[1] = PSDGetRaw(PSD);
		RAWvalueVect[2] = PSDGetRaw(PSD);
		RAWvalueVect[3] = PSDGetRaw(PSD);
		RAWvalueVect[4] = PSDGetRaw(PSD);
		RAWvalueVect[5] = PSDGetRaw(PSD);
		RAWvalueVect[6] = PSDGetRaw(PSD);
		for (m = 0; m < 7; m++) // sort them
		{
			for (n = m + 1; n < 7; n++)
			{
				if (RAWvalueVect[m] > RAWvalueVect[n])
				{
					temp = RAWvalueVect[m];
					RAWvalueVect[m] = RAWvalueVect[n];
					RAWvalueVect[n] = temp;
				}
			}
		}
		RAWvalue = (RAWvalueVect[2]+ RAWvalueVect[3]+ RAWvalueVect[4])/3;  // AVG 3 middle values
		RAWvalue = RAWvalue / 32;         // pick most significant 7 bits (Range 0 to 127)
		if (RAWvalue > 0)
			RAWvalue--;
		PSDtable[RAWvalue] = dist;*/
		previous = 0;

	}
	LCDClear();
	LCDMenu("Calibrate", "", "", "EXIT");
	LCDSetPrintf(3, 0, "Move the robot sensor at a distance of \n\n%d mm", dist);
	//LCDSetPrintf(0, 0, "%d mm distance written at location %d \n",dist, RAWvalue);
	
	k = KEYGet();
	while (k!=KEY4)
	{
		if (k == KEY1)
		{
			RAWvalue = 0;
			RAWvalueVect[0] = PSDGetRaw(PSD); // acquire 7 samples
			RAWvalueVect[1] = PSDGetRaw(PSD);
			RAWvalueVect[2] = PSDGetRaw(PSD);
			RAWvalueVect[3] = PSDGetRaw(PSD);
			RAWvalueVect[4] = PSDGetRaw(PSD);
			RAWvalueVect[5] = PSDGetRaw(PSD);
			RAWvalueVect[6] = PSDGetRaw(PSD);
			for (x = 0; x < 7; x++) // sort them
			{
				for (y = x + 1; y < 7; y++)
				{
					if (RAWvalueVect[x] > RAWvalueVect[y])
					{
						temp = RAWvalueVect[x];
						RAWvalueVect[x] = RAWvalueVect[y];
						RAWvalueVect[y] = temp;
					}
				}
			}
			
			RAWvalue = (RAWvalueVect[2] + RAWvalueVect[3] + RAWvalueVect[4]) / 3;  // AVG 3 middle values
			RAWvalue = RAWvalue / 32;         // pick most significant 7 bits (Range 0 to 127)
			if (RAWvalue > 0) //bring range from 1-128 -> 0-127
				RAWvalue--;
			if (RAWvalue >= PrevRAWvalue)
			{
				if (PSDtable[RAWvalue] == -1) // if location empty, put this value
					PSDtable[RAWvalue] = dist;
				else // if already filled, put the average of previous and current value
					PSDtable[RAWvalue] = (PSDtable[RAWvalue] + dist) / 2;

				PrevRAWvalue = RAWvalue;
				previous = 0;
				LCDClear();
				LCDMenu("Calibrate", "Previous", "Finish", "EXIT");
				LCDSetPrintf(1, 0, "Calibration Done, moving on to next point");
				sleep(1);
				LCDClear();
				if (RAWvalue < 127)
				{
					LCDMenu("Calibrate", "Previous", "Finish", "EXIT");
					LCDSetPrintf(1, 0, "%d mm distance written at location %d \n", PSDtable[RAWvalue], RAWvalue);
					dist = dist - 10;
					LCDSetPrintf(3, 0, "Now move the robot sensor at a distance of \n\n%d mm", dist);

				}
				else
				{
					LCDSetPrintf(1, 0, "%d mm distance written at location %d \n", dist, RAWvalue);
					LCDSetPrintf(3, 0, "Reached closest value possible, Finishing...");
					k = KEY3;
					sleep(1);
				}
			}
			else
			{
				sleep(1);
				LCDClear();
				LCDMenu("Calibrate", "Previous", "Finish", "EXIT");
				LCDSetPrintf(1, 0, "Invalid return from PSD, value ignored");
				dist = dist - 10;
				LCDSetPrintf(3, 0, "Now move the robot sensor at a distance of \n\n%d mm", dist);
			}			
		}
		if (k == KEY2 && previous == 0)
		{
			LCDClear();
			LCDMenu("Calibrate", "", "Finish", "EXIT");
			dist = dist + 10;
			LCDSetPrintf(5, 0, "Evaluating previous value again...");
			PSDtable[PrevRAWvalue] = -1;
			LCDSetPrintf(7, 0, "Move the robot sensor back at a distance of \n\n%d mm", dist);
			previous = 1;

		}
		if (k == KEY3 || dist == 0)
		{
			LCDClear();
			LCDMenu("", "", "", "Done");
			PSDtable[0] = 300;
			if(PSDtable[127] == -1) // if user finished before getiing to last value of table, make last value same as previously acquired distance
				PSDtable[127] = dist;

			fprintf(f, "%03d ", PSDtable[0]);

			for (i = 1; i < 127; i++)
			{
				if (PSDtable[i] == -1)
				{
					for (j = i+1; j < 128; j++)
					{
						if (PSDtable[j] != -1)
							break;
					}
					PSDtable[i] = PSDtable[i - 1]-((PSDtable[i - 1] - PSDtable[j]) / (j - i + 1));

				}
				fprintf(f, "%03d ", PSDtable[i]);
				if ((i - 7)%20 == 0)
					fprintf(f, "\n");
			}
			fprintf(f, "%03d ", PSDtable[127]);
			fprintf(f, "\n");
			fprintf(f, "END TABLE\n");
			fclose(f);
			LCDSetPrintf(1, 0, "Finished the Calibration Process");
			LCDSetPrintf(2, 0, "Check the text file for PSD Table");
			KEYWait(KEY4);
			return 0;

		}
		k = KEYGet();
	}
	return 0;
}
