/**
  Reads all relevant sensor values and stores it in appropriate global variables.
*/
void read_sensors()
{
	Left_white_line = ADC_Conversion(LEFT_SENSOR);	
	Center_white_line = ADC_Conversion(CENTER_SENSOR);	
	Right_white_line = ADC_Conversion(RIGHT_SENSOR);	
	Front_IR_Sensor = ADC_Conversion(FRONT_IR_SENSOR);
}

void arenaLeft_2()
{
    nodeMissEnable = 0;
/*
    stop();
    _delay_ms(1000);


                soft_left();
                _delay_ms(1000);
*/
    velocity(forwardLeftSpeed, forwardRightSpeed);
    left();
    _delay_ms(1000);

    centre = ADC_Conversion(2);

    while(centre < 15)
    {
        centre = ADC_Conversion(2);
    }

    stop();
    _delay_ms(100);

    nodeMissEnable = 1;
}

		int getValue1(void)
{
    
	centre = ADC_Conversion(2);
   sensorLeft = ADC_Conversion(3);
   sensorRight = ADC_Conversion(1);
}

//++++++===Servo __init__ ========++++++++
void set_color()
{
	Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
	Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
	Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor

}

int scan()//return the color no.
{
	red_read();
	blue_read();
	green_read();
	_delay_ms(100);
	if(!(ADC_Conversion(11)>55 && ADC_Conversion(11)<100))
		return EMPTY;
	
	if (red<threshold && green < threshold && blue < threshold)
	{
			return BLACK;
	}
	else
	{
		if (red > blue)
		{
			if (red > green)
				return RED;
			else
				return GREEN;
		}
		else if (blue > green)
			return BLUE;
		else return GREEN;
	}

}

//-------------------------------------------------------------------------------
//Main Function
//-------------------------------------------------------------------------------
int main(void)
{
	init_devices();


	while(1)
	{
		sensor_data_interpretation();
		//print_sensor(1,1,1);				//Prints IR Proximity Sensor 1
		BATT_V = ADC_Conversion(0);
		BATT_Voltage = ((ADC_Conversion(0)*100)*0.07902) + 0.7;	//Prints Battery Voltage Status
		lcd_print(1,1,BATT_Voltage,4);

		//print_sensor(1,1,0);							//Prints Battery voltage binary value

		print_sensor(1,6,5);							//Prints IR Proximity Sensor 1
		print_sensor(1,10,6);							//Prints vlaue of Analog IR Proximity Sensor 2
		print_sensor(1,14,7);							//Prints value of Analog IR Proximity Sensor 3
		print_sensor(2,2,3);							//Prints value of White Line Sensor1
		print_sensor(2,6,2);							//Prints Value of White Line Sensor2
		print_sensor(2,10,1);							//Prints Value of White Line Sensor3

		//print_sensor(2,9,11); 						//Analog Value Of Front Sharp Sensor

		sharp = ADC_Conversion(11);						//Stores the Analog value of front sharp connected to ADC channel 11 into variable "sharp"
		value = Sharp_GP2D12_estimation(sharp);				//Stores Distance calsulated in a variable "value".
		lcd_print(2,14,value,3);
	}

}

/*
This function first rotates the bot by 5 degree till 360 
Stores the voltage at each angle in array
Find maximum value of voltage from the array
Aligns the bot at that value of voltage.
*/
void bot_rotation360()
{    
     init_devices_motion();
	 init_devices_adc();
	 int bot_ang=0;
	 float panel_voltage[72];
	 float max_panel_volt=0;
	 float bat_voltage = 0.0;
	 int j;
	
	
	 for(j=0;j<72;j++)
	 {
		 
		 soft_right();
		                           // halting right wheel and moving only the left wheel for specified time to get the desired angle of rotation(according to calculation)
		 _delay_ms(rot_time);      // rot_time is calculated according to power from the battery
		 
		  stop();
		  _delay_ms(delay_time);
		
		 panel_voltage[j]=value_in_volt(ADC_Conversion(10)); // channel 10 contains voltage reading 
		 lcd_print(2,1,panel_voltage[j],3);
		 bat_voltage=batt_volt(ADC_Conversion(0));
		 lcd_print(1,13,bat_voltage,4);      // Printing battery voltage.
		
	 }
	  _delay_ms(1000);      // Stopping the bot momentarily.
	  
	  // for getting the angle of bot corresponding to maximum voltage and maximum voltage also.
	  for( j=0;j<72;j++)
	  {      
		  if(panel_voltage[j]>max_panel_volt)
		  {
			  max_panel_volt=panel_voltage[j];
			  bot_ang=j;
		  }
	  }
     lcd_print(2,1,panel_voltage[bot_ang],3);      // Printing maximum voltage.
	 
	                                // Realigning the bot at the maximum angle of intensity in circular plane
	   j=1;         
	  while(j!=bot_ang)            // Since the bot_ang variable contains the angle for which the intensity was maximum
	  {
		  
		  soft_right();       //left wheel forward leaving the right wheel at rest to get
		                     //soft rotation at the axis of right wheel
		  _delay_ms(rot_time2);
		  float servo_volt=value_in_volt(ADC_Conversion(10));
		  lcd_print(2,5,servo_volt,3);
		  j++;
		  stop();
		  _delay_ms(delay_time);
		  
	  }
	   lcd_print(2,5,panel_voltage[bot_ang],3);
	 	
	  max_angle_of_bot=bot_ang;
	 
}

