// STANDARD ANSI INCLUDES
#include <std.h> // DSP/BIOS standard include file
#include <hwi.h>
#include <swi.h>
#include <log.h> // LOG_printf calls
#include <mem.h> // MEM_alloc calls
#include <que.h> // QUE functions
#include <sem.h> // Semaphore functions
#include <sys.h>
#include <tsk.h> // TASK functions
#include <rtdx.h> 
#include <math.h> // sinf,cosf,fabsf, etc.
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

// DSP INCLUDES
#include <c6x.h>         // register defines, in c:\ti\c6000\cgtools\include
#include <c6x11dsk.h>    // TI header in the directory c:\ti\c6000\dsk6x11\include
#include <fastrts67x.h>  // TI's real-time math library, in c:\ti\c6700\mthlib\include

// COECSL INCLUDES       // all COECSL functions are usually in the directory f:\C6713DSK\include
#include <c6xdskdigio.h> // COECSL functions for daugher card, encs, PWM
#include <max3100uart.h> // COECSL functions for communication to max serial chip
#include <dac7724.h>     // COECSL functions for the DAC7724 chip
#include <ad7864.h>     // COECSL functions for the AD7864 chip
#include <RCservo.h>     // COECSL functions to set up PWM ch3 and ch4 to drive RC servos
#include <switch_led.h>  // COECSL functions for turning off LEDs, monitoring switches
#include <dspvision.h>
#include <i2c.h>
#include <edma.h>
#include <sharpir.h>
#include <dsk6713.h>
#include <user_VisionFuncs.h>
#include <user_UARTFuncs.h>
#include <user_PIFuncs.h>
#include <user_IR_UltraFuncs.h>

#define ATMEL_ADDR 0x55
#define ATMEL_SEND_REQ 100


unsigned int time=0;
int i2cerror = 0;
unsigned long starttime;
unsigned int IR_dat_1=0, IR_dat_2=0, IR_dat_3=0;

