/*************************** Includes ********************************/
#include <avr/io.h>
#include <stdlib.h>

/*************************** Prototypes ********************************/
void pwm_init(void);
void lcd_delay();        // short delay (50000 clocks)
void lcd_init();         // sets lcd in 4 bit mode, 2-line mode, with cursor on and set to blink
void lcd_cmd();          // use to send commands to lcd
void lcd_disp();         // use to display text on lcd
void lcd_clear();        // use to clear LCD and return cursor to home position
void lcd_row(int row);   // use to put the LCD at the desired row


/* IMPORTANT!

LCD is connected to PortC of the At-Mega128 in the

following manner:

PortC bit 7   :   LCD data bit 7 (MSB)
PortC bit 6   :   LCD data bit 6
PortC bit 5   :   LCD data bit 5
PortC bit 4   :   LCD data bit 4 (LSB)
 
PortC bit 3   :   (not connected)
PortC bit 2   :   LCD enable pin (clock)
PortC bit 1   :   LCD R/W (read / write) signal
PortC bit 0   :   LCD RS (register select) pin

Potentiometer is connected to the vcc, gnd, and contrast pins on the LCD.
The output of the pot (middle pin) is connected to the contrast pin.

*/

   int DELAY = 15 ;//100;

   int DELAY2 = 9;

   int CSTEP = 100;
   int ASTEP = 20;
   int BSTEP = 20;

   int ASTART = 1350;
   int AEND = 1650;
   
   int BSTART = 1350;
   int BEND = 1650;

   int CSTART = 1100;
   int CEND = 1900;

	int AMID = 1500;
	int BMID = 1450;

	int first = 1;
	
	int OBSTACLE = 35;





void lcd_delay()      // delay for 10000 clock cycles
{
   long int ms_count = 0;
   while (ms_count < 10000)
   {
      ms_count = ms_count + 1;
   }
}

void lcd_cmd( unsigned int myData )
{

   unsigned int temp_data = 0;

   temp_data = ( myData | 0b00000100 );       // these two lines leave the upper nibble as-is, and set
   temp_data = ( temp_data & 0b11110100 );  // the appropriate control bits in the lower nibble
   PORTC = temp_data;
   lcd_delay();
   PORTC = (temp_data & 0b11110000);          // we have written upper nibble to the LCD

   temp_data = ( myData << 4 );             // here, we reload myData into our temp. variable and shift the bits
                                             // to the left 4 times.  This puts the lower nibble into the upper 4 bits

   temp_data = (temp_data & 0b11110100);   // temp_data now contains the original
   temp_data = (temp_data | 0b00000100);   // lower nibble plus high clock signal
                                 
   PORTC = temp_data;                      // write the data to PortC
   lcd_delay();
   PORTC = (temp_data & 0b11110000);      // re-write the data to PortC with the clock signal low (thus creating the falling edge)
   lcd_delay();

}

void lcd_disp(unsigned int disp)
{

   unsigned int temp_data = 0;

   temp_data = ( disp & 0b11110000 );
   temp_data = ( temp_data | 0b00000101 );
   PORTC = temp_data;
   lcd_delay();
   PORTC = (temp_data & 0b11110001);
   lcd_delay();                  // upper nibble

   temp_data = (disp << 4 );
   temp_data = ( temp_data & 0b11110000 );
   temp_data = ( temp_data | 0b00000101 );
   PORTC = temp_data;    
   lcd_delay();
   PORTC = (temp_data & 0b11110001);
   lcd_delay();                  // lower nibble

}


void lcd_init()
{
   lcd_cmd(0x33);      // writing 0x33 followed by
   lcd_cmd(0x32);      // 0x32 puts the LCD in 4-bit mode
   
   lcd_cmd(0x28);      // writing 0x28 puts the LCD in 2-line mode

   lcd_cmd(0x0F);      // writing 0x0F turns the display on, curson on, and puts the cursor in blink mode

   lcd_cmd(0x01);      // writing 0x01 clears the LCD and sets the cursor to the home (top left) position

   //LCD is on... ready to write

}

