#include "config.h"
#include "util.h"
#include "glcd.h"
#include "fnt58.h"
#include <stdlib.h>
#include <string.h>

#define STICKBUTTON 1
#define MENUBUTTON 2
#define STOPBUTTON 4

extern void UpdateAnalog(void);
extern void UpdateButtons(void);
extern void DrawBatteryVoltage(unsigned char x, unsigned char y);

extern int joyx, joyy;
extern params_t params;
extern unsigned char buttons;
extern long 		  batteryUpdateCount;

unsigned char calibrate_joy(void);
unsigned char calibrate_battery(void);
void waitbuttonsrelease(void);
void calib_lcd_clear(void);


params_t  cal_params;

void calib_idle()
{
	UpdateButtons();
	UpdateAnalog();
}

unsigned char getbuttons()
{
	unsigned char b=0;
	
	while (b==0)
	{
		b|=buttons;
		calib_idle();
	}
		
	while (buttons!=0)
		{
		b|=buttons;
		calib_idle();
		}
		
	return b;
}

void waitbuttonsrelease()
{
	while (buttons!=0)
		{
		calib_idle();
		}
}

unsigned char pollbuttons()
{
	calib_idle();
	return buttons;
}
	
void calibrate()
{
	unsigned char b;
	
	cal_params=params; // make backup of parameters.

	if (!calibrate_joy())
		goto abort;
	waitbuttonsrelease();
	
	if (!calibrate_battery())
		goto abort;
	waitbuttonsrelease();
		
	
	calib_lcd_clear();

	GLCD_DrawStringC(0,16,"Configuration complete.", fnt58);
	GLCD_DrawStringC(0,24,"hit Menu to save, or", fnt58);
	GLCD_DrawStringC(0,32,"Stop to cancel", fnt58);

	b=getbuttons();
	if (b&STOPBUTTON)
		goto abort;
			
	calib_lcd_clear();


	calib_lcd_clear();
	GLCD_DrawStringC(0,16, "Writing configuration...", fnt58);

	if (savePrefs())
		GLCD_DrawStringC(0,24, "... success!", fnt58);
	else
		GLCD_DrawStringC(0,24, "... failure!", fnt58);
	
	GLCD_DrawStringC(68,56, "[press menu]", fnt58);
	getbuttons();

	GLCD_Clear();
	batteryUpdateCount=0;

	return;
	
abort:
	params=cal_params;
	GLCD_Clear();
	batteryUpdateCount=0;
	return;
}

void calib_lcd_clear()
{
	GLCD_Clear();
	GLCD_DrawCharXor=255;
	//                     1234567890123456789012345
	GLCD_DrawStringC(0, 0," OrcPad Calibration      ", fnt58);
	GLCD_DrawCharXor=0;

}

unsigned char calibrate_joy()
{
	unsigned char b;
	unsigned char s[6];
	unsigned char z;
	
	calib_lcd_clear();
	
	//                     1234567890123456789012345
	GLCD_DrawStringC(0,16,"Move through full range,", fnt58);
	GLCD_DrawStringC(0,24,"center knob, hit 'menu'.", fnt58);
			
	GLCD_DrawStringC(20, 40, "cur", fnt58);
	GLCD_DrawStringC(60, 40, "min", fnt58);
	GLCD_DrawStringC(100, 40, "max", fnt58);

	params.joyymin=9999;
	params.joyymax=0;
	params.joyxmin=9999;
	params.joyxmax=0;
	
	waitbuttonsrelease();
	
	while(1)
		{
		if (joyx>params.joyxmax)
			params.joyxmax=joyx;
		if (joyx<params.joyxmin)
			params.joyxmin=joyx;
			
		if (joyy>params.joyymax)
			params.joyymax=joyy;
		if (joyy<params.joyymin)
			params.joyymin=joyy;
		
		GLCD_DrawStringC(0, 48, "X:", fnt58);
		GLCD_DrawStringC(0, 56, "Y:", fnt58);
		
		itoa(s, joyx, 10);
		z=GLCD_DrawString(20, 48, s, fnt58);
		GLCD_DrawStringC(20+z, 48, "   ", fnt58);
			
		itoa(s, params.joyxmin, 10);
		z=GLCD_DrawString(60, 48, s, fnt58);
		GLCD_DrawStringC(60+z, 48, "   ", fnt58);
			
		itoa(s, params.joyxmax, 10);
		z=GLCD_DrawString(100, 48, s, fnt58);
		GLCD_DrawStringC(100+z, 48, "   ", fnt58);
		
		itoa(s, joyy, 10);
		z=GLCD_DrawString(20, 56, s, fnt58);
		GLCD_DrawStringC(20+z, 56, "   ", fnt58);
		
		itoa(s, params.joyymin, 10);
		z=GLCD_DrawString(60, 56, s, fnt58);
		GLCD_DrawStringC(60+z, 56, "   ", fnt58);
			
		itoa(s, params.joyymax, 10);
		z=GLCD_DrawString(100, 56, s, fnt58);
		GLCD_DrawStringC(100+z, 56, "   ", fnt58);
		
		b=pollbuttons();
		if (b&STOPBUTTON)
			return 0;
			
		if (b&MENUBUTTON)
		{
			params.joycy=joyy;
			params.joycx=joyx;
			return 1;
		}
	}
		
	return 1;
}

unsigned char calibrate_battery(void)
{
	unsigned char s[6];
	unsigned char b;
	long          joyMoveState;
	unsigned char z;
	
	calib_lcd_clear();

	//                        1234567890123456789012345
	GLCD_DrawStringC(0,16,"Use joystick to set,", fnt58);
	GLCD_DrawStringC(0,24,"voltage below to match", fnt58);
	GLCD_DrawStringC(0,32,"the true supply voltage.", fnt58);
			
	joyMoveState=0;
	
	while(1)
	{		
		b=pollbuttons();
		if (b&STOPBUTTON)
			return 0;
		if (b&MENUBUTTON)
			return 1;

		DrawBatteryVoltage(40,56);
		
		itoa(s, params.batsensedivide, 10);
		z=GLCD_DrawString(100, 56, s, fnt58);
		GLCD_DrawStringC(100+z, 56, "   ", fnt58);

		if (joyy>(params.joycy-30) && joyy<(params.joycy+30))
			continue;
		
		joyMoveState+=(joyy-params.joycy)/32*(joyy-params.joycy)/32*sgn(joyy-params.joycy);
		
		while (joyMoveState>2000)
			{
			params.batsensedivide++;
			joyMoveState-=2000;
			}
		if (joyMoveState<-2000)
			{
			params.batsensedivide--;
			joyMoveState+=2000;
			}			
	}
	
	return 1;
}