/* Name: main.c
 * Project: PowerSwitch based on AVR USB driver
 * Author: Christian Starkjohann
 * Creation Date: 2005-01-16
 * Tabsize: 4
 * Copyright: (c) 2005 by OBJECTIVE DEVELOPMENT Software GmbH
 * License: GNU GPL v2 (see License.txt) or proprietary (CommercialLicense.txt)
 * This Revision: $Id: main.c 370 2007-06-25 15:13:48Z cs $
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/wdt.h>
#include <math.h>
#define F_CPU 12000000UL
#include <avr/delay.h>
#include "eeprom.h"
#include "usart.h"
#include "usbdrv.h"
#include "oddebug.h"
//#ifndef F_CPU
//#define F_CPU 12000000UL
//#endif
static volatile int SET_VELOCITY = 0 ;
static volatile char tBuffer[5];
static volatile char setVel = 0 ;
static volatile char getChar = 0 ;	

enum Commands
{
        SERIAL_GET_ENCODER = 0,
        SERIAL_SET_PWM,
        SERIAL_GET_ODOMETRY,
        SERIAL_ENB_SERIAL_TIMEOUTS,
        SERIAL_DIS_SERIAL_TIMEOUTS,
        SERIAL_SET_ROBOT_VEL_PC,
        SERIAL_SET_ROBOT_VEL,
        SERIAL_RESET_ODOMETRY,
        SERIAL_SET_TRAP_POS,
        SERIAL_SET_PROP_VEL_MODE,
	SERIAL_TEST,
	USB_TEST,////
	USB_CHECKSTATE,
	USB_ROTATE,
	USB_TRANSLATE,////
	IDLE  
};

unsigned char currentState = IDLE ;  	


int serialBufferLen = 0  ;
unsigned char serialBuffer[4]; 

int odometryBufferLen = 0 ; 
unsigned char odometryBuffer[12];

int encoderBufferLen  = 0 ;   
unsigned char encoderBuffer[4] ;


unsigned char VelBuffer[4];
short rfinal = 0 ;

unsigned char lsb_fbVel = 0 , msb_fbVel = 0 , lsb_rlVel = 0 , msb_rlVel = 0;

char axis  = 0 ; 
char pwm   = 0 ; 

unsigned char oBuffer[12];
double xFinal=0 , yFinal=0;
int32_t translation=0;

#define BUF_SIZE 12
#define R_GAIN 10
#define I_GAIN 0.1f
#define X_GAIN 1.0f 
#define T_GAIN 0.01f 

USB_PUBLIC uchar usbFunctionSetup(uchar data[8])
{
//uchar           status = eepromRead(EEPROM_LOCATION);
static uchar    replyBuf[BUF_SIZE];

     
     if ( data[1] == USB_CHECKSTATE )
     {
	     usbMsgPtr  = replyBuf ;
	     if ( currentState == IDLE )
	     {
		     replyBuf[0] = 0;
		     replyBuf[1] = 0;
	     }
	     else
	     {
		     replyBuf[0] = 1;
		     replyBuf[1] = 1;
	     }
	     return 2;
     }
    else if ( data[1] == SERIAL_SET_ROBOT_VEL_PC )  /* SETTING THE ROBOT VELOCITY FROM PC*/
    {
	    currentState = SERIAL_SET_ROBOT_VEL_PC;
	    PORTD = 0 ; 		
	    usbMsgPtr = replyBuf;

	    lsb_fbVel = data[2] ; 
	    msb_fbVel = data[3] ; 

	    lsb_rlVel = data[4] ; 
	    msb_rlVel = data[5] ; 

	    replyBuf[0] = data[4]  ; 
	    replyBuf[1] = data[5]  ; 

	return 2 ; 	
    }			
    else if ( data[1] == SERIAL_RESET_ODOMETRY )
    {
	    usbMsgPtr = replyBuf ;

	    txChar_poll(SERIAL_RESET_ODOMETRY);

	    replyBuf[0] = rxChar_poll();          //SERIAL_RESET_ODOMETRY ;
	    replyBuf[1] = 2 ;
	    return 2;
	    
    }
    else if ( data[1] == SERIAL_GET_ODOMETRY )
    {
	    if ( odometryBufferLen == 12 )
	    {
		    usbMsgPtr = odometryBuffer;
		    odometryBufferLen = 0 ; 
		    currentState = IDLE ;
		    return 12;
	    }
	    if( odometryBufferLen == 0 )
	    {
		    currentState = SERIAL_GET_ODOMETRY ; 
	    }
	    usbMsgPtr = replyBuf  ;
	    replyBuf[0] = 2;
	    replyBuf[1] = 2;
	    return 2;	 	    
    }    
    return 0;
}

/* allow some inter-device compatibility */
#if !defined TCCR0 && defined TCCR0B
#define TCCR0   TCCR0B
#endif
#if !defined TIFR && defined TIFR0
#define TIFR    TIFR0
#endif

void disable_robot_serial_timeouts(void)
{
	char rec = 0 ; 
	txChar(SERIAL_DIS_SERIAL_TIMEOUTS);
	rec = rxChar_poll();
}



