/*
LEDS:
1. little red (on board) - alive
2. red - command
3. blue - song
4. green - dance
blue&red - virgil
blue & green - prox detect

COMMANDS:
'a' - m1 left
'b' - m1 right
'c' - m1 stop
'e' - m2 left
'f' - m2 right
'g' - m2 stop
'i' - m3 left
'j' - m3 right
'k' - m3 stop
go forward - a, e
go backward - b, f
go up - j
go down - i
turn right - b, e
turn left - a, f

*/
//ra0 = value1 = front
//ra1 = value2 = top
//ra3 = value3 = bottom --> switch?

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

//pins ra0, ra1, ra3  for proximity sensing
#define  sonar    PIN_B0  //capacitive sensor tied to balloon

#define  led4    PIN_A2 //dancing - yellow
//pin a4 open drain
#define  enable1 PIN_A5


#define  dira1   PIN_B1
#define  dirb1   PIN_B2
#define  dira2   PIN_B3
#define  dirb2   PIN_B4
#define  dira3   PIN_B5
#define  dirb3   PIN_B6
#define  enable3 PIN_B7

#define  enable2 PIN_C0
//RC2 - CCP1 for sound

#define  led3    PIN_C3 // singing-blue
#define  led1    PIN_C4  //alive-green
#define  led2    PIN_C5  //data received-red
//RC6 &RC7 UART
//proximity blue & yellow

long  duty1=0;
char cmd;
short new=0; //flag
//store analog vals from prox sensors
int value1;
int value2;
int value3;
int clear_flag;
int seed;

void stop();
void motor1_right();
void motor2_right();
void motor1_left();
void motor2_left();

#int_ext
void int_ext_isr(){
   int rand_num;
   stop();
   while(! input(sonar)){
      printf("back\r\n");
      motor1_right();
      motor2_right(); //backward - b, f m1 right, m2 right
    }
    stop();
    rand_num=rand();
    if (rand_num>127){
      motor1_left();
      motor2_right(); //left - a, f m1 left, m2 right
    }
    else{
      motor1_right();
      motor2_left(); //right - b, e m1 right, m2 left
    }
    delay_ms(500);
    stop();
}

#int_rda
void serial_isr() {
   cmd=getc();
   putc(cmd);
   new=1;
}

////////////////////////////////////////////////////
void motor1_right(){
//turn motor1 right
      output_high(enable1);
      output_high(dira1);
      output_low(dirb1);
}
void motor1_left(){
//turn motor1 left
      output_high(enable1);
      output_low(dira1);
      output_high(dirb1);
}
void motor1_stop(){
//stop motor1
      output_low(enable1);
}
void motor2_right(){
//turn motor1 right
      output_high(enable2);
      output_high(dira2);
      output_low(dirb2);
}
void motor2_left(){
//turn motor1 left
      output_high(enable2);
      output_low(dira2);
      output_high(dirb2);
}
void motor2_stop(){
//stop motor1
      output_low(enable2);
}
void motor3_right(){
//turn motor1 right
      output_high(enable3);
      output_high(dira3);
      output_low(dirb3);
}
void motor3_left(){
//turn motor1 left
      output_high(enable3);
      output_low(dira3);
      output_high(dirb3);
}
void motor3_stop(){
//stop motor1
      output_low(enable3);
}
void stop(){
   output_low(enable1);
   output_low(enable2);
   output_low(enable3);
}

void set_motors(){
//enter command mode until cmd 'z' sent
   char instruction;
   int i;
   output_high(led2);
   while (cmd!='z'){
      if (new){
         instruction = cmd;
         if (instruction=='a'){
               motor1_left();
               putc(instruction);
               //send_data(instruction, notesa);
               }
         else if (instruction== 'b'){
               motor1_right();
               putc(instruction);
               //send_data(instruction, notesb);
               }
         else if (instruction== 'c'){
               motor1_stop();
               putc(instruction);
               //send_data(instruction, notesc);
               }
         else if (instruction== 'e'){
               motor2_left();
               putc(instruction);
               //send_data(instruction, notese);
               }
         else if (instruction== 'f'){
               motor2_right();
               putc(instruction);
               //send_data(instruction, notesf);
               }
         else if (instruction== 'g'){
               motor2_stop();
               putc(instruction);
               //send_data(instruction, notesg);
               }
         else if (instruction== 'i'){//blow air up
               motor3_left();
               putc(instruction);
               //send_data(instruction, notesi);
               }
         else if (instruction== 'j'){//blow air down
               motor3_right();
               putc(instruction);
               //send_data(instruction, notesj);
               }
         else if (instruction== 'k'){
               motor3_stop();
               putc(instruction);
               //send_data(instruction, notesk);
               }
         else if (instruction== 'm'){
               setup_ccp1(CCP_PWM);
               putc(instruction);
               printf("VP\r\n");
               output_high(led3);
               for (i=100;i<500;i=i+75) {
                  set_pwm1_duty(i);
                  if (new)
                     break; //exit if command entered
                  delay_ms(300);
               }
               set_pwm1_duty(0);
               setup_ccp1(CCP_OFF);
               output_low(led3);
               }
         else  {
               putc(instruction);
               break;
               }
         new=0;
      }//end if new

   } //end while !z
   output_low(led2);

}