void talk_Atmel() {
	while(1){
	TSK_sleep(100);
		i2cerror=0;
	
		// wait for I2C bus to be free
		starttime = CLK_getltime();

		while ( (I2Cbusy == 1) && (i2cerror == 0) ){	
			TSK_yield();
			if ((CLK_getltime() - starttime) > I2C_ACCESS_TIMEOUT) {
				i2cerror = 1;
			}
		}
/*		if (i2cerror == 0) {
			I2Cbusy = 1;
			*(unsigned volatile int *)I2CMDR0 = 0x0; // reset I2C
			*(unsigned volatile int *)I2CCNT0 = 0x1;  
			*(unsigned volatile int *)I2CSAR0 = ATMEL_ADDR;
			*(unsigned volatile int *)I2CMDR0 = 0x2e20;	//send something then stop
			*(unsigned volatile int *)I2CDXR0 = starttime; 		    
			starttime = CLK_getltime();
			while ( ((*(unsigned volatile int *)I2CSTR0 & 0x10) == 0x0) && (i2cerror == 0 ) ) {
				TSK_yield();
				if ((CLK_getltime() - starttime) > I2C_TIMEOUT) {
					i2cerror = 1;
				}
			}
		}
		TSK_sleep(100);
*/		
		//"wake up" atmel to send IR data
		if (i2cerror == 0) {
			I2Cbusy = 1;
			*(unsigned volatile int *)I2CMDR0 = 0x0; // reset I2C
			*(unsigned volatile int *)I2CCNT0 = 0x1;  
			*(unsigned volatile int *)I2CSAR0 = ATMEL_ADDR;
			*(unsigned volatile int *)I2CMDR0 = 0x2620;	//send something don't stop
			*(unsigned volatile int *)I2CDXR0 = ATMEL_SEND_REQ+1; 		//request IR 1  
			starttime = CLK_getltime();
			while ( ((*(unsigned volatile int *)I2CSTR0 & 0x10) == 0x0) && (i2cerror == 0 ) ) {
				TSK_yield();
				if ((CLK_getltime() - starttime) > I2C_TIMEOUT) {
					i2cerror = 1;
				}
			}
		}
		
		//get first IR read
		if (i2cerror == 0) {
			*(unsigned volatile int *)I2CCNT0 = 0x1;  
			*(unsigned volatile int *)I2CMDR0 = 0x2C20;   //receive something, don't stop
			starttime = CLK_getltime();
			while ( ((*(unsigned volatile int *)I2CSTR0 & 0x0008) == 0x0000) && (i2cerror == 0) ) {
				TSK_yield();
				if ((CLK_getltime() - starttime) > I2C_TIMEOUT*2) {
					i2cerror = 1;
				}
			}
		}
		if (i2cerror == 0) {
			IR_dat_1 = *(unsigned volatile int *)I2CDRR0;
		}
		
		while ( (I2Cbusy == 1) && (i2cerror == 0) ){	
			TSK_yield();
			if ((CLK_getltime() - starttime) > I2C_ACCESS_TIMEOUT) {
				i2cerror = 1;
			}
		}
		i2cerror = 0;
				//"wake up" atmel to send IR data
		if (i2cerror == 0) {
			I2Cbusy = 1;
			*(unsigned volatile int *)I2CMDR0 = 0x0; // reset I2C
			*(unsigned volatile int *)I2CCNT0 = 0x1;  
			*(unsigned volatile int *)I2CSAR0 = ATMEL_ADDR;
			*(unsigned volatile int *)I2CMDR0 = 0x2620;	//send something don't stop
			*(unsigned volatile int *)I2CDXR0 = ATMEL_SEND_REQ+2; 		//request IR 2    
			starttime = CLK_getltime();
			while ( ((*(unsigned volatile int *)I2CSTR0 & 0x10) == 0x0) && (i2cerror == 0 ) ) {
				TSK_yield();
				if ((CLK_getltime() - starttime) > I2C_TIMEOUT) {
					i2cerror = 1;
				}
			}
		}
		//get second IR
		if (i2cerror == 0) {
			*(unsigned volatile int *)I2CCNT0 = 0x1;  
			*(unsigned volatile int *)I2CMDR0 = 0x2C20;   //receive something, don't stop
			starttime = CLK_getltime();
			while ( ((*(unsigned volatile int *)I2CSTR0 & 0x0008) == 0x0000) && (i2cerror == 0) ) {
				TSK_yield();
				if ((CLK_getltime() - starttime) > I2C_TIMEOUT*2) {
					i2cerror = 1;
				}
			}
		}
		if (i2cerror == 0) {
			IR_dat_2 = *(unsigned volatile int *)I2CDRR0;
		}
			
		while ( (I2Cbusy == 1) && (i2cerror == 0) ){	
			TSK_yield();
			if ((CLK_getltime() - starttime) > I2C_ACCESS_TIMEOUT) {
				i2cerror = 1;
			}
		}
		
		i2cerror = 0;
				//"wake up" atmel to send IR data
		if (i2cerror == 0) {
			I2Cbusy = 1;
			*(unsigned volatile int *)I2CMDR0 = 0x0; // reset I2C
			*(unsigned volatile int *)I2CCNT0 = 0x1;  
			*(unsigned volatile int *)I2CSAR0 = ATMEL_ADDR;
			*(unsigned volatile int *)I2CMDR0 = 0x2620;	//send something don't stop
			*(unsigned volatile int *)I2CDXR0 = ATMEL_SEND_REQ+3; 		//request IR 3    
			starttime = CLK_getltime();
			while ( ((*(unsigned volatile int *)I2CSTR0 & 0x10) == 0x0) && (i2cerror == 0 ) ) {
				TSK_yield();
				if ((CLK_getltime() - starttime) > I2C_TIMEOUT) {
					i2cerror = 1;
				}
			}
		}	
		//get third IR
		if (i2cerror == 0) {
			*(unsigned volatile int *)I2CCNT0 = 0x1;  
			*(unsigned volatile int *)I2CMDR0 = 0x2C20;   //receive something and stop
			starttime = CLK_getltime();
			while ( ((*(unsigned volatile int *)I2CSTR0 & 0x0008) == 0x0000) && (i2cerror == 0) ) {
				TSK_yield();
				if ((CLK_getltime() - starttime) > I2C_TIMEOUT*2) {
					i2cerror = 1;
				}
			}
		}
		if (i2cerror == 0) {
			IR_dat_3 = *(unsigned volatile int* )(I2CDRR0);
		}
		
		
		I2Cbusy = 0;
		
		LCDPrintfLine1("IR1:%d  IR2:%d", IR_dat_1,IR_dat_2);
		LCDPrintfLine2("IR3:%d", IR_dat_3);
		
//		read_UltraSonicLight(pIR_dat_2, 0x60);
	}
}


// The Main() function is used primarily for performing initializations.
// Most of the interesting code(your code) will be in functions you create.
void main(void) {


    Init_C6713DSK();    // set up DSP to talk to external 8 Meg of SDRAM.
    Init_UART1and2(2);  // configure multi-buffered McBSP for SPI, tell max3100 what baudrate to use.
    Init_Wireless();
    Init_pwm(1);     // zero chip 1's two PWM channels.
    Init_encoders(1);   // initialize encoders to starting zero position.
    Init_RCservo();  // setup second PWM chip to command RC Servos.
    Init_encoders(2);   // initialize encoders to starting zero position.
 	Init_i2c();
	Init_LCD();
 	Init_SharpIRs();
	Init_DAC7724();
    

#ifdef USEVISION
    Init_vision();
#endif
    
    enable_edma();  // needs to be call to enable interrupt for both the camera and the i2c bus

	set_LEDstate(get_switchstate()); // Set LEDs to Switch State
	
	
    
}