/*************************************************************************
Title:    Crab robot main code
Author:   Steven Buss <sbuss@cise.ufl.edu> http://www.cise.ufl.edu/~sbuss/
Software: AVR-GCC 4.2.2, AVR Libc 1.4.6 or higher
Hardware: Atmega 128
License:  GNU General Public License 
          
DESCRIPTION:
	As a general warning, turning on compiler optimizations breaks the 
	gyro code.
    
USAGE:

                    
LICENSE:
    Copyright (C) 2008 Steven Buss

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
                        
*************************************************************************/

/* define CPU frequency in Mhz here if not defined in Makefile */
#ifndef F_CPU
#define F_CPU 14.7456E6
#endif

#include <stdlib.h>

#include <inttypes.h>
#include <stdio.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>		/* this file is obsolete, but the uart code uses it */
#include <avr/pgmspace.h> 	/* need this to write strings to memory and uart */
#include <util/delay.h>
#include <stdbool.h>

#include <string.h>
#include <ctype.h>

/* Peter Fleury's UART code (http://jump.to/fleury/) */
#include "uart.h"

/* A/D conversion code from bdmicro */
#include "adc.h"

/* Kevin Watson's gyro code (http://kevin.org/frc/) */
#include "gyro.h"

/* Drive code */
#include "drive.h"
extern uint8_t mot1H;
extern uint8_t mot2H;
extern uint8_t mot3H;
extern uint8_t mot4H;

/* 9600 baud */
#define UART_BAUD_RATE	9600

/* Counter for milliseconds */
volatile uint16_t mscount;

/* Gyroscope variables, modified in read_gyro() */
volatile uint16_t gyroData;
volatile uint8_t gyroSamples;
volatile uint16_t gyroHistory;
volatile uint32_t gyroCycles;

/* Sonar variables */
#define NUM_SONARS 3
struct sonarData{
	uint16_t lastDistance;
	uint16_t distance;
	bool ping;
	bool wait;
	bool receive;
	uint16_t count;
	uint8_t *triggerPort;
	uint8_t *receivePins;
	uint8_t pingPin;
	uint8_t receivePin;
	uint8_t track;
};
volatile struct sonarData sonars[NUM_SONARS];
volatile uint8_t sonarTurn;
volatile uint16_t timeout;
volatile uint16_t waiter;

/* Command string buffer */
char cmdBuffer[50];


void init_timer(void);		/* Initialize the timer */
void read_gyro(void);		/* Read the gyro, called from an interrupt */
void init_sonar(void);
void process_sonar(void);
void uart_putc_f(unsigned char, FILE*);	/* Handler to remap printf */
void init_uart(void);		/* Initialize the uart for TTL communication */
void init_adc(void);		/* Initialize the Analog to Digital converter */
void calibrate_gyro(void);	/* Calibrate the gyro */
void init(void);			/* Call all the initialization routines */

void ping_sonar(void);


/* 
 * millisecond counter interrupt vector 
 * monitor Timer0, will fire when it's value = OCR0
 */
ISR(TIMER0_COMP_vect)
{
	// this runs 900 times per second

	// this will sample the gyro
	TCNT0 = 0; // reset the interrupt counter

	read_gyro();
	
	if(++mscount == 150) {
		// this executes 6 times per second
		PORTB ^= 0x01;	// Make the LED blink
		mscount = 0;
	}
}