void arenaUturn(void)
{
    nodeMissEnable = 0;
/*
    stop();
    _delay_ms(1000);
*/
 //   velocity(forwardLeftSpeed, forwardRightSpeed);
/*
                line_follow_mm(30);
                stop();
                _delay_ms(1000);

                soft_right();
                _delay_ms(1000);
*/
    stop();
    _delay_ms(100);

    velocity(forwardLeftSpeed, forwardRightSpeed);
    left();
    _delay_ms(2500);

    centre = ADC_Conversion(2);

    while(centre < 15)
    {
        centre = ADC_Conversion(2);
    }

    stop();
    _delay_ms(100);

    nodeMissEnable = 1;
}

//Function used for turning robot by specified degrees
void angle_rotate(unsigned int Degrees)
{
	float ReqdShaftCount = 0;
	unsigned long int ReqdShaftCountInt = 0;

	ReqdShaftCount = (float) Degrees/ 4.090; // division by resolution to get shaft count
	ReqdShaftCountInt = (unsigned int) ReqdShaftCount;
	ShaftCountRight = 0;
	ShaftCountLeft = 0;

	while (1)
	{
		Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
		Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
		Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor
		
		if((ShaftCountRight >= ReqdShaftCountInt) | (ShaftCountLeft >= ReqdShaftCountInt))
		{
			break;
		}		
		
		if((Left_white_line<0x20) && (Center_white_line>0x20) && (Right_white_line<0x20))//Black
		{
			break;
		}		
	}
	stop(); //Stop robot
}

       int getValue2(void)
{	 
   value_front = ADC_Conversion(11); 
   value_4sens= ADC_Conversion(12);
   value_2sens= ADC_Conversion(10);
   value_1sens=ADC_Conversion(9);
    
}

void get_vector() {
Left_white_line = ADC_Conversion(3);
Center_white_line = ADC_Conversion(4);
Right_white_line = ADC_Conversion(5);
print_sensor(2,1,3); //Prints value of White Line Sensor Left
print_sensor(2,5,4); //Prints value of White Line Sensor Center
print_sensor(2,9,5); //Prints Value of White Line Sensor Right
}

void sensor_data_interpretation(void) 
{

 SHARP_1 = ADC_Conversion(9);
 Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
 Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
 Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor
 
}

/*--functions--*/
void set_color()
{
	Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
	Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
	Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor
	/*lcd_print(1,1,Left_white_line,3);	//Prints value of White Line Sensor1
	lcd_print(1,5,Center_white_line,3);	//Prints Value of White Line Sensor2
	lcd_print(1,9,Right_white_line,3);	//Prints Value of White Line Sensor3
	*/
}

int getError2(void)
{
    int error;
    centre = ADC_Conversion(2);
    sensorLeft = ADC_Conversion(3);
    sensorRight = ADC_Conversion(1);

    lcd_print(2, 1, sensorLeft, 3);
    lcd_print(2, 5, centre, 3);
    lcd_print(2, 9, sensorRight, 3);

    if(sensorLeft > 40 && sensorRight > 40)
        error = 100;

    else if(centre > 90)
        error = 0;

    else if(sensorLeft > 45)
        error = 7;

    else if(sensorLeft > 25)
        error = 6;

    else if(sensorLeft > 17)
        error = 5;

    else if(sensorLeft > 9)
        error = 4;

    else if(sensorRight > 110)
        error = -7;

    else if(sensorRight > 85)
        error = -6;

    else if(sensorRight > 55)
        error = -5;

    else if(sensorRight > 35)
        error = -4;

    else if(sensorRight > 25)
        error = -3;

    else if(sensorRight > 18)
        error = -2;

    else if(sensorRight > 15)
        error = -1;

    else
        error = -100;

        return -error;
}

void terminalCheck2()
{
	if (flag == 0)
	{
		if (dir == 0)
			if (ot == 0 || ot == 1)
				forward_mm(30);
			else back_mm(30);
		else if (ot == 0 || ot == 1)
			back_mm(30);
		else forward_mm(30);
		flag = 1;
	}

	if (((ct == 0 || ct == 1) && dir == 0) || ((ct == 2 || ct == 3) && dir == 2))
	{
		left_degrees(30);
		velocity(turn_v, turn_v);
		while (ADC_Conversion(1)<70)
		left();
		//	_delay_ms(100);
		stop();
	}
	else if (((ct == 0 || ct == 1) && dir == 2) || ((ct == 2 || ct == 3) && dir == 0))
		{
			right_degrees(30);
			velocity(turn_v, turn_v);
			while (ADC_Conversion(1)<70)
				right();
			//	_delay_ms(100);
			stop();	
		}
	else {
		left_degrees(150);
		velocity(turn_v, turn_v);
		while (ADC_Conversion(1)<70)
		left();
		//	_delay_ms(100);
		stop();
		
			}
	//printf("Enter term[%d][%d]\n", ct, 1);
	//scanf("%d", &term[ct][1]);
	term[ct][1] = scan();
	if(term[ct][1]==-1)
		lcd_print(2,11,9, 1);
	else
		lcd_print(2,11, term[ct][1], 1);
	_delay_ms(1000);
	if (term[ct][1] == -1 || term[ct][1] == color[ct])
		total--;
	visited[ct] = 1;
	visitedCount++;
}

