#include <NewPing.h>// library for working with ultrasonic sensors, so as not to block the main flow
// NewPing library uses Timer2 for ATmega328P or Timer4 for ATmega32U4
// Also for work functions micros () - NewPing, and millis () - in a sketch are used.
// these functions use Timer0,
// respectively the only free timer for PWM is Timer1

// I use the modified version of NewPing library:
// NewPing :: check_timer () returns an unsigned int:
// 0 - if the distance definition is not yet complete,
// 1 - if the obstacle is too far
// 2 - if the distance to the obstacle is defined

#define APSSID "ESP8266" // the name of the access point network
#define APPASS "1234567890" // access point network password
#define SRVPORT 333 // port on which the server will wait for the TCP connection

const String SNDCMD = "ready";//the main command to the application (Arduino is ready to accept commands)
String send_buf, cmd;

//We use nonblocking poll WiFi module
const unsigned long tmtsrv=1000; //timeout waiting for a response from the android application, ms
const unsigned long tmtwf=100; //timeout of the response from the WiFi module, ms
const unsigned long tmtsnd=20; //timeout for receiving a response ">" about the readiness to send data from the WiFi module, ms
const unsigned long tmtok=50; //timeout of receiving the answer "SEND OK" about the successful execution of the command from the WiFi module, ms

unsigned long ct; //time recorded since the beginning of the waiting timeout for the team com, ms
unsigned long cts; //time recorded since the start timeout for the coms command, ms

int com;//the executable command (0-no command, 1-waiting for a response from the client)
int coms;//the performed stage of the operation of sending the message (0-operation is not started, 1-the WiFi command is sent to the module, 2-data is sent to the application)

// to specify the direction of rotation, it is necessary that in1Pin and in2Pin are reciprocal:
// turn left - in1Pin = 0; in2Pin = 1;
// turn right - in1Pin = 1; in2Pin = 0;
const int en12Pin = 10;//input control enable (1) L293DNE - turn force (this PWM output uses Timer1)
const int in1Pin = 11;//input control 1A (2) L293DNE - direction of rotation
const int in2Pin = 12;//input control 2A (7) L293DNE - direction of rotation

// to specify the direction of motion (forward or backward), it is necessary that in3Pin and in4Pin are reciprocal:
// move forward - in3Pin = 0; in4Pin = 1;
// move backwards - in3Pin = 1; in4Pin = 0;
const int en34Pin = 9;//input control enable (9) L293DNE - travel speed (this PWM output uses Timer1)
const int in3Pin = 7;//input control 3A (10) L293DNE - direction of movement
const int in4Pin = 8;//input control 4A (15) L293DNE - direction of travel movement

//Ultrasonic sensor on the forward mvement
const int tr1Pin = 2;//for the 3rd pin we connect the TRIG input of sensor 1
const int ec1Pin = 3;//on the 4th pin we connect the output of ECHO sensor 1
//Ultrasonic sensor in reverse
const int tr2Pin = 4;//on the 2nd pin we connect the TRIG input of the sensor 2
const int ec2Pin = 5;//on the 7th pin we connect the output of ECHO sensor 2

const int max_dist = 200;//maximum scan distance range ultrasonic sensor
int dist_vpered = 0, dist_nazad = 0;//distance to obstacle in front and behind
bool alarmvp;//danger of collision: false - no danger, true - danger of collision in front
bool alarmsz;//danger of collision: false - no danger, true - danger of collision in rear

#define VLTMTR A0//on A0 we measure the voltage from the divider to measure the voltage on the batteries
//1.1 - internal reference voltage +1.1 V, 9.52 - coefficient of the voltage divider (taking into account the specified resistance of the resistors)
// bit depth ADC on analog inputs - 10 bits, respectively 2 to 10th degree = 1024
const float vref=1.1, kdn = 9.5, acp = 1024.0;
//Фары
#define lfPin A2 //Headlights
#define lbPin A3 //rear lights