void play_sound(){
   int i;
   long rand_note;
   setup_ccp1(CCP_PWM);
   printf("sound\r\n");
   output_high(led3);
   for (i=0;i<6;i++) {
      rand_note = rand();
      printf("rand:%lu\r\n",rand_note);
      rand_note = rand_note*2;
      set_pwm1_duty(rand_note);
      if (new)
         break; //exit if command entered
      delay_ms(300);
   }
   set_pwm1_duty(0);
   setup_ccp1(CCP_OFF);
   output_low(led3);
}

void blink(){
   //500 * 3 * max 255us =
   int i;
   int rand_delay;
   printf("blink\r\n");
   for (i=0;i<5;i++){
      rand_delay = rand();
      printf("rand:%U\r\n",rand_delay);
      if (new)
         break; //exit if command entered
      output_high(led2);
      delay_ms(rand_delay);
      output_low(led2);
      rand_delay = rand();
      printf("rand:%U\r\n",rand_delay);
      if (new)
         break; //exit if command enter
      output_high(led3);
      delay_ms(rand_delay);
      output_low(led3);
      rand_delay = rand();
      printf("rand:%U\r\n",rand_delay);
      if (new)
         break; //exit if command entered
      output_high(led4);
      delay_ms(rand_delay);
      output_low(led4);
   }
}

void dance(){
   //4 safe dance moves: circle right, circle left, up, down
   //perform a move, check prox, perform a move... 3 times?
   int moves[3];
   int move;
   int m;
   printf("dance\r\n");
   output_high(led4);
   moves[0] = rand();
   moves[1] = rand();
   moves[2] = rand();
   for (m=0;m<3;m++){
      move = moves[m];
      if (new)
         break; //exit if command entered
      if (move <64) {
         //circle_right
         motor1_right();
         motor2_left();
         delay_ms(3000);
         motor1_stop();
         motor2_stop();
      }
      else if ((move>=64)&&(move<128)){
         //circle_left;
         motor1_left();
         motor2_right();
         delay_ms(3000);
         motor1_stop();
         motor2_stop();
      }
      else if ((move>=128)&&(move<192)){
         //up()
         motor3_right();
         delay_ms(2000);
         motor3_stop();
      }
      else{
         //down;
         motor3_left();
         delay_ms(2000);
         motor3_stop();
      }
      if (new){
         break; //exit if command entered
      }
   }
   output_low(led4);
}


void main() {
   int rand_int;
   int times;
   //not used
   setup_spi(FALSE);
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
   setup_counters(RTCC_INTERNAL,RTCC_DIV_2);
   setup_timer_1(T1_DISABLED);

   //CCP for sound
   setup_timer_2(T2_DIV_BY_16,255,1);
   //prescale=16, period = 255, postscale=1
   //cycle time will be (1/clock)*4*t2div*(period+1)
   //w/T2_DIV = 1:                            = .0000512  = 19,531.25 Hz
   //w/T@_DIV = 4:   (1/20000000)*4*4*(255+1) = .00002048 = 4,882.8125 Hz
   //w/T2_DIV = 16:                           = .0008192  = 1220.7 Hz
   //Duty cycle res =   log(fosc/fpwm)
	//                	----------------  bits
	//	                  log(2)
   //               =   2.91/.301 = 9.66 bits = 511 decimal

   setup_ccp1(CCP_PWM);
   set_pwm1_duty(duty1); //start with duty cycle full off
   setup_ccp2(CCP_OFF);
   set_tris_a(0b00001011); //analog ins on ra0,1 &3
   set_tris_c(0b10000000); //if want serial port, rc7 = rx
   set_tris_b(0b00000001); //all outputs except B0

   enable_interrupts(global);
   enable_interrupts(int_ext);
   enable_interrupts(int_rda);


   printf("hi\r\n");

   output_high(led1);

   //show all alive
   output_high(led4);
   output_high(led2);
   output_high(led3);
   set_pwm1_duty(500);

   motor3_left(); //down
   motor1_left();
   motor2_left();//all left
   delay_ms(500);
   set_pwm1_duty(250);
   delay_ms(500);
   set_pwm1_duty(127);
   delay_ms(500);

   set_pwm1_duty(67);
   motor3_right(); //up
   motor1_right();
   motor2_right();//all right
   delay_ms(500);
   set_pwm1_duty(33);
   delay_ms(500);
   motor3_stop();
   motor1_stop();
   motor2_stop();//stop

   set_pwm1_duty(0);
   setup_ccp1(CCP_OFF);
   output_low(led4);
   output_low(led2);
   output_low(led3);
   set_adc_channel(0);
   delay_us(20); //wait TACQ
   seed = read_adc();
   printf("rand seed:%U\r\n ",seed) ;


   while(true){
      //---------- listen for commands -----------------------
      if (new){
         set_motors();
         new=0;
      }
      else{
         output_low(led2);
      }
      set_pwm1_duty(0);

      //---------- once path clear listen for commands -----------------------
      //randomly decide to play sound, dance, blink leds in pattern, or do nothing

      rand_int = rand();
      printf("rand:%U\r\n",rand_int);
      if (rand_int <10){
         play_sound();
      }
      else if ((rand_int>50)&&(rand_int<60)){
         dance();
      }
      else if((rand_int>90)&&(rand_int<100)){
         blink();
      }

      //else do nothing, float
  } //end while true
} //end main