#include <Pilot.h>
#include <string.h>
#include "fiatSymbols.h"
#include "fiatOperations.h"
#include "fiatControl.h"
#include "picControl.h"

void resetPic()
{
	/* store 0 in program counter -- sends PIC back to 0, start vector.		*/

#if (SYM_ih_psp_end > 0x0ff)
#error resetPic() will fail
	/* (only works if parallel slave port handler fits in page 0; othewise	*/
	/* we jump to the start of whatever page the PSP handler is on. Oops.	*/
#endif

	pic_write(0x02, 0x00);
}

void steerAngle(int steer)
{
	/* steer = 0 => drive wheel parallel to free wheels */
	/* steer = 1 => drive wheel perpendicular to free wheels */
	
	int from, to, dir, cur;
	from	= pic_read(SYM_fServoDuty);
	to		= steer ? servo_turn : servo_straight;
	dir		= (to > from ? servo_clicks : -servo_clicks);
	for (cur = from; (from<to ? cur<to : cur>to); cur+=dir) {
		pic_write(SYM_fServoDuty, cur);
		{
			char buf[150];
			StrPrintF(buf, "state %x\n", cur);
			statusMessage(buf);
		}
		delayMilliseconds(servo_delayms);
	}
	pic_write(SYM_fServoDuty, to);
	{
		char buf[150];
		StrPrintF(buf, "state %x\n", to);
		statusMessage(buf);
	}
}

void paperPickup()
{
	tape(1);
	cam(0);
}

void paperDrop()
{
	cam(1);
	turnAngle(360-45);	/* wiggle to drop paper */
	turnAngle(45);
}

int camSensor()
{
	return (pic_read(SYM_PORTC) & 0x010)!=0;
}

void cam(int out)
{
	ULong deadline;

	/* set a ten-second deadline */
	deadline = TimGetTicks()+((long)10000*sysTicksPerSecond/1000);

	/* out == 1 => cam out, tape can hold paper */
	/* out == 0 => cam in, tape not holding paper */
	setMotorDir(CAM_MOTOR, out?1:-1);
	setMotorPower(CAM_MOTOR, 100);
	while (camSensor()==1) {
		/* wait until cam is in motion phase */
		if (TimGetTicks()>deadline) {
			statusMessage("deadline expired\n");
			break;
		}
	}
	while (camSensor()==0) {
		/* wait until cam is in stop phase */
		if (TimGetTicks()>deadline) {
			statusMessage("deadline expired\n");
			break;
		}
	}

	/* slow down, turn around, and try to hit the mark more accurately */
	setMotorDir(CAM_MOTOR, out?-1:1);
	setMotorPower(CAM_MOTOR, 60);
	while (camSensor()==1) {
		/* wait until encoder disappears again */
		if (TimGetTicks()>deadline) {
			statusMessage("deadline expired\n");
			break;
		}
	}
	setMotorDir(CAM_MOTOR, 0);	// slam on brakes
	setMotorPower(CAM_MOTOR, 0);
}

#define sign_extend(val) ((val&0x08000) ? (val|0xffff8000) : (val&0x07fff))

void turnAngle(double distance)				/* distance in degrees CW */
{
	int intPart;
	distance = (distance+1440.0);
	intPart = ((double)((int)(distance/360.0)))*360.0;
	distance-=intPart;
	/* distance is now between 0 and 359.9... */
	if (distance>180.0) {
		turnDirAngle(-1, 360.0-distance);
	} else {
		turnDirAngle(1, distance);
	}
}

void turnDirAngle(int dir, double distance)	/* distance in degrees */
{
	/* rotation radius (distance from center of steering wheel to center
	 * of rotation, midway between free wheels) measured at 16.9cm, giving
	 * a circumference of 106cm/rotation.
	 * experimental trial puts it at 101.9.
	 * div by 360.0 turns degrees into rotations.
	 */
	steerAngle(1);
	drive(-dir, distance*(102.5/360.0));
}

void driveForward(int dir, double distance)	/* distance is in cm */
{
	steerAngle(0);
	drive(-dir, distance);
}

int polarizedShaftEncoder(int dir)
{
	int counts = pic_read2(SYM_fSECount);
	if (dir==1) {
		/* make counts always count upward when wheel is turning in
		 * direction 'dir' -- that means that we can always use
		 * the same sense in the test of the while loops in drive().
		 */
		counts = ~counts;
	}
	return (counts&0x0ffff);
}