//We use nonblocking polling of ultrasonic sensors thanks to the library NewPing
const unsigned int pingspd = 300;//polling period ultrasonic sensors - 300 milliseconds
unsigned long tmtping;//here we write the time of the next ping
NewPing uzd1(tr1Pin, ec1Pin, max_dist);//create an object for working with an ultrasonic sensor 1 (forward motion)
NewPing uzd2(tr2Pin, ec2Pin, max_dist);//create an object to work with an ultrasonic sensor 2 (reverse)

void setup() {
  // put your setup code here, to run once:
  analogReference(INTERNAL);//set the reference voltage with respect to which analog measurements take place in 1.1 V
  delay(3000);//wait for the WiFi module to turn on
  Serial.begin(115200);
  pinMode(en12Pin, OUTPUT);
  pinMode(en34Pin, OUTPUT);
  pinMode(in1Pin, OUTPUT);
  pinMode(in2Pin, OUTPUT);
  pinMode(in3Pin, OUTPUT);
  pinMode(in4Pin, OUTPUT);
  pinMode(lfPin, OUTPUT);
  pinMode(lbPin, OUTPUT);
  
  //set the PWM frequency of 62500 Hz of the outputs 9.10 - the timer 1
  //timer 0 and timer 2 can not be used !!! (timer 0 is occupied by the function millis (), timer 2 will be occupied by ultrasonic sensors)
  TCCR1A = TCCR1A & 0xe0 | 1;
  TCCR1B = TCCR1B & 0xe0 | 0x09;
  
  //primary initialization - we stand still
  analogWrite(en12Pin, 0);
  analogWrite(en34Pin, 0);

  //Serial.println("AT+RST"); //reload wifi module
  //delay(3000);
  
  Serial.println("AT");
  delay(1000);
  if(!Serial.find("OK"))// if the module is not ready
  {
    while(1)//hang, wait for the restart, blink all the lights
    {
      digitalWrite(lfPin, LOW);
      digitalWrite(lbPin, LOW);
      delay(500);
      digitalWrite(lfPin, HIGH);
      digitalWrite(lbPin, HIGH);
      delay(1000);
    }
  }
  // WiFi module is configured so that it's rises as softAP (in access point mode)
  // with the configured APSSID network name, APPASS password, channel number 5, password encryption - WPA2_PSK
  
  // Need to write a validation of the access point settings?
  
  // allow many connections to the server (without this the server will not be created)
  Serial.println("AT+CIPMUX=1");
  delay(1000);
  Serial.find("OK"); // remove unnecessary
  // create server
  String cmd="AT+CIPSERVER=1,";
  cmd+=String(SRVPORT);
  Serial.println(cmd);
  delay(1000);
  if(!Serial.find("OK"))// if the command did not pass
  {
    while(1)//hang, wait for the reboot, quickly switch the light from the headlights to the rear and back
    {
      digitalWrite(lfPin, HIGH);
      digitalWrite(lbPin, LOW);
      delay(500);
      digitalWrite(lfPin, LOW);
      digitalWrite(lbPin, HIGH);
      delay(500);
    }
  }
  delay(1000);
  com=0;
  coms=0;
  alarmvp = false;//Getting obstacles not found
  alarmsz = false;//Getting obstacles not found
  //From that moment, ultrasonic sensors poll
  tmtping = millis();
}