void lcd_string(char *a)
{

   /*

   This function writes a string to the LCD.  LCDs can only print one character at a time so we need to
   print each letter or number in the string one at a time.  This is accomplished by creating a pointer to
   the beginning of the string (which logically points to the first character). All strings in C end with the "null" 
   character which is interpreted by the language as a 0.  So to print an entire string to the LCD we point to the 
   beginning of the string, print the first letter, then we increment the pointer (thus making it point to the second 
   letter), print that letter, and keep incrementing until we reach the "null" character".  This can all be easily 
   done by using a while loop that continuously prints a letter and increments the pointer as long as a 0 is not what 
   the pointer points to.

   */

   while (*a != 0)
   {
      lcd_disp((unsigned int) *a);   // display the character that our pointer (a) is pointing to
      a++;                     // increment a
   }
   return;
   
}


void lcd_int(int value)
{

   /*
   This routine will take an integer and display it in the proper order on
   your LCD.  Thanks to Josh Hartman (IMDL Spring 2007) for writing this in lab
   */

   int temp_val;
   int x = 10000;         // since integers only go up to 32768, we only need to worry about
                         // numbers containing at most a ten-thousands place

   while (value / x == 0)   // the purpose of this loop is to find out the largest position (in decimal)
   {                          // that our integer contains.  As soon as we get a non-zero value, we know
      x/=10;                // how many positions there are int the int and x will be properly initialized to the largest
   }                          // power of 10 that will return a non-zero value when our integer is divided by x.
            
   if (value==0) lcd_disp(0x30);
         
   else while (x>=1)            // this loop is where the printing to the LCD takes place.  First, we divide
   {                            // our integer by x (properly initialized by the last loop) and store it in
      temp_val = value / x;     // a temporary variable so our original value is preserved.Next we subtract the
      value -= temp_val * x;    // temp. variable times x from our original value.  This will "pull" off the most
      lcd_disp(temp_val+ 0x30); // significant digit from our original integer but leave all the remaining digits alone.
                                 // After this, we add a hex 30 to our temp. variable because ASCII values for integers
      x /= 10;                   // 0 through 9 correspond to hex numbers 30 through 39.  We then send this value to the
   }                            // LCD (which understands ASCII).  Finally, we divide x by 10 and repeat the process
                                   // until we get a zero value (note: since our value is an integer, any decimal value
   return;                         // less than 1 will be truncated to a 0)
   
}

void lcd_clear()      // this function clears the LCD and sets the cursor to the home (upper left) position
{
   lcd_cmd(0x01);

   return;
}

void lcd_row(int row)   // this function moves the cursor to the beginning of the specified row without changing
{                          // any of the current text on the LCD.

   switch(row)
   {

      case 0: lcd_cmd(0x02);
      case 1:   lcd_cmd(0xC0);

   }

   return;
}

/*void turn_left()
{
    int i;
    OCR1A = 1800;
    for (i =0 ; i < 40; i ++)
        lcd_delay();   
    OCR1A = 0;
}

void turn_right()
{
    int i;
    OCR1A = 750;
    for (i =0 ; i < 40; i ++)
        lcd_delay();
    OCR1A = 0;
}
*/


void config_adc(void)
{
   DDRF = 0b00001010; // set port F to all input
                  // when JTAGEN fuse is set, F4 - F7 don't work
   PORTF = 0x00;      // make sure pull up resistor is not enabled

   ADMUX = 0b01000000; // 5V reference, select channel0 (pin F0)
   ADCSRA |= 0b10000111; // turn on ADC
                    // free funning
                    // divide clock by 128

} 



int analog(int analogch)
{
     int anval;
    ADMUX = 0b01000000|analogch;
    // Start AD conversion.
    ADCSRA |= (1 << ADSC);
    // Wait for ADC conversion to complete.
    while ( ADCSRA & (1 << ADSC) );
      anval = ADCL | (ADCH << 8); //place ACD value into one variable
    return anval;
}

int getSonarLeft()
{
 int temp, anval=177, anch=0, anch2 = 2,i;
   int analogLow = 0;
   int analogHigh = 1023;
   int anRval=1023;
   long total = 0;
   PORTF = 0b00000010;

   for (i = 0; i < 10; i++)
   {

	    anval=analog(anch);
	  analogLow = ADCL; // read ACD low register
   	  analogHigh = ADCH; // read ACD high register 
      anRval = analogLow | (analogHigh << 8); //place ACD value into one variable
	  total += anRval;
}
 PORTF = 0b00000000;
	  return (int)((double) total/ (double) 10.0);
  
}
int getSonarRight()
{
int temp, anval=177, anch=0, anch2 = 2,i;
   int analogLow = 0;
   int analogHigh = 1023;
   int anRval=1023;
   long total = 0;

	PORTF = 0b00001000;
   for (i = 0; i < 10; i++)
   {

	    anval=analog(anch2);
	  analogLow = ADCL; // read ACD low register
   	  analogHigh = ADCH; // read ACD high register 
      anRval = analogLow | (analogHigh << 8); //place ACD value into one variable
	  total += anRval;
}
	PORTF = 0b00000000;
	  return (int)((double) total/ (double) 10.0);
}
void init_sonar()
{

	config_adc();
}

