/*
  3pi template code for NYU "Intro to Robotics" course. 
  Max Sobell and Mike Kanisczak 
  This program was modified from an example program from Pololu.  
*/ 
 
// The 3pi include file must be at the beginning of any program that 
// uses the Pololu AVR library and 3pi. 
#include <pololu/3pi.h> 
// Include the header file with sine and cosine tables 
#include "3pi_kinematics.h" 
 
// This include file allows data to be stored in program space.  The 
// ATmega168 has 16k of program space compared to 1k of RAM, so large 
// pieces of static data should be stored in program space. 
#include <avr/pgmspace.h> 
 
// Introductory messages.  The "PROGMEM" identifier  
// causes the data to go into program space. 
const char hello[] PROGMEM = "Dead Reckoner"; 
 
// Data for generating the characters used in load_custom_characters 
// and display_readings.  By reading levels[] starting at various 
// offsets, we can generate all of the 7 extra characters needed for a 
// bargraph.  This is also stored in program space. 
const char levels[] PROGMEM = { 
  0b00000, 
  0b00000, 
  0b00000, 
  0b00000, 
  0b00000, 
  0b00000, 
  0b00000, 
  0b11111, 
  0b11111, 
  0b11111, 
  0b11111, 
  0b11111, 
  0b11111, 
  0b11111 
}; 
 
char display_characters[9] = { ' ', 0, 1, 2, 3, 4, 5, 6, 255 }; 
 
// This function loads custom characters into the LCD.  Up to 8 
// characters can be loaded; we use them for 7 levels of a bar graph. 
void load_custom_characters() 
{ 
  lcd_load_custom_character(levels+0,0); // no offset, e.g. one bar 
  lcd_load_custom_character(levels+1,1); // two bars 
  lcd_load_custom_character(levels+2,2); // etc... 
  lcd_load_custom_character(levels+3,3); 
  lcd_load_custom_character(levels+4,4); 
  lcd_load_custom_character(levels+5,5); 
  lcd_load_custom_character(levels+6,6); 
  clear(); // the LCD must be cleared for the characters to take effect 
} 
 
// This function displays the sensor readings using a bar graph. 
void display_bars(const unsigned int *s, const unsigned int *minv, const unsigned int* maxv) { 
  // Initialize the array of characters that we will use for the 
  // graph.  Using the space, and character 255 (a full black box). 
  unsigned char i; 
  for (i=0;i<5;i++) { 
    int c = ((int)s[i]-(int)minv[i])*9/((int)maxv[i]-(int)minv[i]); 
    c = (c<0)?0:(c>8)?8:c; 
    // if (i==0) {print_long(s[0]); print_long(c); } 
    print_character(display_characters[c]); 
  } 
} 
 
void update_bounds(const unsigned int *s, unsigned int *minv, unsigned int *maxv) { 
  int i; 
  for (i=0; i<5; i++) {  
    if (s[i]<minv[i]) minv[i] = s[i]; 
    if (s[i]>maxv[i]) maxv[i] = s[i]; 
  } 
} 
 
// return line position 
// YOU MUST WRITE THIS. 
int line_position(unsigned int *s, unsigned int *minv, unsigned int *maxv) { 
  int i, j, total, wsum;	//counters, total and weighted total 
  wsum = 0; 
  total = 0; 
  int sums[5]; 
  for(i = 0; i < 5; i++) { 
    sums[i] = (10*((int)s[i]-(int)minv[i]))/((int)maxv[i]-(int)minv[i]);	//sums 0-10 
    total += sums[i];					//total 0-50	 
  } 
  j=-1000; 
  for(i = 0; i < 5; i++) { 
    wsum += j*sums[i]; 			//weighted sum 
    j += 500;						//j -1000 to 1000 
  } 
  return wsum/total; 
} 
 
int check_for_line(unsigned int *s, unsigned int *minv, unsigned int *maxv) { 
  int i, j, isline; 
  isline = 1; 
  int sums[5]; 
  j = 0; 
  for(i = 0; i < 5; i++) { 
    sums[i] = ( 10*( (int)s[i]-(int)minv[i] ) )/( (int)maxv[i]-(int)minv[i] );	//sums 0-10 
    if( sums[i] <= 1 ) { j++; }	//check each sensor to see if theres still a line 
  } 
  if(j == 5) { isline = 0; }	//there is no line on all 5 sensors 
  return isline; 
} 
// Initializes the 3pi, displays a welcome message, calibrates, and 
// plays the initial music. 
void initialize() 
{ 
  // This must be called at the beginning of 3pi code, to set up the 
  // sensors.  We use a value of 2000 for the timeout, which 
  // corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor. 
  pololu_3pi_init(2000); 
  load_custom_characters(); // load the custom characters 
	 
  // display message 
  print_from_program_space(hello); 
  lcd_goto_xy(0,1); 
  print("Press B"); 
	 
  // wait for B button to be pressed 
  while(!button_is_pressed(BUTTON_B)) { delay(100); }; 
  // wait for 1second after button is pressed 
  delay(1000); 
} 
 
