const int ledPin =  13; // Numero del pin para el Led     
const int EnTxPin =  5; // HIGH:TX y LOW:RX
#define rs485 Serial1
char c;
String inputString;

void setup() 
{ 
  Serial.begin(115200);
  rs485.begin(115200);
  rs485.setTimeout(100); //establecemos un tiempo de espera de 100ms
  // inicializamos los pines
  pinMode(ledPin, OUTPUT);
  pinMode(EnTxPin, OUTPUT);
  digitalWrite(ledPin, LOW); 
} 

uint8_t CMD[ 32 ]; // CMD + Ch + 6
 
void loop() 
{ 
   if (Serial.available())
    {
      c = Serial.read();
      //Serial.print(c);

      if ((c == '\r') || (c == '\n'))
      {
        if (inputString == "ping")
        {
          Serial.println("received ping to all");
          receiveData0(101);
          receiveData0(102);
          receiveData0(103);
          receiveData0(104);
          receiveData0(105);
          receiveData0(106);
          receiveData0(107);
          receiveData0(108);
        }
        else if (inputString.startsWith("ping "))
        {
          String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
          Serial.print("received ping to ");
          int direction = str.toInt();
          Serial.println(direction);
          receiveData0(direction);
        }
        if (inputString == "lim")
        {
          Serial.println("received limit to all");
          receiveData1(101);
          receiveData1(102);
          receiveData1(103);
          receiveData1(104);
          receiveData1(105);
          receiveData1(106);
          receiveData1(107);
          receiveData1(108);
        }
        else if (inputString.startsWith("lim "))
        {
          String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
          Serial.print("received limit to ");
          int direction = str.toInt();
          Serial.println(direction);
          receiveData1(direction);
        }
        else if (inputString == "cal")
        {
          Serial.println("calibrating all");
          calibrationData(101);
          calibrationData(102);
          calibrationData(103);
          calibrationData(104);
          calibrationData(105);
          calibrationData(106);
          calibrationData(107);
          calibrationData(108);
        }
        else if (inputString.startsWith("cal "))
        {
          String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
          Serial.print("calibrating ");
          int direction = str.toInt();
          Serial.println(direction);
          calibrationData(direction);
        }
        else if (inputString.startsWith("send:"))
        {
          Serial.print("send: ");
          String str = inputString.substring(inputString.lastIndexOf(":") + 1);
          int direction = str.toInt();
          
          String command = str.substring(0, str.indexOf(','));
          command.trim();

          int index = 0;
          int commaIndex = 0;
          do {
            commaIndex = str.indexOf(',', commaIndex + 1);
            String tmp = str.substring(commaIndex + 1, str.indexOf(',', commaIndex + 1));
            tmp.trim();
            if (isNumeric(tmp)) {
              CMD[index] = tmp.toInt();
              index++;
            } 
          } while (str.lastIndexOf(',') != commaIndex);
          int angle = CMD[0];
          int speed = CMD[1];
          Serial.print(direction);
          Serial.print(", ");
          Serial.print(angle);
          Serial.print(", ");
          Serial.println(speed);
          sendData(direction, angle, speed);
        }
        inputString = String();

      }
      else
        inputString += c;
    } 
} 

void funcion(int dato)
{
    Serial.println(dato);
}

void sendData(int direction, int angle, int speed)
  {
    digitalWrite(EnTxPin, HIGH); //RS485 como Transmisor
    //---enviamos el ángulo para mover el servo------
    rs485.print("I"); //inicio de trama
    rs485.print(direction);//dirección del esclavo
    rs485.print("S"); //función  S para indicarle el angulo
    rs485.print(angle); //ángulo  o dato
    rs485.print("V"); //función  V para indicarle que velocidad tendra.
    rs485.print(speed); //ángulo  o dato
    rs485.print("F"); //fin de trama
    //----------------------------
    digitalWrite(EnTxPin, HIGH); //RS485 como receptor
  }

int receiveData0(int direction) //Responde con el angulo actual;
  {
      digitalWrite(EnTxPin, HIGH); //RS485 como Transmisor
      //---solicitamos una lectura del sensor----------
      rs485.print("I"); //inicio de trama
      rs485.print(direction);//direccion del esclavo
      rs485.print("L"); //L para indicarle que vamos a Leer el angulo actual
      rs485.print("F"); //fin de trama
      rs485.flush();    //Esperamos hasta que se envíen los datos
      //----Leemos la respuesta del Esclavo-----
      digitalWrite(EnTxPin, LOW); //RS485 como receptor
    
      if(rs485.find("i")) //esperamos el inicio de trama
      {
          int esclavo=rs485.parseInt();  //recibimos la direccion del esclavo
          int dato=rs485.parseInt();  //recibimos el dato
          if(rs485.read()=='f'&&esclavo==101) //si fin de trama y direccion son los correctos
          {
             funcion(dato);   //realizamos la acción con el dato recibido
             return dato;
          }
      }
      return -1;
  }

int receiveData1(int direction) //Responde con el angulo actual;
  {
      digitalWrite(EnTxPin, HIGH); //RS485 como Transmisor
      //---solicitamos una lectura del sensor----------
      rs485.print("I"); //inicio de trama
      rs485.print(direction);//direccion del esclavo
      rs485.print("A"); //L para indicarle que vamos a Leer el limite de angulo
      rs485.print("F"); //fin de trama
      rs485.flush();    //Esperamos hasta que se envíen los datos
      //----Leemos la respuesta del Esclavo-----
      digitalWrite(EnTxPin, LOW); //RS485 como receptor
    
      if(rs485.find("i")) //esperamos el inicio de trama
      {
          int esclavo=rs485.parseInt();  //recibimos la direccion del esclavo
          int dato=rs485.parseInt();  //recibimos el dato
          if(rs485.read()=='f'&&esclavo==101) //si fin de trama y direccion son los correctos
          {
             funcion(dato);   //realizamos la acción con el dato recibido
             return dato;
          }
      }
      return -1;
  }
  
void calibrationData(int direction)
  {
    digitalWrite(EnTxPin, HIGH); //RS485 como Transmisor
    //---enviamos el ángulo para mover el servo------
    rs485.print("I"); //inicio de trama
    rs485.print(direction);//dirección del esclavo
    rs485.print("C"); //función  C para indicarle el comando de calibrado.
    rs485.print("F"); //fin de trama
    rs485.flush();    //Esperamos hasta que se envíen los datos
    //----------------------------
    digitalWrite(EnTxPin, HIGH); //RS485 como receptor
  }

boolean isNumeric(String str) {
  for (char i = 0; i < str.length(); i++) {
    if ( !(isDigit(str.charAt(i)) )) {
      return false;
    }
  }
  return true;
}