ISR(TIMER2_COMP_vect) {
	TCNT2 = 0; // reset interrupt counter

	// PORTC1 = motor1 signal = PB5
	// PORTC3 = motor2 signal = PB6
	// PORTC5 = motor3 signal = PE3
	// PORTC7 = motor4 signal = PE4
	PORTC = (PINC & 0b01010101) | ((PINB & 0b00100000) >> 4) | ((PINB & 0b01000000) >> 3) | ((PINE & 0b00001000) << 2) | ((PINE & 0b00010000) << 3);


	/* Handle sonar arbitration */
	
	if(waiter == 0) {
		timeout++;
		if(timeout < 1728) {
			if(!sonars[sonarTurn].ping && !sonars[sonarTurn].wait) {
				// Sonar is ready to be fired 
				*(sonars[sonarTurn].triggerPort) &= ~(sonars[sonarTurn].pingPin);	// force pingPin low
				*(sonars[sonarTurn].triggerPort) |= sonars[sonarTurn].pingPin;	// force pingPin high
				sonars[sonarTurn].ping = 1;
				sonars[sonarTurn].track = 1;
			} else if(sonars[sonarTurn].ping && !sonars[sonarTurn].wait) {
				// 17.36 us have elapsed, plenty long enough for the trigger pulse
				*(sonars[sonarTurn].triggerPort) &= ~(sonars[sonarTurn].pingPin);	// finished ping, force low
				sonars[sonarTurn].count = 0;
				sonars[sonarTurn].wait = 1;
				sonars[sonarTurn].track = 2;
			} else if(*(sonars[sonarTurn].receivePins) & sonars[sonarTurn].receivePin) {
				// We have a response pulse! 
				sonars[sonarTurn].count++;
				sonars[sonarTurn].receive = 1;
				sonars[sonarTurn].track = 3;
			}
		} else if(sonars[sonarTurn].receive || timeout > 1728) {
			// Finished our response pulse, save data and reset 
			sonars[sonarTurn].ping = 0;
			sonars[sonarTurn].wait = 0;
			sonars[sonarTurn].receive = 0;
			sonars[sonarTurn].lastDistance = sonars[sonarTurn].distance;
			sonars[sonarTurn].distance = sonars[sonarTurn].count;
			sonars[sonarTurn].count = 0;
			sonarTurn = (sonarTurn + 1) % NUM_SONARS;
			sonars[sonarTurn].track = 4;
			timeout = 0;
			waiter = 1;
		}
	} else if(waiter > 0 && waiter < 200) {
		waiter++;
	} else {
		waiter = 0;
	}

}

void init_timer(void) {
	/*
	 * Initialize timer0 to generate an output compare interrupt, and
	 * set the output compare register so that we get that interrupt
	 * every millisecond.
	 */
	TIFR  |= _BV(OCIE0) | _BV(OCIE2);
	TCCR0  = _BV(WGM01)|_BV(CS02)|_BV(CS00); /* CTC, prescale = 128 */
	TCNT0  = 0;
	TIMSK |= _BV(OCIE0);    /* enable output compare interrupt */
	//OCR0   = 125;          /* match in 1 ms */
	OCR0 = 128;  // ((14745600 Hz / 128) / 2^7) = 900 Hz
	
	TCCR2  = _BV(WGM01)|_BV(CS01)|_BV(CS00); /* CTC, prescale = 32 */
	TCNT2  = 0;
	TIMSK |= _BV(OCIE2);
	OCR2 = 8; // ((14745600 Hz / 32) / 8) = 57.6 KHz

	mscount = 0;
	gyroData = 0;
	gyroSamples = 0;
	gyroHistory = 0;
	gyroCycles = 0;
}

void read_gyro(void) {
	/*
	 * read the gyro.  This is called from an interrupt, don't call it in the main loop
	 */
	adc_chsel(0);
	adc_start();	/* tell the A/D converter to convert */
	adc_wait();		/* wait for the A/D to finish */
	gyroHistory += adc_read();	/* read the A/D register */
	if(++gyroSamples == 8) {
		//gyroData = gyroHistory >> 3; /* divide gyroHistory by 8 */
		//gyroData = adc_readn(0,8);
		gyroData = gyroHistory / 8;
		gyroSamples = 0;
		gyroHistory = 0;
		++gyroCycles;	/* I want to keep track of how many times gyroData has been updated */
		Process_Gyro_Data(); /* call Kevin's gyro.c */
	}
}

/*
 * This is used to make printf go to uart
 */
void uart_putc_f(unsigned char data, FILE * f) {
	/*
	 * This is used to make printf go to uart
	 */
	uart1_putc(data);
}

