#include <DmxSimple.h>
const int ledPin =  13; // Numero del pin para el Led     
const int EnTxPin =  49; // HIGH:TX y LOW:RX
#define rs485 Serial2
#define DMX_ON false
char c;
const int id[8] = {101, 102, 103, 104, 105, 106, 107, 108};
int limit[8] = {360, 360, 360, 360, 360, 360, 360, 360};
int center[8] = {180, 180, 180, 180, 180, 180, 180, 180};
String inputString;

const int foco[4] = {313, 305, 226, 297};
const int strobe[4] = {247, 61, 69, 233};

unsigned long time_strobe = millis();
unsigned long time_foco = millis();
unsigned long time_mov = millis();
unsigned long time_stop = millis();
unsigned long time_escape = millis();
int cont_foco = 0;
int cont_mov = 0;
byte port = 0;

byte mode = 0;
bool flag_mode0 = false;

int CMD[ 64 ]; // CMD + Ch + 6
bool flag_ramp = false;
byte inc = 0;
boolean flag_center= false;

void FC1_CHANGE() { //PD3
  port = PIND & 0x08; //B00001100;
  if (port == 0x00) mode = 1;
  else mode = 0;
  Serial.println(mode);
}

void FC2_CHANGE() { //PD2
  port = PIND & 0x04; //B00001100;
  if (port == 0x00) mode=2;
  else mode = 0;
  Serial.println(mode);
}

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);
  pinMode(18, INPUT);
  pinMode(19, INPUT);
  digitalWrite(ledPin, LOW); 
  attachInterrupt(5, FC1_CHANGE, CHANGE);//PIN18 PD3 FC1
  attachInterrupt(4, FC2_CHANGE, CHANGE);//PIN19 PD2 FC2
  
    /* The most common pin for DMX output is pin 3, which DmxSimple
  ** uses by default. If you need to change that, do it here. */
  DmxSimple.usePin(15);

  /* DMX devices typically need to receive a complete set of channels
  ** even if you only need to adjust the first channel. You can
  ** easily change the number of channels sent here. If you don't
  ** do this, DmxSimple will set the maximum channel number to the
  ** highest channel you DmxSimple.write() to. */
  DmxSimple.maxChannel(512);
  strobe_control(0);
  time_strobe = millis();
  time_foco = millis();
  time_stop = millis();
  cont_foco = 0;
  cont_mov=0;
  if (digitalRead(18)&&digitalRead(19)) mode=0;
  else if((!digitalRead(18))&&digitalRead(19)) mode=1;
  else if(digitalRead(18)&&(!digitalRead(19))) mode=2;
} 

void strobe_control(int num, int val1, int val2)
  {
    DmxSimple.write(strobe[num], 255);
    DmxSimple.write(strobe[num]+4, 255);
    DmxSimple.write(strobe[num]+5, val1);
    DmxSimple.write(strobe[num]+6, val2);
//    for(int i=0; i<8; i++)
//    {
//      DmxSimple.write(strobe[0]+i, val);
//      DmxSimple.write(strobe[1]+i, val);
//      DmxSimple.write(strobe[2]+i, val);
//      DmxSimple.write(strobe[3]+i, val);
//    }
    
  }

void strobe_control(int val)
  {
    if (val==0)
      {
        for(int i=0; i<8; i++)
          {
            DmxSimple.write(strobe[0]+i, val);
            DmxSimple.write(strobe[1]+i, val);
            DmxSimple.write(strobe[2]+i, val);
            DmxSimple.write(strobe[3]+i, val);
          }
      }
    else
      {
        for(int i=0; i<4; i++)
          {
            strobe_control(i, 128, val);
          }
      }

  }
void foco_control(int num, int red, int green, int blue, int white)
  {
    DmxSimple.write(foco[num], red);
    DmxSimple.write(foco[num]+1, green);
    DmxSimple.write(foco[num]+2, blue);
    DmxSimple.write(foco[num]+3, white);
  }

