#include "kitt.h"

int main(void)
{
	init();
	
	/* Self-test of AD converter */
	if (255 != *ad)
	{
		unsigned int i = 0;
		set_diode(0xFF);
		write_to_motor_latch();
		while (i < 50000) i++;
		set_diode(0x00);
		write_to_motor_latch();
	}
	
	/* Go to replay mode if no tape */
	if (0x0F == *diodes)
	{
		replay_mode = 1;
		set_diode(0xFF);
	}
	
	_avben();
	
	while (replay_mode) { }

	while (1)
	{
		signed int dxdt;
		/* Perform page write with interrupts disabled */
		if (page_full)
		{
			set_diode(0xFF);
			write_to_motor_latch();
			_avbdi();
			write_to_eeprom();
			_avben();
			set_diode(0x00);
			write_to_motor_latch();
		}
		/* Find P and D components and feed to regulator */
		last_offset = read_offset();
		dxdt = calc_derivative();
		regulate_PD(last_offset, dxdt);
	}
}

/*
 *	Initializes all global variables and data structures. 
 */
void init(void)
{
	unsigned int i;
	
	motors = (unsigned short*)ADDR_MOTORS;
	diodes = (unsigned short*)ADDR_DIODES;
	ad = (unsigned short*)ADDR_AD;
	eeprom = (unsigned short*)ADDR_EEPROM;

	for (i = 0; i < NBR_PULSES; i++)
	{
		pulses[i] = FREE_RUNNING;
	}
	
	for (i = 0; i < NBR_STORED_OFFSETS; i++)
	{
		listnode node;
		listnode *tmp = &node;
		tmp->val = 0;
		tmp->next = &history[i + 1];
		tmp->prev = &history[i - 1];
		history[i] = *tmp;
	}
	history[NBR_STORED_OFFSETS - 1].next = &history[0];
	history[0].prev = &history[NBR_STORED_OFFSETS - 1];
	history_head = &history[0];
	
	last_offset = 0;
	latch_val = 0;
	pulse_counter = 0;
	power_counter = 0;
	servo_counter = 0;
	servo_pulser = 3;
	power_left = 0;
	power_right = 0;
	motor_pause = 0;
	pause_counter = 0;

	for (i = 0; i < PAGE_SIZE; i++)
	{
		page_entries[i] = 0;
	}
	curr_column = 0;
	curr_page = 0;
	page_full = 0;
	replay_mode = 0;
	last_side = 0;
}

/*
 *	The periodic interrupt routine. 
 */
exp2()
{
#ifdef SENSE
	/* Read value of AD converter */
	unsigned short ad_val = 0;
	if (10 == pulse_counter)
	{
		ad_val = *ad;
	}
	/* Set PCM sequence for servo */
	pulse_servo();
#endif
	if (!motor_pause)
	{
		if (replay_mode)
		{
			step_playback();
		}
		else
		{
			step_normal();
		}
		pulse_counter++;
	}
#ifdef SENSE
	else
	{
		pause_counter++;
		if (2000 == pause_counter)
		{
			motor_pause = 0;
			pause_counter = 0;
		}
		else if (ad_val >= 130)
		{
			pause_counter = 0;
		}
	}
	if (ad_val >= 130)
	{
		motor_pause = 1;
		set_motors(FREE_RUNNING);
	}
#endif
	write_to_motor_latch(); /* flush contents of latch_val */
}

/*
 *	This function controls the PCM pulse to the servo. Should be
 *	called once during each interrupt. 
 */
void pulse_servo(void)
{
	if (23 == servo_pulser)
	{
		set_servo(0xFF);
		if (50 == servo_counter)
		{
			servo_counter = 0;
		}
		servo_counter++;
		servo_pulser = 3;
	}
	else if (22 == servo_pulser && 25 < servo_counter)
	{
		set_servo(0xFF);
	}
	else
	{
		set_servo(0x00);
	}
	servo_pulser++;
}

/*
 *	Sets the motors for this cycle and steps to next 
 *	address in memory. 
 */
void step_playback(void)
{
	set_motors(*(eeprom + curr_page + curr_column));
	curr_column++;
	if (128 == curr_column)
	{
		curr_page += 128;
		curr_column = 0;
	}
	if (10 == pulse_counter)
	{
		pulse_counter = 0;
	}
}

/*
 *	Sets the motors for this cycle and stores the offset 
 *	in the history list. 
 */
void step_normal(void)
{
	if (10 == pulse_counter)
	{
		pulse_counter = 0;
		set_pulse_array();
		push_to_history(last_offset); 
	}
	set_motors(pulses[pulse_counter]);
}

/*
 *	Saves an offset into the circular history list. 
 */
void push_to_history(signed short offset)
{
	history_head = history_head->next;
	history_head->val = offset;
}

/* 
 *	Returns an approximation of the derivative: 
 *	1) Check for the latest change in offset. 
 *	2) Compute derivative so that larger offset gives greater 
 *	   derivative and longer time decreases the value. 
 */
signed int calc_derivative(void)
{
	listnode *curr = history_head;
	signed short t0_val = history_head->val;
	unsigned short t = 1;
	do
	{
		if (t0_val - curr->val)
		{
			break;
		}
		curr = curr->prev;
		t++;
	} while (curr != history_head);
	if (t != NBR_STORED_OFFSETS)
	{
		return (t0_val - curr->val) * (NBR_STORED_OFFSETS - t);
	}
	else
	{
		return 0;
	}
}