void init_uart(void) {
	/*
	 *  Initialize UART library, pass baudrate and AVR cpu clock
	 *  with the macro 
	 *  UART_BAUD_SELECT() (normal speed mode )
	 *  or 
	 *  UART_BAUD_SELECT_DOUBLE_SPEED() ( double speed mode)
	 */
	uart1_init( UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU) ); 
	
	fdevopen(uart_putc_f, NULL);
	
	printf("---\r"); /* This makes sure the SparkFun BlueSmirf isn't in command mode */
}

void init_sonar(void) {
	DDRA = 0b00101010;
	sonarTurn = 0;
	// right sonar
	sonars[0].lastDistance = sonars[0].distance = sonars[0].count = 0;
	sonars[0].ping = sonars[0].wait = sonars[0].receive = false;
	sonars[0].triggerPort = &PORTA;
	sonars[0].receivePins = &PINA;
	sonars[0].pingPin = 0x02;
	sonars[0].receivePin = 0x01;

	// left sonar
	sonars[1].lastDistance = sonars[1].distance = sonars[1].count = 0;
	sonars[1].ping = sonars[1].wait = sonars[1].receive = false;
	sonars[1].triggerPort = &PORTA;
	sonars[1].receivePins = &PINA;
	sonars[1].pingPin = 0x08;
	sonars[1].receivePin = 0x04;

	// front sonar
	sonars[2].lastDistance = sonars[2].distance = sonars[2].count = 0;
	sonars[2].ping = sonars[2].wait = sonars[2].receive = false;
	sonars[2].triggerPort = &PORTA;
	sonars[2].receivePins = &PINA;
	sonars[2].pingPin = 0x20;
	sonars[2].receivePin = 0x10;

	timeout = 0;
	waiter = 0;
}


void init_adc(void) {
	adc_init();
}

void calibrate_gyro() {
	Start_Gyro_Bias_Calc();
	//_delay_ms(2000);	/* We have to wait at least two seconds to get a good neutral reading */
	int i = 0;
	long j;
	while(i < 10) {
		j=0;
		//PORTB ^= 0x01;
		while(j < 100000)
			j++;
		i++;
	}
	Stop_Gyro_Bias_Calc();
}


void init(void) {
	/* Enable interrupts */
	sei();

	/* Initialize the pwms first so the robot doesn't go crazy */
	drive_init();

	/* Let everything settle out before running code */
	_delay_ms(500);

	/* Initialize the uart interface */
	init_uart();

	/* Initialize the timer for interrupts for the gyro */
	init_timer();

	/* Initialize the A/D converter */
	init_adc();

	/* Initialize the gyro (Kevin Watson's code) */
	Initialize_Gyro();
	calibrate_gyro(); /* WARNING: Removing this breaks the gyro */

	init_sonar();	
}