void gostraight()
{
	long i,j;


	int first = 1;
	
	
   //OCR1C = 1950;


    for ( i  = 0; i < DELAY; i ++)
        lcd_delay();


   
   		int ctr1,ctr2;

   /*Step1*/

   		for (j = OCR1C; j < CEND; j += CSTEP)
		{
		   	  OCR1C = j;//1850;
			  for (i = 0; i < DELAY2; i++)
			  	lcd_delay();
		}
		OCR1C = CEND;
		
    	for (i  = 0; i < DELAY; i ++)
        	lcd_delay();


  /*Step2*/
  
		if (first)
		{ 
			int ctr3 = AMID;//////////////////????????????????????
			int ctr4 = BMID;
			first = 0;
			while (1)
			{
				if (ctr3 >= AEND && ctr4 >= BEND)
					break;
				OCR1A = ctr3;
				OCR1B = ctr4;
				ctr3 += ASTEP;
				ctr4 += BSTEP;
				for (i = 0; i < DELAY2; i++)
			  		lcd_delay();
			}
		}

		ctr1 = AEND;
		ctr2 = BEND;

		while (1)
		{
			if (ctr1 <= ASTART && ctr2 <= BSTART)
				break;
		
			OCR1A = ctr1;
			OCR1B = ctr2;
		
			ctr1 -= ASTEP;
			ctr2 -= BSTEP;
			for (i = 0; i < DELAY2; i++)
		  		lcd_delay();
		}

		for (i  = 0; i < DELAY; i ++)
        	lcd_delay();

  /*Step3*/
		
			for (j = CEND; j > CSTART; j -= CSTEP)
			{
			   	  OCR1C = j;//550;
				  for (i = 0; i < DELAY2; i++)
					  	lcd_delay();
			}
			OCR1C = CSTART;


       		 for ( i  = 0; i < DELAY; i ++)
        		lcd_delay();
    
		
	  /*Step4*/	

	

		ctr1 = ASTART;
		ctr2 = BSTART;

		while (1)
		{
			if (ctr1 >= AEND && ctr2 >= BEND)
				break;
		
			OCR1A = ctr1;
			OCR1B = ctr2;
		
			ctr1 += ASTEP;
			ctr2 += BSTEP;
			for (i = 0; i < DELAY2; i++)
		  		lcd_delay();
		}

		for (i  = 0; i < DELAY; i ++)
        	lcd_delay();


    

}

void turnleft ()
{
	long i ,j,k;

//	move (2,CEND,CSTART,-CSTEP);
	if (OCR1C != CSTART)
	{
	for (i = OCR1C; i >= CSTART; i-= CSTEP)
	{
		OCR1C = i;
		for (j = 0; j < DELAY2; j++)
			lcd_delay();
	}
	for (j = 0; j < DELAY; j++)
			lcd_delay();
	}
	OCR1C = CSTART;

//move (0,AEND,ASTART,-ASTEP);
	for (i = OCR1A, k = OCR1B; i >= ASTART && k <= BEND; i-= ASTEP, k += BSTEP)
	{
		OCR1A = i;
		OCR1B = k;
		for (j = 0; j < DELAY2; j++)
			lcd_delay();
	}
	for (j = 0; j < DELAY; j++)
			lcd_delay();
	OCR1A = ASTART;
	OCR1B = BEND;
	
	//move (1,BSTART,BEND,BSTEP);
	

	//move (2,CSTART,CEND,CSTEP);
	for (i = CSTART; i <= CEND; i += CSTEP)
	{
		OCR1C = i;
		for (j = 0; j < DELAY2; j++)
			lcd_delay();
	}
	for (j = 0; j < DELAY; j++)
			lcd_delay();
	
	OCR1C = CEND;
	//move (0,AEND,AMID, -ASTEP);


	for (i = ASTART, k = BEND; i <= AMID && k >= BMID ; i += ASTEP, k-= BSTEP)
	{
		OCR1A = i;
		OCR1B = k;
		for (j = 0; j < DELAY2; j++)
			lcd_delay();
	}
	for (j = 0; j < DELAY; j++)
			lcd_delay();
	OCR1A = AMID;
	OCR1B = BMID;

	if (OCR1C != CSTART)
	{
		for (i = CEND; i >= CSTART; i-= CSTEP)
		{
			OCR1C = i;
			for (j = 0; j < DELAY2; j++)
				lcd_delay();
		}
		for (j = 0; j < DELAY; j++)
			lcd_delay();
	}
	OCR1C = CSTART;
}