/*
 *	Sets the target power for both motors using a proportional 
 *	part and a derivative part. 
 */
void regulate_PD(signed short P, signed int D)
{
	signed short D_effect;
	signed short left = 5; /* initial power */
	signed short right = 5; 
	
	if (-127 == P) /* lost tape, continue in same direction */
	{
		if(0 < last_side) 
		{
			power_left = 10;
			power_right = 1;
		}
		else if(0 > last_side)
		{
			power_left = 1;
			power_right = 10;
		}
		return;
	} 
	else if (126 == P) /* in the air, stop motors */
	{
		power_left = 0;
		power_right = 0;
		return;
	}
	
	/* Do the P */
	left += P;
	right -= P;
	
	/* Do the D */
	if (D > 135) D = 135; /* max(D) = 90 */
	else if (D < -135) D = -135; /* min(D) = -90 */
	D_effect = D / 34; /* D_effect [-3,3] */
	
	left += D_effect;
	right -= D_effect;
	
	if (left > 10) left = 10;
	else if (left < 0) left = 0;
	if (right > 10) right = 10;
	else if (right < 0) right = 0;
	
	power_left = (unsigned short)left;
	power_right = (unsigned short)right;
}

/*
 *	Sets up the pulses vector. Decides the mode of each motor 
 *	for the NBR_PULSES upcoming interrupts. 
 */
void set_pulse_array(void)
{
	int i;
	for (i = 0; i < NBR_PULSES; i++)
	{
		pulses[i] = FREE_RUNNING;
	}
#ifndef DEBUG
	for (i = 0; i < power_left; i++)
	{
		pulses[i] |= LEFT;
	}
	for (i = 0; i < power_right; i++)
	{
		pulses[i] |= RIGHT;
	}
#endif
}

/*
 *	Reads the value of the sensor diodes and returns a decimal 
 *  representation. 
 */
signed short read_offset(void)
{
	unsigned short int pattern = *diodes;
	pattern ^= 0x0F; /* this makes active diode binary 1 */

	switch (pattern)
	{
		case 0x00:	return -127;					/* lost tape, continue in the same direction */
		case 0x08:	last_side = 1; return 5;		/* */
		case 0x0C:	last_side = 1; return 3;		/* Left */
		case 0x04:	last_side = 1; return 1;		/* */
		case 0x02:	last_side = -1; return -1;		/* */
		case 0x03:	last_side = -1; return -3;		/* Right */
		case 0x01:	last_side = -1; return -5;		/* */
		case 0x0F:	return 126;						/* in the air (probably) */
		default:	return 0;
	}
}

/* Append first 6 bits of pattern to motor/diode latch */
void set_motors(unsigned short pattern)
{
	if (!motor_pause && !replay_mode)
	{
		store_value(pattern);
	}
	latch_val = (latch_val & 0xC0) | (pattern & 0x3F);
}

/* Append bit 7 of pattern to motor/diode latch */
void set_diode(unsigned short pattern)
{
	latch_val = (latch_val & 0xBF) | (pattern & 0x40);
}

/* Append bit 8 of pattern to motor/diode latch */
void set_servo(unsigned short pattern)
{
	latch_val = (latch_val & 0x7F) | (pattern & 0x80);
}

/* Write latch_val to motor/diode latch */
void write_to_motor_latch(void)
{
	*motors = latch_val;
}

/*
 *	Queue a value for writeback to eeprom. The value will not be 
 *	written until the page is full. 
 */
void store_value(unsigned short value)
{
	page_entries[curr_column] = value;
	curr_column++;
	if (curr_column == 128)
	{
		page_full = 1;
		curr_column = 0;
	}
}

/*
 *	Write the active page to eeprom. 
 */
void write_to_eeprom(void)
{
	int i;
	/* disable protection */
	*(eeprom + 0x5555) = 0xAA;
	*(eeprom + 0x2AAA) = 0x55;
	*(eeprom + 0x5555) = 0xA0;
	/* write page */
	for (i = 0; i < PAGE_SIZE; i++)
	{
		*(eeprom + curr_page + i) = page_entries[i];
	}
	curr_page += 128;
	page_full = 0;
}

/*
 *	The following functions can be used if DEBUG mode is active. 
 *	This is set in the header file. 
 */
#ifdef DEBUG

/*
 *	Returns the sum of the nodes in the history list. 
 *	CAUTION: if NBR_STORED_OFFSETS is large, the sum may not 
 *	fit in 8 bits. 
 */
unsigned short sum_history(void)
{
	listnode *tmp;
	unsigned short sum, i;
	sum = 0;
	tmp = history_head;
	for (i = 0; i < NBR_STORED_OFFSETS; i++)
	{
		sum += tmp->val;
		tmp = tmp->prev;
	}
	return sum;
}

/*
 *	Function for printing a signed short to the it68 
 *	development environment. 
 */
void print_short(signed short val)
{
	char outbuf[6];
	int i = 0;
	int j = 0;
	char tmp;
	if (val < 0)
	{
		outbuf[i] = '-';
		i++;
		j++;
		val = -val;
	}
	do /* find the chars */
	{
		outbuf[i] = (char)(48+ val%10);
		i++;
	} while ((val /= 10) > 0);
	outbuf[i] = '\n';
	outbuf[i + 1] = '\0';
	i--;
	do /* swap the chars */
	{
		tmp = outbuf[j];
		outbuf[j] = outbuf[i];
		outbuf[i] = tmp;
		j++;
		i--;
	} while (i - j > 1);
	_print_str(outbuf);
}
#endif