int main(void) {
	/* UART command buffer */
    unsigned int c;
	int bp = 0; /* buffer position */
	/* Set LED to output */
	DDRB |= 0x01;
	long i;
	int volts;

	/* Initialize everything */
	init();

	PORTB &= 0xFE; /* turn the LED off (note that an interrupt will make it blink) */

	printf("\n\r***************************************************\n\r");
	printf("*                                                 *\n\r");
	printf("*                     Crab                        *\n\r");
	printf("*                                                 *\n\r");
	printf("*                 Version 0.20                    *\n\r");
	printf("*                April 22, 2008                   *\n\r");
	printf("*                                                 *\n\r");
	printf("*             Written by Steven Buss              *\n\r");
	printf("*        http://www.cise.ufl.edu/~sbuss/          *\n\r");
	printf("*                                                 *\n\r");
	printf("*      ADXRS300 Gyro library by Kevin Watson      *\n\r");
	printf("*     Interrupt UART library by Peter Fleury      *\n\r");
	printf("*                                                 *\n\r");
	printf("*   Licensed under the GPL, see website for code  *\n\r");
	printf("*                                                 *\n\r");
	printf("***************************************************\n\n\r");

	printf("About to test gyro, please don't move it.\n\r");
	
	calibrate_gyro();
	
	printf("Read information from gyro: num cycles: %6lu, bias: %5u, angle: %6ld\n\r", gyroCycles, Get_Gyro_Bias(), Get_Gyro_Angle());

	printf("Finished gyro test.\n\r");

	i = 0;

	//process_sonar();

	printf("\n\rgyro, cycles, angle, volts*10, sonar0\n\r");
	while(1) {
		c = uart1_getc();
		if(c == 13) {
			cmdBuffer[bp] = '\0';
			bp = 0;
			if(!strcmp(cmdBuffer, "cal gyro")) {
				printf("\n\rCalibrating Gyro, don't move it.\n\r");
				calibrate_gyro();
				printf("Read information from gyro: bias: %5u, angle: %6ld\n\r", Get_Gyro_Bias(), Get_Gyro_Angle());
			} else if(!strcmp(cmdBuffer, "reset gyro")) {
				printf("\n\rResetting and recalibrating Gyro, please don't move it.\n\r");
				Reset_Gyro_Angle();
				calibrate_gyro();
				printf("Read information from gyro: bias: %5u, angle: %6ld\n\r", Get_Gyro_Bias(), Get_Gyro_Angle());
			} else if(!strcmp(cmdBuffer, "pause")) {
				printf("\n\rPaused execution, hit enter to continue\n\r");
				while(uart1_getc() != 13) ;
			} else if(!strcmp(cmdBuffer, "pwm")) {
				printf("\n\rEnter pwm1, then hit enter: ");
				
				c = uart1_getc();
				while(c != 13) {
					c = uart1_getc();
					if(!(c & UART_NO_DATA ) ) {
						cmdBuffer[bp++] = c;
						uart1_putc(c);
					}
				}
				cmdBuffer[bp] = '\0';
				OCR1A = atoi(cmdBuffer);
				bp = 0;

				printf("\n\rEnter pwm2, then hit enter: ");
				c = uart1_getc();
				while(c != 13) {
					c = uart1_getc();
					if(!(c & UART_NO_DATA ) ) {
						cmdBuffer[bp++] = c;
						uart1_putc(c);
					}
				}
				cmdBuffer[bp] = '\0';
				OCR1B = atoi(cmdBuffer);
				bp = 0;

				printf("\n\rEnter pwm3, then hit enter: ");				
				c = uart1_getc();
				while(c != 13) {
					c = uart1_getc();
					if(!(c & UART_NO_DATA ) ) {
						cmdBuffer[bp++] = c;
						uart1_putc(c);
					}
				}
				cmdBuffer[bp] = '\0';
				OCR3A = atoi(cmdBuffer);
				bp = 0;

				printf("\n\rEnter pwm4, then hit enter: ");				
				c = uart1_getc();
				while(c != 13) {
					c = uart1_getc();
					if(!(c & UART_NO_DATA ) ) {
						cmdBuffer[bp++] = c;
						uart1_putc(c);
					}
				}
				cmdBuffer[bp] = '\0';
				OCR3B = atoi(cmdBuffer);
				bp = 0;
			} else if(!strcmp(cmdBuffer, "drive")) {
				printf("\n\n\rInteractive drive mode!\n\r");
				printf("Use wasd for direction, hit enter to exit.\n\r");
				
				do {
					c = uart1_getc();
					//if(!(c & UART_NO_DATA ) ) {
						if(c == 119) {	// w
							drive(255, 255, 0, 0, 0);
						} else if(c == 97) { // a
							drive(0, 255, 0, 255, 0);
						} else if(c == 100) { // d
							drive(255, 0, 255, 0, 0);
						} else if(c == 115) { // s
							drive(0, 0, 255, 255, 0);
						} else if(c == 48) { // 0
							mot1H = 128;
							mot2H = 128;
							mot3H = 128;
							mot4H = 128;
						} else {
							drive(mot1H, mot2H, mot3H, mot4H, 0);
						}
					//}
				} while(c != 13); // for a do...while loop
			} else if(!strcmp(cmdBuffer, "vision")) {
				// This mode is what's running while MATLAB is giving directions
				long turnAngle = 0;
				uint8_t cmd1, cmd2, cmd3, cmd4;
				uint16_t cmdTimeout = 0;
				cmd1 = cmd2 = cmd3 = cmd4 = mot1H = mot2H = mot3H = mot4H = 128;
				do {
					c = uart1_getc();
					if(c == 'r') {
						turnAngle = 100+Get_Gyro_Angle();
						if(sonars[1].distance > 30) {
							cmd1 = cmd3 = 78;
							cmd2 = cmd4 = 178;
						}
						cmdTimeout = 0;
					} else if(c == 'l') {
						turnAngle = Get_Gyro_Angle()-100;
						if(sonars[0].distance > 30) {
							cmd1 = cmd3 = 178;
							cmd2 = cmd4 = 78;
						}
						cmdTimeout = 0;
					} else if(c == 's') {
						cmd1 = cmd2 = cmd3 = cmd4 = 128;
						turnAngle = Get_Gyro_Angle();
						cmdTimeout = 0;
					} else if(c == 'f') {
						if(sonars[2].distance > 30) {
							cmd1 = cmd2 = 255;
							cmd3 = cmd4 = 0;
						}
						cmdTimeout = 0;
					}
					if(cmdTimeout++ > 2000) {
						cmd1 = cmd2 = cmd3 = cmd4 = 128;
						turnAngle = Get_Gyro_Angle();
						cmdTimeout = 0;
					}
					if(sonars[1].distance < 30) {
						cmd1 = cmdMux(168,cmd1);
						cmd2 = cmdMux(88,cmd2);
						cmd3 = cmdMux(168,cmd3);
						cmd4 = cmdMux(88,cmd4);
					}
					if(sonars[0].distance < 30) {
						cmd1 = cmdMux(88,cmd1);
						cmd2 = cmdMux(168,cmd2);
						cmd3 = cmdMux(88,cmd3);
						cmd4 = cmdMux(168,cmd4);
					}
					if(sonars[2].distance < 30) {
						cmd1 = cmdMux(88,cmd1);
						cmd2 = cmdMux(88,cmd2);
						cmd3 = cmdMux(168,cmd3);
						cmd4 = cmdMux(168,cmd4);
					}
					drive(cmd1, cmd2, cmd3, cmd4, turnAngle);
						
				} while(c != 13); // for the do...while loop
			} else if(!strcmp(cmdBuffer, "sonar")) {
				printf("dist1 delt1 dist2 delt2 dist3 delt3 turn timeout waiter\n\r");
				do {
					c = uart1_getc();
					printf("%5d %5d %5d %5d %5d %5d   %1d   %4d   %4d \r", sonars[0].distance, sonars[0].distance - sonars[0].lastDistance, sonars[1].distance, sonars[1].distance - sonars[1].lastDistance, sonars[2].distance, sonars[2].distance - sonars[2].lastDistance, sonarTurn, timeout, waiter);
				} while (c != 13);
			} else if(!strcmp(cmdBuffer, "sdrive")) {
				// back up from objects
				bool close = false;
				do {
					close = false;
					c = uart1_getc();
					if(sonars[2].distance < 40) { // sonars[2] is the front sonar
						drive(0, 0, 255, 255, 0);
						close = true;
					}
					if(sonars[0].distance < 40) { // sonars[0] is the right sonar
						drive(0, 255, 0, 255, 0);
						close = true;
					}
					if(sonars[1].distance < 40) { // sonars[1] is the left sonar
						drive(255, 0, 255, 0, 0);
						close = true;
					}
					if(close) {
						drive(mot1H, mot2H, mot3H, mot4H, 0);
					} else {
						mot1H = mot2H = mot3H = mot4H = 128;
					}
				} while(c != 13);
			}
					

			printf("\n\n\rgyro, cycles, angle, volts*10, sonar0\n\r");
		} else {
	        if ( !(c & UART_NO_DATA ) ) { /* Got data from UART */
				cmdBuffer[bp++] = c; /* Add character to buffer */
				//uart1_putc((unsigned char)c);
			}

			volts = (int)((((double)gyroData) * 5.0 / 1024.0) * 10); /* convert to Volts */

			printf("%4u %7lu %6ld %5d\r", gyroData, gyroCycles, Get_Gyro_Angle(), volts);
		}
	}
	return 1;
}