void drive(int dir, double distance)	/* distance is in cm */
{
	int counts;
	int count_target;
	int disti;
	int dist_elapsed;
	ULong deadline;
	double max_speed=1.2;
	double slow_speed=0.2;
	double speed_desired;
	int slow_range=500;		/* start slowing at slow_range, arrive at
							 * slow_speed when just about at 0 clicks to go */

	/* set a ten-second deadline */
	deadline = TimGetTicks()+((long)10000*sysTicksPerSecond/1000);

	counts = polarizedShaftEncoder(dir);
	disti = (int) (distance*(480.0/12.75));	/* 12.75cm/480 clicks */
	/* TODO: if I ever recalibrate cm/clicks, adjust angle tuning inversely */
	
	/* distance currently measured in revs */
	if (disti > 0x06000) {
		char buf[150];
		StrPrintF(buf, "Too far away: %x counts\n", count_target);
		statusMessage(buf);
		return;
	}
	count_target = counts+disti;

	setMotorDir(DRIVE_MOTOR, dir);
	dist_elapsed = count_target - polarizedShaftEncoder(dir);
	dist_elapsed = sign_extend(dist_elapsed);
	while (dist_elapsed>0) {
		if (dist_elapsed<slow_range) {
			/* slow down linearly as we approach target */
			speed_desired =
				(max_speed-slow_speed)
				*((double)dist_elapsed)/((double)slow_range)
					+slow_speed;
		} else {
			speed_desired = 1.2;
		}
		adjust_speed(speed_desired);
/*		{
			char buf[150];
			StrPrintF(buf, "In while loop: %d/%d %d\n",
				disti, dist_elapsed, (int)(speed_desired*100));
			statusMessage(buf);
		}
*/
		if (TimGetTicks()>deadline) {
			statusMessage("deadline expired\n");
			break;
		}
		dist_elapsed = count_target - polarizedShaftEncoder(dir);
		dist_elapsed = sign_extend(dist_elapsed);
	}
	setMotorPower(DRIVE_MOTOR, 0);	/* turn off motor */
	setMotorDir(DRIVE_MOTOR, 0);	/* put on the (electric) brakes */
	delayMilliseconds(250);			/* let drive wheel settle */
	{
		char buf[150];
		dist_elapsed = count_target - polarizedShaftEncoder(dir);
		dist_elapsed = sign_extend(dist_elapsed);
		StrPrintF(buf, "Overshot by %d clicks\n", dist_elapsed);
		statusMessage(buf);
	}
}

void tape(int wind)
{
	/* wind == 1 ->wind tape */
	/* wind == -1 ->unwind tape */
	setMotorDir(TAPE_MOTOR, -wind);	/* motor polarity reversed
									 * from wind sense */
	setMotorPower(TAPE_MOTOR, 100);
	delayMilliseconds(500);		/* TODO: sense tape reel motion */
	setMotorPower(TAPE_MOTOR, 0);
}

void setMotorDir(int whichMotor, int state)
/* states are -1 (backward), 0 (braking), 1 (forward) */
{
	static int bits_ary[3] = { 0x02, 0x00, 0x01 };
	int bits = bits_ary[state+1];
	int mask = 0x03;
	int oldVal;

	bits = bits << (whichMotor*2);
	mask = mask << (whichMotor*2);

	oldVal = pic_read(SYM_PORTB);
	oldVal = (oldVal & (~mask)) | bits;
	pic_write(SYM_PORTB, oldVal);
}

void setMotorPower(int motor, int power)
{
	if (motor == DRIVE_MOTOR) {
		pic_write(SYM_CCPR1L, (int) power*2.55);
	} else if (motor == CAM_MOTOR) {
		pic_write(SYM_CCPR2L, (int) power*2.55);
	} else if (motor == TAPE_MOTOR) {
		if (power == 100) {
			pic_write(SYM_PORTC, pic_read(SYM_PORTC)|0x01);
		} else {
			pic_write(SYM_PORTC, pic_read(SYM_PORTC)&0xfe);
		}
	}
}

void delayMilliseconds(int millis)
{
	ULong ticks = TimGetTicks()+((long)millis*sysTicksPerSecond/1000);
	while (TimGetTicks()<ticks)	/* spin */
		;
}

double get_speed()
{
	/* returns speed (unsigned) in revs/sec */
	/* period is in time-units/click.
	 * there are 256 time-units in a Timer2 interrupt.
	 * there are 1000 Timer2 interrupts in a second
	 * there are 480 clicks in a wheel revolution
	 */
	int period;
	double speed;
	period = pic_read2(SYM_fSEPeriod);
	if (period == 0) {
		speed = 100.0;	/* real fast */
	} else {
		speed = 256000.0/(((double)period)*480.0);
	}
	return speed;
}

void show_speed()
{
	char buf[150];
	int speedInt, speedFrac;
	double speed;
	speed = get_speed();
	speedInt = (int) speed;
	speedFrac = ((int) (speed*100.0))-(speedInt*100);

	StrPrintF(buf, "speed=%d.%02d\n",
		speedInt, speedFrac);
	statusMessage(buf);
}

void adjust_speed(double desired_speed)
{
	static int previous_power=0;
	static int previous_count=0;
	double current_speed;
	int current_count;
	int power;

	current_count = pic_read2(SYM_fSECount);
	if (current_count==previous_count) {
		/* haven't moved since last check -- no valid speed value.
		 * don't do anything this time. Set speed to something that
		 * makes wheel move.
		 */
		setMotorPower(DRIVE_MOTOR, 100);
		return;
	} else {
		current_speed = get_speed();
	}

	if (current_speed > desired_speed) {
		/* going too fast -- cut back power */
		power = (previous_power * 95) / 100;
	} else {
		/* going too slow -- increase power */
		power = (previous_power * 115) / 100;
	}
	if (power<20) power = 20;	/* min power */
	if (power>100) power = 100;	/* min power */
	setMotorPower(DRIVE_MOTOR, power);
	previous_power = power;
	previous_count = current_count;

/*
	{
		char buf[150];
		StrPrintF(buf, "power=%03d speed=%03d\n", power, ((int)(current_speed*100)));
		statusMessage(buf);
	}
*/
}