/*
last move:
1 - forward center
2 - forward left
3 - forward right
turns are 100ms steps are 400ms
*/

#define RAND_MAX 255
#include <16F876A.h>
#device adc=10
#use delay(clock=4000000)
#fuses XT,NOWDT,PUT,NOPROTECT,NODEBUG,NOBROWNOUT,NOLVP,NODEBUG,NOWRT
#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=9)
#include <STDLIB.H> //for rand function

#define  enable2 PIN_C1 //CCP2
#define  enable1 PIN_C2 //CCP1
#define  led2    PIN_C3
#define  led1    PIN_C4
#define  led3    PIN_C5

#define  whisker PIN_B0
#define  whisker_left PIN_B1
#define  dira1   PIN_B2
#define  dirb1   PIN_B3
#define  whisker_right PIN_B4
#define  dira2   PIN_B5
#define  dirb2   PIN_B6

//functions
void go_back();
void forward();
void backward();
void left();
void right();
void stop();
void go();
//globals
int rand_num;
int mode=0;
int last_move=0;
long a2dval;
float light1;
float light2;
float light3;
float light_average;


//RBO ISR ////////////////////////////////////////
#int_ext
void int_ext_isr(){
         output_high(led3);
         stop();
         //if in mode 1 and see companion just inch back
         if (mode==1){
            set_adc_channel(1);
            delay_ms(200); //wait TACQ
            a2dval = read_adc();
            printf("a2d:%lU\r\n ",a2dval) ;
            light1 = (a2dval*5.0)/1024;
            printf("light1: %f\r\n ",light1) ;
            if (light1<=(light_average-.15)){
               backward();
               delay_ms(100);
               stop();
            }
            else go_back();
         }
         else{
            rand_num=rand();
            if (rand_num>127){
               while(! input(whisker)){
                  printf("back\r\n");
                  backward();
                  delay_ms(800);
                  stop();
               }
               printf("right\r\n");
               right();
               delay_ms(500);
               stop();
            }
            else{
               while(! input(whisker)){
                  printf("back\r\n");
                  backward();
                  delay_ms(800);
                  stop();
               }
               printf("left\r\n");
               left();
               delay_ms(500);
               stop();
            }
         }
         output_low(led3);
}
/////////////////////////////////////////////////
void go_back(){
   stop();
   //reverse last step
   /*
   last move:
   1 - forward center
   2 - forward left
   3 - forward right
   */
   if (last_move==1){
      backward();
      delay_ms(400);
      stop();
   }
   if (last_move==2){
      backward();
      delay_ms(400);
      stop();
      delay_ms(10);
      right();
      delay_ms(100);
      stop();
      delay_ms(10);
   }
   if (last_move==3){
      backward();
      delay_ms(400);
      stop();
      delay_ms(10);
      left();
      delay_ms(100);
      stop();
      delay_ms(10);
   }
}
void go(){
   int dir;
   dir = rand();
   printf("rand move\r\n");
   if (dir>127){
      printf("left\r\n");
      left();
      delay_ms(100);
      stop();
      delay_ms(10);
      //now forward
      forward();
      delay_ms(400);
      stop();
      delay_ms(1000);
      last_move = 2;
   }
   else{
      printf("right\r\n");
      right();
      delay_ms(100);
      stop();
      delay_ms(10);
      //now forward
      forward();
      delay_ms(400);
      stop();
      delay_ms(1000);
      last_move = 3;
   }
}

void forward(){
   output_low(dira1);
   output_high(dirb1);
   output_low(dira2);
   output_high(dirb2);

   output_high(enable1);
   output_high(enable2);
}
void backward(){
   output_high(dira1);
   output_low(dirb1);
   output_high(dira2);
   output_low(dirb2);

   output_high(enable1);
   output_high(enable2);
}
void left(){
   output_high(dira1);
   output_low(dirb1);
   output_low(dira2);
   output_high(dirb2);

   output_high(enable1);
   output_high(enable2);
}
void right(){
   output_low(dira1);
   output_high(dirb1);
   output_high(dira2);
   output_low(dirb2);

   output_high(enable1);
   output_high(enable2);
}
void stop(){
   output_low(enable1);
   output_low(enable2);
}

