// 4-channel RC receiver for controlling
// an RC car / boat / plane / quadcopter / etc.
// using an ESP8266 and an Android phone with RoboRemo app

// Disclaimer: Don't use RoboRemo for life support systems
// or any other situations where system failure may affect
// user or environmental safety.

#include <ESP8266WiFi.h>
#include <WiFiClient.h>

/* GPIO     NodeMCU
 *  0    =    RX        Motor
    1    =    TX - TXD0
    2    =    D4 - TXD1 Motor
    3    =    RX - RXD0
    4    =    D2        Motor
    5    =    D1        Motor
    6    =    ---
    7    =    ---
    8    =    ---
    9    =    SD2
    10   =    SD3
    11   =    ---
    12   =    D6
    13   =    D7       Lijn links
    14   =    D5       LED 1
    15   =    D8       Lijn Rechts
    16   =    D0       LED 2
*/

// config:

const char *ssid = "auto";  // You will connect your phone to this Access Point
const char *pw = "qwerty123"; // and this is the password
IPAddress ip(192, 168, 0, 1); // From RoboRemo app, connect to this IP
IPAddress netmask(255, 255, 255, 0);
const int port = 9876; // and this port

WiFiServer server(port);
WiFiClient client;


char cmd[100]; // stores the command chars received from RoboRemo
int cmdIndex;

int chVal[] = {1500, 1500, 1500, 1500}; // default value (middle)
float fPowerMotor_Links = 0;
float fPowerMotor_Rechts = 0;
int iSetPower = 0;
int iDeltaL = 0;
int iDeltaR = 0;

bool bRichting;
bool bRichting_Links;
bool bRichting_Rechts;


unsigned long lastCmdTime = 60000;
unsigned long aliveSentTime = 0;



boolean cmdStartsWith(const char *st) { // checks if cmd starts with st
  for(int i=0; ; i++) {
    if(st[i]==0) return true;
    if(cmd[i]==0) return false;
    if(cmd[i]!=st[i]) return false;;
  }
  return false;
}


void exeCmd() { // executes the command from cmd

lastCmdTime = millis();

  // example: set RoboRemo slider id to "ch0", set min -1023 and set max 1023
  
  if( cmdStartsWith("ch") ) {
    int ch = cmd[2] - '0';
    if(ch>=0 && ch<=9 && cmd[3]==' ') {
      chVal[ch] = (int)atof(cmd+4);
      
      //Serial.println(chVal[ch]);
      if (chVal[ch] >= 0){
        iSetPower = chVal[ch];
        bRichting = true; //vooruit
        bRichting_Links = true;
        bRichting_Rechts = true;       
      }
      else
      {
        iSetPower = (chVal[ch] * -1);
        bRichting = false; //Achteruit
        bRichting_Links = false;
        bRichting_Rechts = false;       
      }
     }
  }
  
    if( cmdStartsWith("lr") ) {
    int ch = cmd[2] - '0';
    if(ch>=0 && ch<=9 && cmd[3]==' ') {
      chVal[ch] = (int)atof(cmd+4);

      //Serial.println(chVal[ch]);
      if (chVal[ch] >= 0){
        iDeltaL = 0;
        if (chVal[ch] < 25){
          iDeltaR = 0;
        }else
        {
          iDeltaR = 100;
        }
        //iDeltaR = chVal[ch];
      }
      else
      {
        iDeltaL = ( chVal[ch] * -1);
        if (iDeltaL < 25){
          iDeltaL = 0;
        }else
        {
          iDeltaL = 100;
        }
       iDeltaR = 0;
      }
     }      
    }

    //  de motor snelheid
    fPowerMotor_Links  = (iSetPower - ( iSetPower * 0.01 * iDeltaL));
    fPowerMotor_Rechts = (iSetPower - ( iSetPower * 0.01 * iDeltaR));

    if (fPowerMotor_Links < 150)
    {
      fPowerMotor_Links = 0;
    }else
    {
      fPowerMotor_Links = (250 + (fPowerMotor_Links * 0.885)) ;
    }

    if (fPowerMotor_Rechts < 150)
    {
      fPowerMotor_Rechts = 0;
    }else
    {
      fPowerMotor_Rechts = (250 + (fPowerMotor_Rechts * 0.885)) ;
    }    

    if( cmdStartsWith("lg") ) {
    int ch = cmd[2] - '0';
    if(ch>=0 && ch<=9 && cmd[3]==' ') {
      chVal[ch] = (int)atof(cmd+4);
      
      //Serial.println(chVal[ch]);
      if (chVal[ch] == 1){
        digitalWrite(16, true);
        digitalWrite(14, true);
      }
      else
      {
        digitalWrite(16, false);
        digitalWrite(14, false);
      }
     }
  }

    
    analogWrite(4, fPowerMotor_Links);
    analogWrite(5, fPowerMotor_Rechts);
    digitalWrite(0, bRichting_Links);
    digitalWrite(2, bRichting_Rechts);  
  
  
}



void setup() {

  pinMode(5, OUTPUT); // 
  pinMode(4, OUTPUT); // 
  pinMode(0, OUTPUT); // 
  pinMode(2, OUTPUT); // 

  pinMode(16, OUTPUT); // LED 1
  pinMode(14, OUTPUT); // LED 2

  delay(1000);

  cmdIndex = 0;

  Serial.begin(115200);

  WiFi.softAPConfig(ip, ip, netmask); // configure ip address for softAP 
  WiFi.softAP(ssid, pw); // configure ssid and password for softAP

  server.begin(); // start TCP server

  Serial.println("ESP8266 RC receiver 1.1 powered by RoboRemo");
  Serial.println((String)"SSID: " + ssid + "  PASS: " + pw);
  Serial.println((String)"RoboRemo app must connect to " + ip.toString() + ":" + port);

  
}


void loop() {

  // if contact lost for more than half second
  //if(millis() -  > 500) {  
  //Serial.println((String)"millis: " + millis());
  if((millis() - lastCmdTime) > 1000) { 
    //Serial.println((String)"contact los: " + (millis() - lastCmdTime));   
    //for(int i=0; i<chCount; i++) {
    analogWrite(4, 0);
    analogWrite(5, 0);

    
  }

  if(!client.connected()) {
    client = server.available();
    //Serial.println((String)"client.connected: " + client.connected());
    return;
  }

  // here we have a connected client

  if(client.available()) {
    char c = (char)client.read(); // read char from client (RoboRemo app)
    Serial.println((String)"char c: " + c);
    lastCmdTime = millis();

    if(c=='\n') { // if it is command ending
      cmd[cmdIndex] = 0;
      exeCmd();  // execute the command
      cmdIndex = 0; // reset the cmdIndex
      
    } else {      
      cmd[cmdIndex] = c; // add to the cmd buffer
      if(cmdIndex<99) cmdIndex++;
    }
  } 

  if(millis() - aliveSentTime > 500) { // every 500ms
    client.write("alive 1\n");
    // send the alibe signal, so the "connected" LED in RoboRemo will stay ON
    // (the LED must have the id set to "alive")
    Serial.println((String)"aliveSentTime: " + (millis()- aliveSentTime));
    aliveSentTime = millis();
    
    // if the connection is lost, the RoboRemo will not receive the alive signal anymore,
    // and the LED will turn off (because it has the "on timeout" set to 700 (ms) )
  }

}