#include <SoftwareSerial.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
#include <wiring_private.h>
#include <pins_arduino.h>

/***   Global variables   ***/
SoftwareSerial _bt_softwareSerial((3),(2));
int mode=8;
Adafruit_SSD1306 oled_display(4);
int _white=630;
int _black=30;
Servo _servo5;
Servo _servo6;
int _bt_cmd=0;
Servo _servo11;
Servo _servo10;
const uint16_t NO_CRY[] = {523,562,494,187,523,187,494,187,494,187,440,562,0,562,659,375,698,187,659,187,698,187,523,187};

/***   Function declaration   ***/
void setFooterText(String msg);
void setHeaderText(String msg);
void setMainText(String msg);
void playMelody(int pin,const uint16_t* melody, int length);
void move_base_5_6(int speed, int advance, int turn);
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();
  String name = "DumboBot";
  String str = "AT+NAME"+name+"\r\n";
  _bt_softwareSerial.write(str.c_str());


  oled_display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  oled_display.clearDisplay();
  oled_display.setTextColor(WHITE);
  oled_display.display();
  delay(1);

    setFooterText("Manual ctrl");
  _white=30;
  _black=730;
  mode=8;

  pinMode((9),INPUT);

  _servo5.attach(5);
  _servo6.attach(6);
  _servo11.attach(11);
  _servo10.attach(10);
  pinMode( (7) , INPUT );
  pinMode( (13) , OUTPUT );
}


void loop()
{
    if (digitalRead((9))) {
      setHeaderText("BT conected");
     }else {
      setHeaderText("BT dissconected");
      mode=8;
      _servo5.write(((0*90)/100+90));
      _servo6.write(((0*90)/100+90));
     }
    if (_bt_softwareSerial.available()>0  || _bt_cmd>0)
    {
      int cmd=_bt_softwareSerial.read();

      if (cmd==0){
        _bt_cmd=0;
        _servo5.write(((0*90)/100+90));
        _servo6.write(((0*90)/100+90));
        setMainText("stop");
        }
      if ((cmd==1)||(_bt_cmd==1)){
            _servo5.write(((0*90)/100+90));
        _servo6.write((((-10)*90)/100+90));
        }
      if ((cmd==4)||(_bt_cmd==4)){
            _servo5.write(((10*90)/100+90));
        _servo6.write(((0*90)/100+90));
        }
      if ((cmd==2)||(_bt_cmd==2)){
            _servo5.write(((10*90)/100+90));
        _servo6.write((((-10)*90)/100+90));
        }
      if ((cmd==3)||(_bt_cmd==3)){
            _servo5.write((((-10)*90)/100+90));
        _servo6.write(((10*90)/100+90));
        }
      if ((cmd==6)||(_bt_cmd==6)){
            _servo11.write(0);
        _servo10.write(180);
        setMainText("Adios");
        }
      if ((cmd==5)||(_bt_cmd==5)){
            _servo11.write(100);
        _servo10.write(80);
        setMainText("Hola");
        }
      if ((cmd==7)||(_bt_cmd==7)){
            playMelody(8,NO_CRY,sizeof(NO_CRY)/(2*sizeof(uint16_t)));
        }
      if ((cmd==8)||(_bt_cmd==8)){
            setFooterText("Manual ctrl");
        mode=8;
        _servo5.write(((0*90)/100+90));
        _servo6.write(((0*90)/100+90));
        setMainText("stop");
        }
      if ((cmd==9)||(_bt_cmd==9)){
            setFooterText("Line Follow");
        mode=9;
        }
      if ((cmd==11)||(_bt_cmd==11)){
            setFooterText("Avoid obstac");
        mode=11;
        }}
    if (mode == 9) {
        uint16_t var = analogRead(A0);
        // setMainText(String(var));

          int16_t turnrate = (int16_t) ((var - _black - (_white-_black)/2))/28;
          //Serial.println(turnrate);
          // int turnrateInt = int16_t (turnrate*100);
          // setMainText(String(var));
          move_base_5_6(35,25,(turnrate));
        /*if(analogRead((A0))<(0.4*(_black-_white)+_white))
        {
          move_base_5_6(35,25,(-15));
        }
        else if(analogRead((A0))<(0.6*(_black-_white)+_white))
        {
          move_base_5_6(35,25,0);
        }
        else
        {  move_base_5_6(35,25,15);
  
        } 
        */
      }
      else if (mode == 11) {
      if (distance((13),(7))<(30)){
        // setMainText("uuups");
        move_base_5_6(35,25,35);

      }
      else
      {
        // setMainText("free");
        move_base_5_6(35,25,0);
      } }


  true;

}

