#include "pad.h"
#include "main.h"
#include "ed_host_int.h"
#include "ed_slave_int.h"
#include "ed_pad_int.h"
#include "menu.h"
#include "config.h"
#include "util.h"
#include "slave.h"

#include <math.h>
#include <string.h>

unsigned char diag_about(void);
unsigned char diag_baud(void);
unsigned char diag_config(void);
unsigned char diag_drive(void);
unsigned char diag_configdrivemode(void);

void setmotor(unsigned char port, int dirpwm);

//const char *mainmenu[]={"Analog","Digital","DAC","Motors","QuadPhase","Servos",
//						"Drive Mode","Config", "About", "Quit", ""};
const char *mainmenu[]={"Drive Mode","Config", "About", "Quit", ""};

void diag(void)
{
	signed char res;

	res=0;
	
	while(res!=-1 && res!=3) // while not exit or Quit
		{
		res=menu("Diagnostics", mainmenu, res);

		if (res==0)
			diag_drive();
		if (res==1)
			diag_config();
		if (res==2)
			diag_about();
		
		}
	pad_LcdClear();
}

unsigned char diag_about(void)
{
	pad_LcdClear();
	
	pad_LcdPrintC(0x00, 0x10, 'b', "OrcBoard/Pad (v4)");
	pad_LcdPrintC(0x00, 0x20, 'B', "Developed by Edwin Olson");
	pad_LcdPrintC(0x00, 0x28, 'B', "and MIT MASLab.");
	pad_LcdPrintC(0x00, 0x38, 'B', "maslab.csail.mit.edu");

	pad_gets();
	return 0;
}

const char *configmenu[]={"Baud","DriveMode","Quit", ""};

unsigned char diag_config(void)
{
	signed char res;

	res=0;
	
	while (1)
	{
		res=menu("Config", configmenu, res);
	
		if (res==2 || res<0)
			return 0;
		
		switch(res)
		{
			case 0:
				diag_baud();
				break;
			case 1:
				diag_configdrivemode();
				break;
		}
	}
	return 0;
}

const char *baudmenu[]={"115.2","230.4", "250", "500","Quit", ""};

unsigned char diag_baud(void)
{
	signed char res;

	res=params.baud;
	
	res=menu("Baud", baudmenu, res);

	if (res>3)
		return 0;
		
	params.baud=res;
	savePrefs();
	loadPrefs();
	
	return 0;
}

const char *configdrivedmenu[]={"LR","RL","lR","Rl","Lr","rL","lr","rl","Quit",""};

unsigned char diag_configdrivemode(void)
{
	signed char res;

	res=params.drivemode;
	
	res=menu("Drive Mode", configdrivedmenu, res);
	if (res>=0 && res<=7)
		{
		params.drivemode=res;
		savePrefs();
		loadPrefs();
		}
	return 0;
}

#define PI  (3.1415926535f)
#define HALFPI (PI/2)
#define QUARTERPI (PI/4)
#define RECIP_QUARTERPI (4/PI)

unsigned char diag_drive(void)
{
	unsigned char b;
	unsigned char cmd[5];
	float theta, r;
	float lscale, rscale;
	float temp;
	
	pad_LcdClear();
			
	
	pad_waitButtonRelease();
	
	while (1)
		{
		pad_LcdPrintC(0x00,0x08, 'D', "Drive Mode");

		b=pad_pollButtons();
		slave_requeststate();

		if (b&(PAD_MENU | PAD_STOP))
			{
			pad_waitButtonRelease();
			return 0;
			}

		pad_LcdPrintC(0x08, 0x18, 'B',"Joy");
		
		formatInt(pad_joyx, cmd, 4);
		pad_LcdPrint(0x20,0x18, 'B', cmd);
		
		formatInt(pad_joyy, cmd, 4);
		pad_LcdPrint(0x50,0x18, 'B', cmd);
		
		pad_LcdPrintC(0x08, 0x28, 'B',"PWM");
		
		formatInt(getmotoractualsignedpwm(0), cmd, 4);
		pad_LcdPrint(0x20,0x28, 'B', cmd);
		
		formatInt(getmotoractualsignedpwm(2), cmd, 4);
		pad_LcdPrint(0x50,0x28, 'B', cmd);

		pad_LcdPrintC(0x08,0x38,'B',"mA");

		formatInt(getmotorcurrent(0),cmd ,5);	
		pad_LcdPrint(0x18,0x38, 'B', cmd);

		formatInt(getmotorcurrent(2),cmd ,5);	
		pad_LcdPrint(0x48,0x38, 'B', cmd);

		theta=atan2(128-pad_joyy, pad_joyx-128);
		
		// note: multiplier of 2.2 gives a little "gain" over the nominal 2.0
		// in order to make it a bit easier to go full blast.
		r=2.2f*sqrt(pow(pad_joyy-128,2) + pow(pad_joyx-128,2));
		if (r>255.0f)	// saturate	
			r=255.0f;
		if (r<20.0f)	// create a "dead" zone in the middle
			r=0.0f;
		
		// give other clients a chance. floating point is slow.
		for (b=0;b<20;b++)
			idle();
	
		//theta is [-PI, PI] (spec of atan2)
		if (theta>=0 && theta<=HALFPI)
		{
			rscale=-1.0f+theta*RECIP_QUARTERPI;
			lscale=1.0f;
		}
		else if (theta>HALFPI)
		{
			rscale=1.0f;
			lscale=1.0f-(theta-HALFPI)*RECIP_QUARTERPI;
		}
		else if (theta>=-HALFPI)
		{
			rscale=-1.0f;
			lscale=-1.0f+(theta+HALFPI)*RECIP_QUARTERPI;
		}
		else
		{
			rscale=1.0f-(theta+PI)*RECIP_QUARTERPI;
			lscale=-1.0f;
		}
		
		// give other clients a chance. floating point is slow.
		for (b=0;b<20;b++)
			idle();
		
		if (params.drivemode&DRIVEMODE_LEFTINV)
			lscale=-lscale;
		if (params.drivemode&DRIVEMODE_RIGHTINV)
			rscale=-rscale;
		if (params.drivemode&DRIVEMODE_SWAPLR)
		{
			temp=rscale;
			rscale=lscale;
			lscale=temp;
		}
		
		setmotor(0, (int) (lscale*r));
		setmotor(2, (int) (rscale*r));
		
		//setmotor(0, 5*(pad_joyy+pad_joyx-256)/2);
		//setmotor(2, -5*(pad_joyy-pad_joyx)/2);

		for (b=0;b<50;b++)
			idle();
		}
	return 0;
}