void turnright ()
{

	long i ,j,k;

//	move (2,CEND,CSTART,-CSTEP);
	if (OCR1C != CEND)
	{
	for (i = OCR1C; i <= CEND; i+= CSTEP)
	{
		OCR1C = i;
		for (j = 0; j < DELAY2; j++)
			lcd_delay();
	}
	for (j = 0; j < DELAY; j++)
			lcd_delay();
	}
	OCR1C = CEND;



//move (0,AEND,ASTART,-ASTEP);
/*
	for (i = ASTART, k = BEND; i <= AEND && k >= BSTART; i += ASTEP, k -= BSTEP)
	{
		OCR1A = i;
		OCR1B = k;
		for (j = 0; j < DELAY2; j++)
			lcd_delay();
	}
	for (j = 0; j < DELAY; j++)
			lcd_delay();
	OCR1A = AEND;
	OCR1B = BSTART;
	*/
	for (i = OCR1A, k = OCR1B; i >= ASTART && k <= BEND; i-= ASTEP, k += BSTEP)
	{
		OCR1A = i;
		OCR1B = k;
		for (j = 0; j < DELAY2; j++)
			lcd_delay();
	}
	for (j = 0; j < DELAY; j++)
			lcd_delay();
	OCR1A = ASTART;
	OCR1B = BEND;
	//move (1,BSTART,BEND,BSTEP);
	

	//move (2,CSTART,CEND,CSTEP);
	for (i = CEND; i <= CSTART; i -= CSTEP)
	{
		OCR1C = i;
		for (j = 0; j < DELAY2; j++)
			lcd_delay();
	}
	for (j = 0; j < DELAY; j++)
			lcd_delay();
	
	OCR1C = CSTART;
	//move (0,AEND,AMID, -ASTEP);
	for (i = ASTART, k = BEND; i <= AMID && k >= BMID ; i += ASTEP, k-= BSTEP)
	{
		OCR1A = i;
		OCR1B = k;
		for (j = 0; j < DELAY2; j++)
			lcd_delay();
	}
	for (j = 0; j < DELAY; j++)
			lcd_delay();
	OCR1A = AMID;
	OCR1B = BMID;


}

int guess ()
{
	double a_number;
    
    a_number = (float) random();
    
    a_number = (float) random() / (float) 0x7fffffff;

	if (a_number > 0.5)
		return 0;
	return 1;
}

/*************************** Globals ********************************/
    signed int motor1drive;
    signed int motor2drive;
    signed int motor3drive;