void Read_And_Print() {
    stop();
    Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
    Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
    Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor

    /*lcd_print(2, 1, Left_white_line, 3);
    lcd_print(2, 7, Center_white_line, 3);
    lcd_print(2, 13, Right_white_line, 3);*/

}

/* Rotating the panel in the plane of servo hinge by one degree each till 165 
 Here we have rotated the servo only upto 165 not 180 due to large size of panel, that is when rotated by more than 
 165 it start touching the upper plate of bot.
 Here first the servo is rotated upto 165 each by one degree and the value corresponding to voltage is noted in the array
 then maximum value is calculated from the array and the corresponding angle.
 Then the panel is aligned to that angle.
 Delay time for each rotation is 400 milli-seconds.
*/
void servo_rotation_165()
{ 
  init_devices_servo();
  unsigned int  i = 0;
  float max_panel_volt=0.0;
  int counter=0;
  float panel_voltage_s[165];
	
  for (i = 0; i <165; i++)
 {
   servo_1(i);
   _delay_ms(delay_time);
   
   panel_voltage_s[i]=value_in_volt(ADC_Conversion(10));   // ADC_Conversion gives analog value of voltage through channel 10
   lcd_print(1,13,batt_volt(ADC_Conversion(0)),4);	// Printing the battery voltage.
   lcd_print(2,9,panel_voltage_s[i],3);       // Printing voltage of panel at specific angles.
                                             // _delay_ms(50);	
 }

 // finding the maximum intensity of value and corresponding angle
   for(int j=0;j<165;j++)
  {
    if(panel_voltage_s[j]> max_panel_volt)
    {
     max_panel_volt= panel_voltage_s[j];
     counter=j;       // identifier counter contains the angle for which the intensity is maximum
    }
  }
   lcd_print(2,9,panel_voltage_s[counter],3);   // Printing maximum voltage.
   
           
// setting the panel at that angle of maximum intensity
   
   servo_panel_0();
  
  for (int j = 0; j<counter;j++)             // Setting the panel at maximum voltage.
  {  
	  servo_1(j);
	  _delay_ms(delay_time);
	  float panel_volt=value_in_volt(ADC_Conversion(10));
	  lcd_print(2,13,panel_volt,3);
	  
  }
  
  lcd_print(2,13,panel_voltage_s[counter],3);      // Checking the value to be exact by printing again.

  _delay_ms(1000);
  servo_1_free();
  
 
} 

void read_sensor()
{
	Left_white_line = ADC_Conversion(3);	//Getting data of Left WL Sensor
	Center_white_line = ADC_Conversion(2);	//Getting data of Center WL Sensor
	Right_white_line = ADC_Conversion(1);	//Getting data of Right WL Sensor
	
/*	print_sensor(1,1,3);	//Prints value of White Line Sensor1
		print_sensor(1,5,2);	//Prints Value of White Line Sensor2
		print_sensor(1,9,1);	//Prints Value of White Line Sensor3
		*/
	lcd_print(1,1,Left_white_line,3);
	lcd_print(1,5,Center_white_line,3);
	lcd_print(1,9,Right_white_line,3);
	
}

int main(void)
{
    cli();
    set_ADC();
    set_lcd();
    sei();

    PORTH |= 0x08; //Turn off the IR sharp sensors.
    PORTG |= 0x04; //Turn off the White line sensors.

    int sensors[5];
    int i;

    while(1)
    {
        for(i=0; i<5; i++)
        {
            sensors[i] = ADC_Conversion(i+4);
            if(i==2)
                lcd_print(1, 7, sensors[i], 4);
            else if(i < 2)
                lcd_print(2, 4*(i) + 1, sensors[i], 3);
            else
                lcd_print(2, 4*(i-1) + 2, sensors[i], 3);
        }

    }

    return 0;
}

void turn()	//turn robo by 180 degree
{
	if (dir == 0 && (ot == 2 || ot == 3))
	{
		velocity(turn_v, turn_v);
		left_degrees(180);
	}
	else if (dir == 2 && (ot == 0 || ot == 1))
	{
		velocity(turn_v, turn_v);
		left_degrees(180);
	}
	else
	{
		velocity(turn_v, turn_v);
		left_degrees(150);
		while (ADC_Conversion(2)<70)
			left();
		stop();
	}
	lcd("turn");
	//_delay_ms(2000);
	dir = (dir + 2) % 4;
	//printf("Turn\n");
	angle += 180;
}