void loop() {
  // We ask where to go
  if(com==0)//If there is no command, then we send the request to the application
  {   
    if(coms==0)//send a command to send data to the WiFi module
    {// the message about readiness and about the distance to the obstacles of the format "ready0701807,6"
      // where ready - everything is fine, ready to accept the command from the application
      // 070 - distance to the obstacle in front of 70 cm
      // 180 - distance to the obstacle from the rear 180 cm
      // 7,6 - voltage on the batteries
      // at the end we add \ n so that the application reads from the socket of the command immediately completed
      cleanRdBuf(); //clean the serial port buffer

      float vbatt = analogRead(VLTMTR) / acp * vref * kdn;//calculation of input voltage Vin Arduino
      send_buf = SNDCMD + convToStrL3(dist_vpered) + convToStrL3(dist_nazad) + convToStrFloat(vbatt) +"\n";
      cmd="AT+CIPSENDEX=0,";//request WiFi module AT+CIPSENDEX=0,12 
      cmd+=send_buf.length();//(0 is the connection identifier, 12 is the length of the message)
      Serial.println(cmd);
      coms=1;//now waiting for readiness to send data
      cts = millis();
    }
    if(coms==1)//Waiting for a response from the WiFi module that you can send data
    {
      if((millis()-cts)>=tmtsnd)//waiting for the end of timeout
      {
        if(Serial.find(">"))// if the command has passed, then you need to give a message
        {
          Serial.println(send_buf);
          coms=2;
          cts=millis();
        }else{
          //command error AT+CIPSENDEX=0,12
          //can not connect
          com=0;
          coms=0;
        }
      }
    }
    if(coms==2)//Waiting for a response from the WiFi module that the data was successfully sent
    {
      if((millis()-cts)>=tmtok)//waiting for the end of timeout
      {
        if(Serial.find("SEND OK"))
        {
          //it's ok
          com=1;//now waiting for a response from the server
          coms=0;
          ct=millis();//remember the time
        }else{
          //breakage or data transmission error 
          //can not send
          com=0;
          coms=0;
        }
      }
    }
  }
  if(com==1)
  {
    if(((Serial.available()>0)&&((millis()-ct)>=tmtwf))||((millis()-ct)>=tmtsrv))//we expect a response of at least tmtwf msec and not more than tmtsrv msec
    {// reading data from the WiFi module, so far we do not care about the availability of the connection, then we add
      String rcv_buf="";// read buffer
      while(Serial.available()>0)
      {// read everything in Serial port
        char cbuf=Serial.read();
        rcv_buf.concat(cbuf);
      }
      rcv_buf.trim();//clean beginning and end of the whitespace characters
      ParseCommand(rcv_buf);
      com=0;//waiting for a new command from the application
      cleanRdBuf();//clean the serial port buffer
    }//end of timeout processing condition
  }//end of processing condition for waiting command response (com==1)
 
  //poll of ultrasonic sensors (sensors can not be polled simultaneously)
  if(millis()>=tmtping)
  {
    tmtping += pingspd;//the time of the next measurement of the distance to the obstacle
    alarmvp = false;//we reset the danger of collision
    uzd1.ping_timer(uzd1_ecCheck);
  }
  // We start polling the second sensor with a delay of half of the polling time
  if(millis()>=(tmtping-pingspd/2))//Simultaneous polling of NewPing objects can not be done - they interfere with each other
  {
    alarmsz = false;//we reset the danger of collision
    uzd2.ping_timer(uzd2_ecCheck);
  }
}
//clean read buffer
void cleanRdBuf()
{
  while(Serial.available())
  {
    Serial.read();
  }
}

void ParseCommand(String rcv_buf)
{
  int pos=rcv_buf.indexOf("+IPD,0");
  if(pos<0)//if the wrong answer from the WiFi module
  {
    pos=0;
    rcv_buf="+IPD,0,4:0000";
  }
  char dvig=rcv_buf.charAt(pos+9);//the symbol indicates where we are going: forward (A) or backward (B)
  char temp=rcv_buf.charAt(pos+10);//the symbol indicates at what speed we are moving 0-stand, 9-full gas
  int dvel=int(temp)-int('0');
  char pvrt=rcv_buf.charAt(pos+11);//the symbol indicates where to turn: to the right (C) or to the left (D)
  temp=rcv_buf.charAt(pos+12);//the symbol indicates the degree of rotation of the wheel: 0-do not rotate, 9-turn to the maximum angle
  int pvel=int(temp)-int('0');
  
  switch(dvig){
    case 'A':
      //едем вперёд
      if(alarmvp && (dvel > 5))//If there is a danger of collision in front and high speed
      {
        ostanov();//stop toy car
      } else {
        dvigvpered(dvel);//otherwise you can move with a given speed
      }
      break;
    case 'B':
      //едем назад
      if(alarmsz && (dvel > 5))//If there is a danger of collision from behind and high speed
      {
        ostanov();//stop toy car
      } else {
        dvignazad(dvel);//otherwise you can move with a given speed
      }
      break;
    default:
      ostanov();//stop toy car
  }
  switch(pvrt){
    case 'C':
      pvrtvpravo(pvel);//turn right
      break;
    case 'D':
      pvrtvlevo(pvel);//turn left
      break;
    default:
      pryamo();//if we do not turn, then we go straight
  }// decided where to go
}