int main(void)
{

    long i = 0, j = 0;
	   //Walk!



   DDRC = 0xFF;               // set portC to output  (could also use DDRC = 0b11111111)
   DDRE = 0xFF;          // set portE to output
 //DDRE = 0b11111111;     // set portE to output
   DDRB = 0xFF;            // set portB to output

   lcd_init();     // set lcd in 4 bit mode, 2-line mode, with cursor on and set to blink 

   lcd_string("Your LCD is working.");   
   lcd_row(1);
   lcd_string("PWM Generator Test");



   pwm_init();

	init_sonar();

	OCR1A = AMID;
	OCR1B = BMID;
	OCR1C = 600;

	while (1)
	{
		int s1 = getSonarLeft();
		int s2 = getSonarRight();
   		
	   lcd_clear();
		lcd_int (s1);
		lcd_string (" ");
		lcd_int (s2);
		for ( i = 0; i < DELAY; i ++)
			lcd_delay();
		lcd_row(1);
		if ((s1 < 25 && s2 < 25)||(s1 < 15)||(s2 <15))
		{
			int v;
			v =guess ();
			if (v)
			{
				lcd_string ("GO LEFT");
			    turnleft ();
				turnleft ();
				turnleft ();
				

			}
			else
			{
				lcd_string ("GO RIGHT");
			    turnright ();
				turnright ();
				turnright ();
				
			}
		}
		else
		{
			lcd_string ("GO STRAIGHT");
			gostraight();
			
		}
	}
/*
   while (1)
   {//550,2200

   	//start A 550. Start B 500
	//End A 2250 and End B 2200


	//Start C 350 and End C 2400

	//Center A 1450 and Center B 1575 and Center C 1375  and in program: start C 600 and End C 2200

	// For end positions of big motors, 
		 //moving back: 1. Safe values: 1625,1725 2. limiting values: 1700, 1800
		 //moving ahead 1. Safe values: 1275, 1400	 limiting values: 1250, 1375
 //proposal: Motor 1 oscilates 1275 - 1625, Motor 2 oscilates 1400 - 1725 Motor 3 oscilates 600 - 2200
	   OCR1A = 1250;
	   OCR1B = 1375;
		OCR1C = 600;
	}*/

	

while(1);
   //OCR1C = 1950;


    for ( i  = 0; i < DELAY; i ++)
        lcd_delay();


   while (1)
   {
   		int ctr1,ctr2;

   /*Step1*/

   		for (j = CSTART; j < CEND; j += CSTEP)
		{
		   	  OCR1C = j;//1850;
			  for (i = 0; i < DELAY2; i++)
			  	lcd_delay();
		}
		OCR1C = CEND;
		
    	for (i  = 0; i < DELAY; i ++)
        	lcd_delay();


  /*Step2*/
  
		if (first)
		{
			int ctr3 = AMID;
			int ctr4 = BMID;
			first = 0;
			while (1)
			{
				if (ctr3 >= AEND && ctr4 >= BEND)
					break;
				OCR1A = ctr3;
				OCR1B = ctr4;
				ctr3 += ASTEP;
				ctr4 += BSTEP;
				for (i = 0; i < DELAY2; i++)
			  		lcd_delay();
			}
		}

		ctr1 = AEND;
		ctr2 = BEND;

		while (1)
		{
			if (ctr1 <= ASTART && ctr2 <= BSTART)
				break;
		
			OCR1A = ctr1;
			OCR1B = ctr2;
		
			ctr1 -= ASTEP;
			ctr2 -= BSTEP;
			for (i = 0; i < DELAY2; i++)
		  		lcd_delay();
		}

		for (i  = 0; i < DELAY; i ++)
        	lcd_delay();

  /*Step3*/
		
			for (j = CEND; j > CSTART; j -= CSTEP)
			{
			   	  OCR1C = j;//550;
				  for (i = 0; i < DELAY2; i++)
					  	lcd_delay();
			}
			OCR1C = CSTART;


       		 for ( i  = 0; i < DELAY; i ++)
        		lcd_delay();
    
		
	  /*Step4*/	

	

		ctr1 = ASTART;
		ctr2 = BSTART;

		while (1)
		{
			if (ctr1 >= AEND && ctr2 >= BEND)
				break;
		
			OCR1A = ctr1;
			OCR1B = ctr2;
		
			ctr1 += ASTEP;
			ctr2 += BSTEP;
			for (i = 0; i < DELAY2; i++)
		  		lcd_delay();
		}

		for (i  = 0; i < DELAY; i ++)
        	lcd_delay();


    }
return 0;
} // main

void    pwm_init(void)
{
    TCCR1A = 0b10101000;    // ......00:P&FC PWM // 11......,..11....:OC1A,OC1B inv
    TCCR1B = 0b00010010;    // ...10...:P&FC PWM // .....010:bclk/8 (~8kHz)
    DDRB = 0xFF;            // set PWM pins as outputs, PB5 (OC1A) & PB6 (OC1B)
    ICR1= 18400;//10400;
    TCNT1=0x0000;
    OCR1A = 0x0000;            // 1/2 duty, ~2.5v dc level, motor 1 on PB5 (OC1A)
    motor1drive = 0;
    OCR1B = 0x0000;            // 0/256 duty, ~2.5v dc level, motor 2 on PB6 (OC1B)
    motor2drive = 0;
    OCR1C = 0x0000;
    motor3drive = 0;
} // pwm_init