/***   Function definition   ***/
void setFooterText(String msg){
  int16_t x;
  int16_t y;
  int16_t w;
  int16_t h;
  int16_t pos=8;
  oled_display.setTextSize(1);
  oled_display.getTextBounds(msg.c_str(),0,0,&x,&y,&w,&h);
  if (w>110)
  {
    String msg2;
    uint8_t i=0;
    for (i=0;i<=(msg.length()-18);i++)
    {
      msg2=msg.substring(i,18+i);
      oled_display.fillRect(8,24,112,8,BLACK);
      oled_display.setCursor(8,24);
      oled_display.print(msg2);
      oled_display.display();
      delay(100);
      if (i==0)
        delay(300);
    }
  }
  else
  {
    x=(128-w)/2;
    oled_display.fillRect(8,24,112,8,BLACK);
    oled_display.setCursor(x,24);
    oled_display.print(msg);
    oled_display.display();
    delay(1);  }
}
void setHeaderText(String msg){
  int16_t x;
  int16_t y;
  int16_t w;
  int16_t h;
  oled_display.setTextSize(1);
  oled_display.getTextBounds(msg.c_str(),0,0,&x,&y,&w,&h);
  if (w>110){
    String msg2;
    uint8_t i=0;
    for (i=0;i<=(msg.length()-18);i++)
    {
      msg2=msg.substring(i,18+i);
      oled_display.fillRect(8,0,112,8,BLACK);
      oled_display.setCursor(8,0);
      oled_display.print(msg2);
      oled_display.display();
      delay(100);
      if (i==0)
        delay(300);
    }
  }
  else
  {
    oled_display.fillRect(8,0,112,8,BLACK);
    x=(128-w)/2;
    oled_display.setCursor(x,0);
    oled_display.print(msg);
    oled_display.display();
    delay(1);
  }
}
void setMainText(String msg){
  oled_display.setTextSize(2);
  int16_t x;
  int16_t y;
  int16_t w;
  int16_t h;
  oled_display.fillRect(16,8,96,16,BLACK);
  oled_display.getTextBounds(msg.c_str(),0,0,&x,&y,&w,&h);
  if (w>94)
  {
    msg=msg.substring(0,8);
    oled_display.getTextBounds(msg.c_str(),0,0,&x,&y,&w,&h);
  }
  x=(128-w)/2;
  oled_display.setCursor(x,9);
  oled_display.print(msg);
  oled_display.display();
  delay(1);
}
void playMelody(int pin,const uint16_t* melody, int length)
{
  unsigned int note;
  unsigned long duration;
  uint16_t* melody_ptr=(uint16_t*)melody;
  for (int i=0;i<length;i++)
  {
    note=*melody_ptr++;
    duration=*melody_ptr++;
    tone(pin,note,duration);
    delay(duration);
    noTone(pin);
  }
}
void move_base_5_6(int speed, int advance, int turn)
{
_servo5.write(90+(int)(65.0*(float)(speed*(advance-turn))/10000.0));
_servo6.write(90-(int)(65.0*(float)(speed*(advance+turn))/10000.0));
};
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;
}