// For dead reckoning 
// ml = left motor speed 
// mr = right motor speed 
// dt = 5 ms 
// *x, *y, *theta -- this function should update, they are pointers to heading variables) 
void update_position(int ml, int mr, long dt, long *x, long *y, long *theta) { 
  *theta += ( dt * motor2angle(ml, mr) );			//c2 averages, c1000 converts into seconds 
  *x += ( dt * Sin(*theta/c1000) * motor2speed( (ml + mr)/c2 ) )/cmillion;	//c1000 and c1000 normalize to degrees and seconds 
  *y += ( dt * Cos(*theta/c1000) * motor2speed( (ml + mr)/c2 ) )/cmillion;
} 
 
 
// This is the main function, where the code starts.  All C programs 
// must have a main() function defined somewhere. 
int main() 
{ 
  // global array to hold sensor values 
  unsigned int sensors[5];  
  // global arrays to hold min and max sensor values 
  // for calibration 
  unsigned int minv[5], maxv[5];  
  // line position relative to center 
  unsigned int run; 
  int position[2]; 
  long time[2]; 
  int val[2]; 
  int i, speed, counter, d, dcoeff, integ, icoeff, pcoeff, pc, pause, isline, first_time, dv; 
  int ml, mr;	//motor left and motor right 
  long dt, x, y, theta; 
  int return_started, theta_done, rot_speed; 
  return_started = 0; 
  theta_done = 0; 
	 
  // set up the 3pi, and wait for B button to be pressed 
  initialize(); 
	 
  read_line_sensors(sensors,IR_EMITTERS_ON); 
  for (i=0; i<5; i++) { minv[i] = maxv[i] = sensors[i]; } 
	 
  // make a dance left and right 
  // dance(); 
	 
  // Display calibrated sensor values as a bar graph. 
  theta = 0; 
  x = 0; 
  y = 0; 
  time[0] = millis(); 
  val[0] = 0; 
  first_time = 1; 
  isline = 1;	//assume we're starting on a line 
  run = 0; 
  speed = 35; 
  pcoeff = 12;	//lower number is more sensitive 
  dcoeff = 30; 
  icoeff = 50; 
  counter = 0; 
  while(1) { 
    if (button_is_pressed(BUTTON_B)) { run = 1-run; delay_ms(200); } 
    if (button_is_pressed(BUTTON_A)) { speed -= 10; delay_ms(100); } 
    if (button_is_pressed(BUTTON_C)) { speed += 10; delay_ms(100); } 
    // Read the line sensor values 
    read_line_sensors(sensors,IR_EMITTERS_ON); 
    // update minv and mav values, 
    // and put normalized values in v 
    update_bounds(sensors,minv,maxv); 
		 
    if( first_time ) { 
      position[1] = position[0]; 
      set_motors(40,-40); 
      for(counter=0;counter<500;counter++) { 
	delay_ms(1); 
	update_bounds(sensors,minv,maxv);	//calibrate the sensors 
      } 
      for(counter=0;counter<2000;counter++) { 
	if(counter < 500 || counter >= 1500) { 
	  set_motors(30,30); 
	  delay_ms(1); 
	  update_bounds(sensors,minv,maxv);	//calibrate the sensors 
	} 
	else { 
	  set_motors(-30,-30); 
	  delay_ms(1); 
	  update_bounds(sensors,minv,maxv);	//calibrate the sensors 
	} 
      } 
      set_motors(-40,40); 
      for(counter=0;counter<500;counter++) { 
	delay_ms(1); 
	update_bounds(sensors,minv,maxv); 
      } 
      set_motors(0,0);	//stop calibrating 
      first_time = 0; 
      isline = 1; 
      counter = 0; 
      return_started = 0; 
    } 
		 
    //compute line positon 
    position[1] = position[0]; 
    position[0] = line_position(sensors,minv,maxv);	 
    //derivative 
    d = position[0] - position[1]; 
    //integral 
    integ = position[0] + position[1]; 
    counter++; 
    if( !return_started ) 
      isline = check_for_line(sensors,minv,maxv); 
    else 
      isline = 0; 
    if( run ) {	
      if( isline ) { 
	val[1] = val[0]; 
	val[0] = position[0]/pcoeff + d/dcoeff + integ/icoeff; 
	dv = val[0] - val[1]; 
	if( dv > 2 ) { 
	  dv = 2; 
	  //speed--; 
	} 
	if( dv < -2 ) { 
	  dv = -2; 
	  //speed--; 
	} 
	if( (val[0] < 5) && (val[0] > -5) ) 
	  val[0] = 0;	//dead band 
	if(speed < 35) { 
	  if( (counter%100) == 0 ) speed++; 	//increment the speed every 5 cycles or approx 13*100 ms = 1300 ms = 1.3 seconds to get to top speed
	} 
	ml = speed + val[0]; 
	mr = speed - val[0]; 
	counter %= 100; 
      } else { 
	if(!return_started){ 
	  rot_speed = 40;				 
	  return_started = 1; 
	  theta_done = 0;
	  set_motors(0,0);
	  pause = 10;
	} 
	clear(); 
	print_long(theta_done);						
	//first set theta 
	if(return_started && !theta_done ) { 
	  clear();
	  print_long(theta/c1000);
	  rot_speed = 40; 
	  if( (theta/c1000) > (c360/c2) ) {
	    while ( (theta/c1000) > (c360/c2) ) {
	      print_long(11111111);						
	      ml = -rot_speed; 
	      mr = rot_speed; 
	      set_motors(ml, mr);
	      //delay_ms( pause );
	      time[1] = time[0]; 
	      time[0] = millis(); 
	      dt = time[0]-time[1]; 
	      update_position(ml, mr, (long)dt, &x, &y, &theta);
	      delay_ms(1);
	    } 
	  }
	  else if( (theta/c1000) < (c360/c2) ) {
	    while ( (theta/c1000) < (c360/c2) ) {
	      ml = rot_speed; 
	      mr = -rot_speed;
	      set_motors(ml, mr);
	      //delay_ms( pause );
	      time[1] = time[0]; 
	      time[0] = millis(); 
	      dt = time[0]-time[1]; 
	      update_position(ml, mr, (long)dt, &x, &y, &theta); 
	      clear();
	      print_long(theta/c1000);
	      delay_ms(10);
	    }
	  }
/* 	  if( (theta > c179) && (theta < c181) ) {  */
/* 	    ml = 0;  */
/* 	    mr = 0;  */
/* 	    theta_done = 1; */
/* 	    set_motors(ml, mr); */
/* 	    //delay_ms( pause ); */
/* 	    time[1] = time[0];  */
/* 	    time[0] = millis();  */
/* 	    dt = time[0]-time[1];  */
/* 	    update_position(ml, mr, (long)dt, &x, &y, &theta);   */
/* 	  }  */
	  set_motors(0,0);
	  theta_done = 1;
	}
	if (return_started && theta_done) {
	  clear();
	  print_long(y);
	  //	  delay_ms(1000);
	  while(y>0) {  
	    lcd_goto_xy(0,0); 
	    print_long(y);
	    ml = rot_speed; 
	    mr = rot_speed; 
	    set_motors(ml, mr); 
	    time[1] = time[0]; 
	    time[0] = millis(); 
	    dt = time[0]-time[1]; 
	    update_position(ml, mr, (long)dt, &x, &y, &theta); 
	    delay_ms(10);
	  }
	  clear();
	  print_long(5566);
	  //	  delay_ms(1000);
	  //	  time[0] = millis(); //ignore the long delay
	  while( (theta/c1000) < (c180 + c90) ) { //you should mess with the 90 on this line, im not sure its right 
	    lcd_goto_xy(0,0); 
	    print_long(66666);
	    ml = rot_speed; 
	    mr = -rot_speed; 
	    set_motors(ml, mr); 
	    time[1] = time[0]; 
	    time[0] = millis(); 
	    dt = time[0]-time[1]; 
	    update_position(ml, mr, (long)dt, &x, &y, &theta); 
	    delay_ms(10);
	  } 
	  clear();
	  print_long(6677);
	  //	  delay_ms(1000);
	  clear();
	  print_long(x);
	  //	  delay_ms(1000);
	  while(x>0) { 
	    lcd_goto_xy(0,0); 
	    print_long(x);
	    ml = rot_speed; 
	    mr = rot_speed; 
	    set_motors(ml, mr); 
	    time[1] = time[0]; 
	    time[0] = millis(); 
	    dt = time[0]-time[1]; 
	    update_position(ml, mr, (long)dt, &x, &y, &theta); 
	    delay_ms(10);
	  } 
	  ml = 0; 
	  mr = 0; 
	  set_motors(ml, mr); 
	  print_long(88888);
	  delay_ms(1000);
	  //we should be there now, so stop. 
	}
      } 
    } else { 
      ml = 0; 
      mr = 0;
      set_motors(ml, mr);
    } 
		
    if(!return_started){ 
      time[1] = time[0]; 
      time[0] = millis(); 
      dt = time[0]-time[1]; 
      update_position(ml, mr, (long)dt, &x, &y, &theta); 
      set_motors(ml, mr); 
      clear();
      print_long(x);
    } 
		
    // display bargraph 
    //clear(); 
    //print_long(theta); 
    //print_long(y); 
    //print_long(motor2speed(30));		 
    //lcd_goto_xy(0,1); 
    // for (i=0; i<8; i++) { print_character(display_characters[i]); } 
    //display_bars(sensors,minv,maxv); 
    delay_ms(5); 
  } 
}