#include <SoftwareSerial.h>
#include <Servo.h>

Servo servos[13];
#include <wiring_private.h>
#include <pins_arduino.h>

/***   Global variables   ***/
SoftwareSerial _bt_softwareSerial((3),(2));
int _white=800;
int _black=30;
int _bt_cmd=0;

/***   Function declaration   ***/
void move_base(int l, int r, int speed, int advance, int turn);
void LEDMatrix_init(int cs, int din, int clk);
void writeRow(int cs, int din, int clk, int row, int data);
void maxAll (int cs, int din, int clk, int reg, int col);
void putByte (int din, int clk, int data);
void expression(int cs, int din, int clk, int col1, int col2, int col3, int col4, int col5, int col6, int col7, int col8);
long playMelody(int pin, String melody);
int follow(int light, int gain);
unsigned long _pulseIn(uint8_t pin, uint8_t state, unsigned long timeout);
long US_init(int trigger_pin, int echo_pin);
long distance(int trigger_pin, int echo_pin);

/***   Tasks declaration   ***/

/***   ISR function declaration   ***/

void setup()
{



    _bt_softwareSerial.begin(9600);
  _bt_softwareSerial.flush();

  servos[(5)].attach((5));
  servos[(9)].attach((9));
  servos[(6)].attach((6));
  servos[(11)].attach((11));
  pinMode((12),OUTPUT);
  pinMode((13),OUTPUT);
  pinMode((10),OUTPUT);

  maxAll((12),(13),(10),11,7);
  maxAll((12),(13),(10),9,0);
  maxAll((12),(13),(10),12,1);
  maxAll((12),(13),(10),15,0);
  int i=0;
  for (i = 1; i <= 8; i++)
    maxAll((12),(13),(10),i,0);
  maxAll((12),(13),(10),10,15);

    move_base((5),(9),0,0,0);
  _white=300;
  _black=700;
  servos[(6)].write(90);
  servos[(11)].write(90);
  expression((12),(13),(10),192,48,12,3,3,12,48,192);
  playMelody((7),String("1568,562,1047,562,1245,93,1397,93,1568,375,1047,375,1245,93,1397,93,1175,1875,1397,562,932,562,1245,93,1175,93,1397,375,932,562,1245,93,1175,93,1047,750;"));

  pinMode( (4) , INPUT );
  pinMode( (8) , OUTPUT );
}


void loop()
{
    if (_bt_softwareSerial.available()>0  || _bt_cmd>0)
    {
      int cmd=_bt_softwareSerial.read();

      if ((cmd==3)||(_bt_cmd==3)){
            move_base((5),(9),0,0,0);
        expression((12),(13),(10),129,66,36,24,24,36,66,129);
        delay(20);
        servos[(6)].write(30);
        servos[(11)].write(145);
        }
      if ((cmd==4)||(_bt_cmd==4)){
            move_base((5),(9),100,100,0);
        servos[(6)].write(165);
        servos[(11)].write(25);
        expression((12),(13),(10),16,32,64,255,255,64,32,16);
        delay(20);
        }
      if ((cmd==5)||(_bt_cmd==5)){
            move_base((5),(9),100,-100,0);
        servos[(6)].write(30);
        servos[(11)].write(145);
        expression((12),(13),(10),8,4,2,255,255,2,4,8);
        delay(20);
        }
      if ((cmd==6)||(_bt_cmd==6)){
            move_base((5),(9),25,0,-100);
        servos[(6)].write(90);
        servos[(11)].write(25);
        expression((12),(13),(10),24,24,24,24,24,90,60,24);
        delay(20);
        }
      if ((cmd==7)||(_bt_cmd==7)){
            move_base((5),(9),25,0,100);
        servos[(6)].write(165);
        servos[(11)].write(90);
        expression((12),(13),(10),24,60,90,24,24,24,24,24);
        delay(20);
        }
      if ((cmd==8)||(_bt_cmd==8)){
           do{

        expression((12),(13),(10),128,134,134,153,153,121,97,1);
        move_base((5),(9),25,75,follow(analogRead((A0)),10));
        delay(20);
        }while(_bt_softwareSerial.read()!=3);}
      if ((cmd==9)||(_bt_cmd==9)){
       do{     if (distance((8),(4))<(30)){
          move_base((5),(9),100,(-75),10);
          servos[(6)].write(30);
          servos[(11)].write(145);
          expression((12),(13),(10),129,66,36,24,24,36,66,129);
          delay(20);
          tone((7),800,500);
          delay(500);


        }
        else
        {
          move_base((5),(9),100,50,25);
          servos[(6)].write(165); //subir brazos
          servos[(11)].write(25);
          expression((12),(13),(10),16,32,64,255,255,64,32,16);
          delay(20);
          tone((7),1000,500);
          delay(500);

        }delay(60);
        }while(_bt_softwareSerial.read()!=3);}

        if ((cmd==10)||(_bt_cmd==10)){
        servos[(6)].write(30); //bajar brazo izq
        
        }

        if ((cmd==11)||(_bt_cmd==11)){
        servos[(11)].write(145); //bajar brazo derecho
        }

        if ((cmd==12)||(_bt_cmd==12)){
        servos[(6)].write(165); //subir brazo izq
        }

        if ((cmd==13)||(_bt_cmd==13)){
        servos[(11)].write(25); //subir brazo derecho
        }}

}