void loop() 
{ 
   if (mode==0)
    {   
     flag_center=false;
     if (!flag_mode0)
      {
       strobe_control(0);
       foco_control(0, 0, 0, 0, 0);
       foco_control(1, 0, 0, 0, 0);
       foco_control(2, 0, 0, 0, 0);
       foco_control(3, 0, 0, 0, 0);
       flag_mode0=true;
      }
 
     if (Serial.available())
      {            
        c = Serial.read();
        //Serial.print(c);
  
        if ((c == '\r') || (c == '\n'))
        {
          if (inputString == "ping")
          {
            Serial.println("received ping to all");
            for (int i=0; i<8; i++)
              {
                time_escape=millis();
                while ((ping(id[i])==-1)&&(millis()-time_escape<500));
                //delay(1);
              }
          }
          else if (inputString.startsWith("ping "))
          {
            String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
            Serial.print("received ping to ");
            int direction = str.toInt();
            Serial.println(direction);
            ping(id[direction-1]);
          }
          if (inputString == "lim")
          {
            Serial.println("received limit to all");
            for (int i=0; i<8; i++)
              {
                limit[i] = lim(id[i]);
                time_escape=millis();
                while ((limit[i]==-1)&&(millis()-time_escape<500))
                  {
                    limit[i] = lim(id[i]);
                  }
                //delay(1);
              }
          }
          else if (inputString.startsWith("lim "))
          {
            String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
            Serial.print("received limit to ");
            int direction = str.toInt();
            Serial.println(direction);
            limit[direction-1] = lim(id[direction-1]);
            while (limit[direction-1]==-1)
              {
                limit[direction-1] = lim(id[direction-1]);
              }
          }
          else if (inputString == "center")
          {
            Serial.println("Center to all");
            for (int i=0; i<8; i++)
              {
                 sendData(id[i], 0, 100);
                 delay(100);
              }
          }
          else if (inputString.startsWith("center "))
          {
            String str = inputString.substring(inputString.lastIndexOf(" ") + 1);
            Serial.print("center to ");
            int direction = str.toInt();
            Serial.println(direction-1);
            sendData(id[direction-1], limit[direction-1]/2, 100);
          }
          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(id[direction-1]);
          }
          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(id[direction-1], angle, speed);
          }
          inputString = String();
  
        }
        else
          inputString += c;
      } 
    }
    else if (mode==1)
      {
        flag_mode0=false;
        if(!flag_center)
          {
            strobe_control(255);
            foco_control(0, 0, 0, 0, 255);
            foco_control(1, 0, 0, 0, 255);
            foco_control(2, 0, 0, 0, 255);
            foco_control(3, 0, 0, 0, 255);
            for (int i=0; i<=7; i++) sendData(id[i], 0, 100);
            delay(10000);
            center_all(70000);
            flag_center=true;
            strobe_control(0);
          }
        else
          {
            if (flag_ramp)
            {
             foco_control(0, 0, 0, 0, inc);
             foco_control(1, 0, 0, 0, inc);
             foco_control(2, 0, 0, 0, inc);
             foco_control(3, 0, 0, 0, inc);
             inc++;
             if(inc==255) flag_ramp=false;
             delay(10);
            }
            else
            {
               foco_control(0, 0, 0, 0, inc);
               foco_control(1, 0, 0, 0, inc);
               foco_control(2, 0, 0, 0, inc);
               foco_control(3, 0, 0, 0, inc);
               inc--;
               if(inc==0) flag_ramp=true;
               delay(10);
            }
          }       
      }
    else if (mode==2)
      {
        flag_center=false;
        flag_mode0=false;
        if ((millis()-time_foco)>500)
          {
            if (cont_foco==0)foco_control(3, 0, 0, 0, 0);
            else if(cont_foco>4) cont_foco=0;
            else foco_control(cont_foco-1, 0, 0, 0, 0);
            foco_control(cont_foco, 0, 0, 0, 255);
            strobe_control(0);
            time_foco=millis();
            cont_foco++;
          }
         if ((millis()-time_mov)>20000)
          {
            if (cont_mov==2)
              {
                strobe_control(random(20,255));
                foco_control(0, 0, 0, 0, 255);
                foco_control(1, 0, 0, 0, 255);
                foco_control(2, 0, 0, 0, 255);
                foco_control(3, 0, 0, 0, 255);
                delay(1000);
                for (int i=0; i<=7; i++) sendData(id[i], 0, 100);
                delay(2000);
                center_all(70000);
              }
            else if(cont_mov>=10) cont_mov=-1;
            else 
              {
                for(int i=0; i<=7; i++) 
                {
                  sendData(id[i], random(0,360), 100);
                  delay(100);
                }
                time_mov=millis();
              }
            Serial.println(cont_mov);
            cont_mov++; 
          }
        
        
      }
} 

void center_all(unsigned long time)
  {
//    for (int i=0; i<=7; i++)
//          {
//             sendData(id[i], 0, 0);
//             delay(1000);
//          }
    //sendData(id[i], 0, 0);
    calibrationData(101);
    calibrationData(102);
    calibrationData(103);
    calibrationData(104);
    calibrationData(105);
    calibrationData(106);
    calibrationData(107);
    calibrationData(108);
//    for (int i=0; i<=7; i++)
//      {
//         calibrationData(id[i]);
//         delay(1000);
//      }
        delay(time);
  }
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
  }

bool state = false;
int ping(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
      //delay(10);

      time_escape = millis();
      state = rs485.find("i");
      while(!state&&((millis()-time_escape)<=500))state = rs485.find("i");
      //while(rs485.find("i"))//&&((millis()-time_escape)<=500)) //esperamos el inicio de trama
      if (state)
      {
          int esclavo=rs485.parseInt();  //recibimos la direccion del esclavo
          int dato=rs485.parseInt();  //recibimos el dato
          if(rs485.read()=='f'&&esclavo==direction) //si fin de trama y direccion son los correctos
          {
             Serial.print(direction);
             Serial.print(" ");
             Serial.println(dato);
             return dato;
          }
      }
      return -1;
  }

int lim(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
      //delay(1);
      time_escape = millis();
      state = rs485.find("i");
      while(!state&&((millis()-time_escape)<=500))state = rs485.find("i");
      //while(rs485.find("i"))//&&((millis()-time_escape)<=500)) //esperamos el inicio de trama
      if (state)
      {
          int esclavo=rs485.parseInt();  //recibimos la direccion del esclavo
          int dato=rs485.parseInt();  //recibimos el dato
          if(rs485.read()=='f'&&esclavo==direction) //si fin de trama y direccion son los correctos
          {
             Serial.print(direction);
             Serial.print(" ");
             Serial.print(dato);
             Serial.print(" ");
             Serial.println(dato/2);
             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;
}