void sense(){
      short mate;
      int i;
      //light center
      delay_ms(1000);
      a2dval = read_adc();
      printf("a2d:%lU\r\n ",a2dval) ;
      light1 = (a2dval*5.0)/1024;
      printf("light1: %f\r\n ",light1) ;
      if (light1<=(light_average-.15)){
         //led spotted
         printf("led spotted\r\n");
         output_high(led1);
         mode = 1;
         return;
      }

      //turn left
      left();
      delay_ms(100);
      stop();
      delay_ms(1000);
      //light left
      a2dval = read_adc();
      printf("a2d:%lU\r\n ",a2dval) ;
      light2 = (a2dval*5.0)/1024;
      printf("light2: %f\r\n ",light2) ;
      if (light2<=(light_average-.15)){
         //led spotted
         printf("led spotted\r\n");
         output_high(led1);
         mode = 1;
         return;
      }

      //turn right
      right();
      delay_ms(200);
      stop();
      delay_ms(1000);
      //light right
      a2dval = read_adc();
      printf("a2d:%lU\r\n ",a2dval) ;
      light3 = (a2dval*5.0)/1024;
      printf("light3: %f\r\n ",light3) ;
      if (light3<=(light_average-.15)){
         //led spotted
         printf("led spotted\r\n");
         output_high(led1);
         mode = 1;
         return;
      }
}
void compare(){
      //now compare measurements
      //first light
      if ((light1<light2)&&(light1<light3)){
         //turn back left
         printf("left\r\n");
         left();
         delay_ms(200);
         stop();
         delay_ms(10);
         forward();
         delay_ms(400);
         stop();
         delay_ms(1000);
         last_move = 2;
      }
      else if ((light2<light1)&&(light2<light3)){
         //turn center
         printf("center\r\n");
         left();
         delay_ms(100);
         stop();
         delay_ms(10);
         //1 left turn
         forward();
         delay_ms(400);
         stop();
         delay_ms(1000);
         last_move = 1;
      }
      else if ((light3<light1)&&(light3<light2)){
         //stay right
         printf("right\r\n");
         forward();
         delay_ms(400);
         stop();
         delay_ms(1000);
         last_move = 3;
      }
      else {
         //all pretty ==, take a random walk
         go();
      }
}
float calibrate_light(){
      int i;
      float light_temp;
      float average_light;
      average_light=0;
      set_adc_channel(1);
      delay_ms(200); //wait TACQ
      for (i=0;i<10;i++){
         a2dval = read_adc();
         printf("a2d:%lU\r\n ",a2dval) ;
         light_temp = (a2dval*5.0)/1024;
         average_light=average_light+light_temp;
      }
      average_light=average_light/10;
      printf("average_light:%f\r\n", average_light);
      return average_light;
}
short slow_circle(){
   int i;
   for (i=10;i>0;i--){
      right();
      delay_ms(100);
      stop();
      a2dval = read_adc();
      printf("a2d:%lU\r\n ",a2dval) ;
      light3 = (a2dval*5.0)/1024;
      printf("light3: %f\r\n ",light3) ;
      if (light3<=(light_average-.15)){
         //light found again, inch towards
         forward();
         delay_ms(100);
         stop();
         return 1;
      }
      delay_ms(500);
   }
   return 0;
}

///////////////////////////////////////////////

void main(){
   //setup
   short found;
   int i=60;
   signed long xsteps=0;
   signed long ysteps=0;
   long xheading=0;
   long yheading=0;
   setup_adc_ports(ALL_ANALOG);
   setup_adc(ADC_CLOCK_INTERNAL);
   set_adc_channel(0);
   setup_counters(RTCC_INTERNAL,RTCC_DIV_256);
   //INTS_PER_SECOND 19     // (4000000/(4*256*256)) = 4000000/ 262144 = 18.8
   set_tris_b(0b00000001);
   set_tris_c(0b10000000);
   ext_int_edge (L_TO_H);
   enable_interrupts(global);
   enable_interrupts(int_ext);
   srand(read_adc());
   output_low(led3);
   //go
   output_high(led1); //alive
   output_high(led2);
   stop();
   printf("\r\nhi\r\n") ;
   set_adc_channel(1);
   delay_ms(2000); //wait to settle
   light_average = calibrate_light();
   output_low(led1);
   while (true){
      printf("mode:%u\r\n", mode);
      if (mode==0){
         output_low(led1);
         sense();
         }
      if (mode==0){
         compare();
         }
      if (mode==1){
         delay_ms(1000);
         //make random move
         go();
         //check for companion
         a2dval = read_adc();
         printf("a2d:%lU\r\n ",a2dval) ;
         light1 = (a2dval*5.0)/1024;
         printf("light: %f\r\n ",light1) ;
         if (light1<=(light_average-.15)){
            //companion still there
            delay_ms(1000);
            printf("companion still here\r\n");
         }
         else {
            //companion lost
            printf("companion lost, going back\r\n");
            go_back();
            delay_ms(2000);
            //check again
            a2dval = read_adc();
            printf("a2d:%lU\r\n ",a2dval) ;
            light1 = (a2dval*5.0)/1024;
            printf("light: %f\r\n ",light1) ;
            if (light1<=(light_average-.15)){
               //companion found!
               printf("companion found\r\n");
               delay_ms(1000);
            }
            else {
               printf("companion still lost circling\r\n");
               delay_ms(1000);
               found = slow_circle();
               if (! found){
                  //companion really lost
                  printf("companion totally lost\r\n");
                  delay_ms(1000);
                  mode = 0;
               }
               else {
                  printf("companion found after circle\r\n");
               }
            }
         }
      }
   }//end while true
}//end main