/***   Function definition   ***/
void move_base(int l, int r, int speed, int advance, int turn)
{
servos[l].write(90+(int)(65.0*(float)(speed*(advance-turn))/10000.0));
servos[r].write(90-(int)(65.0*(float)(speed*(advance+turn))/10000.0));
};
void LEDMatrix_init(int cs, int din, int clk) {
maxAll(cs,din,clk,11,7);
maxAll(cs,din,clk,9,0);
maxAll(cs,din,clk,12,1);
maxAll(cs,din,clk,15,0);
int i=0;
for (i = 1; i <= 8; i++) {
maxAll(cs,din,clk,i, 0);
}
maxAll(cs,din,clk,10,15);
}
void writeRow(int cs, int din, int clk, int row, int data) {
digitalWrite(cs,LOW);
putByte(din,clk,row);
putByte(din,clk,data);
digitalWrite(cs,LOW);
digitalWrite(cs,HIGH);
}
void maxAll (int cs, int din, int clk, int reg, int col) {
digitalWrite(cs,LOW);
putByte(din,clk,reg);
putByte(din,clk,col);
digitalWrite(cs,LOW);
digitalWrite(cs,HIGH);
}
void putByte (int din, int clk, int data) {
byte i = 8;
byte mask;
while(i > 0) {mask = 0x01 << (i - 1);
    digitalWrite(clk,LOW);
    if (data & mask)
{
      digitalWrite(din,HIGH);
    }else{
      digitalWrite(din,LOW);
    }
    digitalWrite(clk,HIGH);
    --i;
}
}
void expression(int cs, int din, int clk, int col1, int col2, int col3, int col4, int col5, int col6, int col7, int col8) {
writeRow(cs,din,clk,1, col1);
writeRow(cs,din,clk,2, col2);
writeRow(cs,din,clk,3, col3);
writeRow(cs,din,clk,4, col4);
writeRow(cs,din,clk,5, col5);
writeRow(cs,din,clk,6, col6);
writeRow(cs,din,clk,7, col7);
writeRow(cs,din,clk,8, col8);
}
long playMelody(int pin, String melody)
{
int first=0;
int second=0;
int note;
int duration;
int go_on=1;
while(go_on)
{
  second=melody.indexOf(',');
  note=melody.substring(first,second).toInt();
  first=second+1;
  melody=melody.substring(first);
  first=0;
  second=melody.indexOf(',');
  if (second==-1)
    go_on=0;
  duration=melody.substring(first,second).toInt();
  first=second+1;
  melody=melody.substring(first);
  first=0;
  tone(pin,note,duration);
  delay(duration);
  noTone(pin);
 }
}
int follow(int light, int gain)
{
  return (int)(((float)(gain)/100.0)*((float)((_white-_black)/2+_black)-(float)(light)));
}
unsigned long _pulseIn(uint8_t pin, uint8_t state, unsigned long timeout)
{
  uint8_t bit = digitalPinToBitMask(pin);
  uint8_t port = digitalPinToPort(pin);
  uint8_t stateMask = (state ? bit : 0);
  unsigned long width = 0;
  unsigned long numloops = 0;
  unsigned long maxloops = microsecondsToClockCycles(timeout) / 16;
  while ((*portInputRegister(port) & bit) == stateMask)
    if (numloops++ == maxloops)
      return 0;
  while ((*portInputRegister(port) & bit) != stateMask)
    if (numloops++ == maxloops)
      return 0;
  while ((*portInputRegister(port) & bit) == stateMask) {
    if (numloops++ == maxloops)
      return 0;
    width++;
  }
  return clockCyclesToMicroseconds(width * 21 + 16);
}
long US_init(int trigger_pin, int echo_pin)
{
  digitalWrite(trigger_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigger_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigger_pin, LOW);
  long microseconds = _pulseIn(echo_pin ,HIGH,100000);
  return microseconds;
}
long distance(int trigger_pin, int echo_pin)
{
  long microseconds = US_init(trigger_pin, echo_pin);
  long distance;
  distance = microseconds/29/2;
  if (distance == 0){
    distance = 999;
  }
  return distance;
}