void setVelocity ( short fbVel , short rlVel )
{
	lsb_fbVel = fbVel ;
	msb_fbVel = (fbVel >> 8 );
	
	lsb_rlVel = rlVel;
	msb_rlVel = ( rlVel >> 8 );
	
	char received ;
	txChar_poll(SERIAL_SET_ROBOT_VEL_PC);
	received  = rxChar_poll();

	txChar_poll(lsb_fbVel);
	txChar_poll(msb_fbVel);

	txChar_poll(lsb_rlVel);
	txChar_poll(msb_rlVel);

	received = rxChar_poll();
	//PORTD = 0 ;

}



void synchronize_with_robot(void)
{	
	char sync_msg_hh[6] = { 's', 'h', 'r', 'i', 'm', 'p' };
	//const char sync_msg_ro[6] = { 't', 'i', 's', 'j', 'n', 'q' };
	int i=0	 ,  rec=0;
	//int status;
	
	
	for(; ;) 
	{
		txChar_poll(sync_msg_hh[i]);
		rec = rxChar_poll();
		if ( rec == 't' ) PORTD = 128 ;  
		i++;
			
		if ( i == 6 ) break ;  
	}
	
}


void getOdometry(unsigned char* oBuffer )
{
	txChar_poll(SERIAL_GET_ODOMETRY);
	oBuffer[0]  = rxChar_poll();
	oBuffer[1]  = rxChar_poll();
	oBuffer[2]  = rxChar_poll();
	oBuffer[3]  = rxChar_poll();
	oBuffer[4]  = rxChar_poll();
	oBuffer[5]  = rxChar_poll();
	oBuffer[6]  = rxChar_poll();
	oBuffer[7]  = rxChar_poll();
	oBuffer[8]  = rxChar_poll();
	oBuffer[9]  = rxChar_poll();
	oBuffer[10] = rxChar_poll();
	oBuffer[11] = rxChar_poll();

	return ;	
}

int main(void)
{
    uchar   i, j  ,flag = 0 ;
    unsigned int baudRate = 77	; // 9600 
    wdt_enable(WDTO_1S);

    	
    DDRD = ~(1 << 2);   /* all outputs except PD2 = INT0 */
    PORTD = 0;
    PORTB = 0;          /* no pullups on USB pins */
    DDRB = ~0;          /* output SE0 for USB reset */



    j = 0;
    while(--j){         /* USB Reset by device only required on Watchdog Reset */
        i = 0;
        while(--i);     /* delay >10ms for USB reset */
    }
    DDRB = ~USBMASK;    /* all outputs except USB data */
    TCCR0 = 5;          /* set prescaler to 1/1024 */

	
    PORTD = 0 ; 	
    usartInit(baudRate);	
    
    synchronize_with_robot();
    disable_robot_serial_timeouts();


    PORTD  = 128 ; 	
    usbInit();	
    sei();	
    flag = 0 ;
    currentState = IDLE ;
    PORTD = 128;  

    j = 0 ;
    PORTD = 0  ;
    int indicator = 0 ;
    for(;;)
    {    
	    /* main event loop */
	    wdt_reset();
	    usbPoll();


	if ( currentState == SERIAL_SET_ROBOT_VEL_PC ) 
	{
		while ( ! ( UCSRA & ( 1 << UDRE)) ) 
		{
	    		wdt_reset();
			usbPoll();		
		}
		UDR = SERIAL_SET_ROBOT_VEL_PC ;
		while ( ! ( UCSRA & ( 1 << RXC ) ) )
		{
	    		wdt_reset();
			usbPoll();
			
		}
		indicator = UDR  ;
		while ( ! ( UCSRA & ( 1 << UDRE) ) ) 
		{
	    		wdt_reset();
			usbPoll();		
		}
		UDR = lsb_fbVel ;
		while ( ! ( UCSRA & ( 1 << UDRE) ) ) 
		{
	    		wdt_reset();
			usbPoll();		
		}
		UDR = msb_fbVel ;
		/*delay inbetween */
		for ( j = 0 ; j < 10 ; j++ )
		{
			_delay_ms(2);
			wdt_reset();
			usbPoll();		
		}
		/*rest of it */
		while ( ! ( UCSRA & ( 1 << UDRE) ) ) 
		{
	    		wdt_reset();
			usbPoll();		
		}
		UDR = lsb_rlVel ;
		PORTD = 128 ; 
		while ( ! ( UCSRA & ( 1 << UDRE) ) ) 
		{
	    		wdt_reset();
			usbPoll();		
		}
		PORTD = 0 ;
		UDR = msb_rlVel;
		while ( ! ( UCSRA & ( 1 << RXC ) ) ) 
		{
	    		wdt_reset();
			usbPoll();
		}
		indicator = UDR ; 
		currentState = IDLE ; 

	}
	if ( currentState == SERIAL_GET_ODOMETRY )
	{
		while ( ! ( UCSRA & ( 1 << UDRE ) ) ) 
		{
	    		wdt_reset();
			usbPoll();		
		}
		UDR = SERIAL_GET_ODOMETRY ;
		for ( i = 0 ; i < 12 ; i++ )
		{
			while ( ! ( UCSRA & ( 1 << RXC ) ) ) 
			{
	    			wdt_reset();
				usbPoll();			
			}
			odometryBuffer[i] = UDR ; 
		}
		
		odometryBufferLen = 12 ;
		currentState = IDLE ; 
	}
	
    }
    return 0;
}