//scan the distance to the obstacle ahead
void uzd1_ecCheck()
{
  if(uzd1.check_timer()==2)//if the distance to the obstacle is determined, then ...
  {
    dist_vpered = uzd1.ping_result / US_ROUNDTRIP_CM;//calculate the distance to the obstacle
    if(dist_vpered < 60)//If the distance to the obstacle is less than 60 cm in front
    {
    //  alarmvp = true;//collision hazard in front
    }else{
      alarmvp = false;//we reset the danger of collision
    }
  } else if(uzd1.check_timer()==1)//obstacle is too far
  {
    dist_vpered = 299;//reset the distance to the obstacle in FAR
  }
}
//scan the distance to the obstacle behind
void uzd2_ecCheck()
{
  if(uzd2.check_timer()==2)//if the distance to the obstacle is determined, then ...
  {
    dist_nazad = uzd2.ping_result / US_ROUNDTRIP_CM;//calculate the distance to the obstacle
    if(dist_nazad < 60)//If the distance to the obstacle is less than 60 cm behind
    {
    //  alarmsz = true;//collision hazard from behind
    } else {
      alarmsz = false;//we reset the danger of collision
    }
  }else if(uzd2.check_timer()==1)//obstacle is too far
  {
    dist_nazad = 299;//reset the distance to the obstacle in FAR
  }
}

bool dvigvpered(int dvel)//move forward
{
  analogWrite(en34Pin, dvel * 28);
  digitalWrite(in3Pin, LOW);
  digitalWrite(in4Pin, HIGH);
  digitalWrite(lfPin, HIGH);
  digitalWrite(lbPin, LOW);
}
bool dvignazad(int dvel)//move backwards
{
  analogWrite(en34Pin, dvel * 28);
  digitalWrite(in3Pin, HIGH);
  digitalWrite(in4Pin, LOW);
  digitalWrite(lfPin, LOW);
  digitalWrite(lbPin, HIGH);
}
bool pvrtvlevo(int pvel)//turn left
{
  analogWrite(en12Pin, pvel * 28);
  digitalWrite(in1Pin, LOW);
  digitalWrite(in2Pin, HIGH);
}
bool pvrtvpravo(int pvel)//turn right
{
  analogWrite(en12Pin, pvel * 28);
  digitalWrite(in1Pin, HIGH);
  digitalWrite(in2Pin, LOW);
}
void ostanov()//stop
{
  analogWrite(en34Pin, 0);
}
void pryamo()//straight out
{
  analogWrite(en12Pin, 0);
}
String convToStrL3(int val)//the function of converting a positive number into a 3-digit string, for example 1 becomes "001"
{
  if(abs(val) < 10)
  {
    return ("00" + String(val));
  }else if(abs(val) < 100)
  {
    return ("0" + String(val));
  }else{
    return String(val);
  }
}
String convToStrFloat(float val)//function of converting a fractional number into a 3-digit string, for example "6.9"
{
  if(abs(val) < 10.0)
  {
    return String(abs(val), 1);
  }else{
    return "0.0";
  }
}