//Carlos Cuellar 19275
#include <Servo.h>
#include <AccelStepper.h>
AccelStepper stepper(1,3,2);
Servo servo1;
const byte numBytes = 32;
byte receivedBytes[numBytes];
byte numReceived = 0;
String dato1user;
String dato2user;
String dato3user;
String dato4user;
String dato5user;

boolean newData = false;
byte temp[numBytes];
int menu=1;
int menuStep=0;
int menuServ=0;
int menuLed=0;
int menuPots=0;
int menuAll=0;
int ingreso=0;
int steps=0;
int contsteps=0;
int controlador=1;
int lectura_principal=1;
int signo_dir=0;
int LEDseleccionado=0;
int toggle1=0;
int toggle2=0;
int toggle3=0;
int tiempoLED1=500;
int tiempoLED2=500;
int tiempoLED3=500;
long tiempoold1=millis();
long tiemponew1=0;
long tiempoold2=millis();
long tiemponew2=0;
long tiempoold3=millis();
long tiemponew3=0;
int contl1=0;
int contl2=0;
int contl3=0;
int pot1=0;
int pot2=0;
int pot3=0;
int valorservo=0;
int valorstep=0;
char *name = NULL;
int todo[6];
void setup() {
    Serial.begin(9600);
    Serial.println("<Arduino is ready>");
    servo1.attach(12);
    stepper.setMaxSpeed(500);
    stepper.setAcceleration(50);
    pinMode(11, OUTPUT);
    pinMode(9, OUTPUT);
    pinMode(10, OUTPUT);
}

void loop() {
    if(lectura_principal==1){
    recvBytesWithStartEndMarkers();
    }
    switch (controlador) {
  case 1:
    if(menu==1){
      Serial.println("Seleccione una opcion");
      Serial.println("1 Mover Servo");
      Serial.println("2 Mover Stepper");
      Serial.println("3 Manejar LEDs");
      Serial.println("4 Estado Sistema");
      Serial.println("5 Manejar TODOS los actuadores");
      menu=0;
      controlador=2;
    }
    break;
  case 2:
    if(atoi(temp)==1 && menu==0 && menuServ==0){
      Serial.println("Ingrese la posicion del Servo");
      menuServ=1;
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=3;
    }
    if(atoi(temp)==2 && menu==0 && menuStep==0){
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      Serial.println("Ingrese 1 para los grados que movera el step o 2 para la direccion");
      controlador = 9;
      newData=false;
    }
    if(atoi(temp)==4 && menu==0 && menuPots==0){
      pot1=analogRead(A0);
      pot2=analogRead(A1);
      pot3=analogRead(A2);
      Serial.print("Valor pot1: ");
      Serial.println(pot1);
      Serial.print("Valor pot2: ");
      Serial.println(pot2);
      Serial.print("Valor pot3: ");
      Serial.println(pot3);
      Serial.print("Valor Servo: ");
      Serial.println(valorservo);
      Serial.print("Valor Step y dir: ");
      Serial.println(valorstep);
      Serial.print("Valor Periodo LED1: ");
      Serial.println(tiempoLED1);
      Serial.print("Valor Periodo LED2: ");
      Serial.println(tiempoLED2);
      Serial.print("Valor Periodo LED3: ");
      Serial.println(tiempoLED3);

      menuPots=0;
      memset(temp, 0, sizeof(temp));
      memset(receivedBytes, 0, sizeof(receivedBytes));
      menu=1;
      newData=false;
      controlador=1;
    }
    if(atoi(temp)==5 && menu==0 && menuAll==0){
      Serial.println("Ingrese la posicion del Servo, steps y direccion del Step y periodo de los 3 leds");
      menuAll=1;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=7;
    }
    if(atoi(temp)==3 && menu==0 && menuLed==0){
      Serial.println("Ingrese 9, 10 o 11 para elegir un LED");
      menuLed=1;
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=8;
      
    }
    break;
  case 3:
    if(menuServ=1 && newData==true){
      menuServ=0;
      menu=1;
      newData=false;
      valorservo=atoi(receivedBytes);
      servo1.write(atoi(receivedBytes));
      controlador=1;
    }
    break;
  case 4:
      if(atoi(temp)==1 && newData==true){
        signo_dir=1;
        Serial.println("enciende");
        menuStep=0;
      menu=1;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=1;
      }
      else if(atoi(temp)==0 && newData==true){
        signo_dir=0;
        Serial.println("apaga");
      menuStep=0;
      menu=1;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=1;
      }
      
    break;
  case 5:
    if(signo_dir==0 && newData==true){
      moveToPosition(-atoi(temp));
      valorstep=-atoi(temp);
      menu=1;
      menuStep=0;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=1;
    }
    else if(signo_dir==1 && newData==true){
      moveToPosition(atoi(temp));
      valorstep=atoi(temp);
      menu=1;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=1;
    }
    break;
  case 6:
    // statements
    break;
  case 7:
    if(newData==true){
      int j=0;
      name = strtok(receivedBytes, " ");
      //todo[j]=atoi(name);
    while(name != NULL)
    {
        j++;
        name = strtok(NULL, " ");
        todo[j]=atoi(name); 
        //Serial.println(todo[j]);
        
        
    }
    valorservo=todo[1];
    servo1.write(todo[1]);
    valorstep=todo[2];
    moveToPosition(todo[2]);
    tiempoLED1=todo[3];
    tiempoLED2=todo[4];
    tiempoLED3=todo[5];
    Serial.println(todo[0]);
    Serial.println(todo[1]);
    Serial.println(todo[2]);
    Serial.println(todo[3]);
    Serial.println(todo[4]);
    Serial.println(todo[5]);
    Serial.println(todo[6]);

      menuAll=0;
      memset(temp, 0, sizeof(temp));
      memset(receivedBytes, 0, sizeof(receivedBytes));
      menu=1;
      newData=false;
      controlador=1;

    }
    break;
  case 8:
    if(newData==true){
    LEDseleccionado=atoi(receivedBytes);
    Serial.println("Decida el tiempo en ms del parpadeo de LED seleccionado");
    Serial.println(atoi(LEDseleccionado));
    memset(receivedBytes, 0, sizeof(receivedBytes));
    memset(temp, 0, sizeof(temp));
    newData=false;
    controlador=10;
    }
    // statements
    break;
  case 9:
      //Serial.println("entra");
      //recvBytesWithStartEndMarkers();
      //lectura_principal=0;
      if(atoi(receivedBytes)==1 && newData==true){
      Serial.println("Ingrese la cantidad de pasos");
      menuStep=1;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=5;
      lectura_principal=1;
      }
      else if(atoi(receivedBytes)==2 && newData==true){
      Serial.println("Ingrese 1 para la izq y 0 para la derecha");
      menuStep=1;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=4;
      lectura_principal=1;
      
      }
    // statements
    break;
  case 10:
  //Serial.println(LEDseleccionado);
  if(LEDseleccionado==9 && newData==true){
    tiempoLED1=atoi(receivedBytes);
      menu=1;
      menuLed=0;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=1;
  }
  if(LEDseleccionado==10 && newData==true){
    tiempoLED2=atoi(receivedBytes);
      menu=1;
      menuLed=0;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=1;
  }
  if(LEDseleccionado==11 && newData==true){
    tiempoLED3=atoi(receivedBytes);
      menu=1;
      menuLed=0;
      memset(receivedBytes, 0, sizeof(receivedBytes));
      memset(temp, 0, sizeof(temp));
      newData=false;
      controlador=1;
  }
    // statements
    break;

}
    
    

    
   /* if(menuLed=1 && ingreso==1){
      menuLed=0;
      menu=1;
      newData=false;
    }
    if(menuAll=1 && ingreso==1){
      menuAll=0;
      menu=1;
      newData=false;
    }
*/

    //showNewData();
    
    tiemponew1=millis();
    if((tiemponew1-tiempoold1)>=tiempoLED1){
      
      if (toggle1==1){
        digitalWrite(9, HIGH);
        toggle1=0;
      }
      else{
        digitalWrite(9, LOW);
        toggle1=1;
      }
      
      tiempoold1 = millis();
    }
    tiemponew2=millis();
    if((tiemponew2-tiempoold2)>=tiempoLED2){
      
      if (toggle2==1){
        digitalWrite(10, HIGH);
        toggle2=0;
      }
      else{
        digitalWrite(10, LOW);
        toggle2=1;
      }
      
      tiempoold2 = millis();
    }
    tiemponew3=millis();
    if((tiemponew3-tiempoold3)>=tiempoLED3){
      
      if (toggle3==1){
        digitalWrite(11, HIGH);
        toggle3=0;
      }
      else{
        digitalWrite(11, LOW);
        toggle3=1;
      }
      
      tiempoold3 = millis();
    }
    }


void recvBytesWithStartEndMarkers() {
    static boolean recvInProgress = false;
    static byte ndx = 0;
    byte startMarker = 0x3C;
    byte endMarker = 0x3E;
    byte rb;
   

    while (Serial.available() > 0 && newData == false) {
        rb = Serial.read();

        if (recvInProgress == true) {
            if (rb != endMarker) {
                receivedBytes[ndx] = rb;
                ndx++;
                if (ndx >= numBytes) {
                    ndx = numBytes - 1;
                }
            }
            else {
                receivedBytes[ndx] = '\0'; // terminate the string
                recvInProgress = false;
                numReceived = ndx;  // save the number for use when printing
                ndx = 0;
                newData = true;
                memset(temp, 0, sizeof(temp));
                for(int i=0; i< numReceived;i++){
                  temp[i]=receivedBytes[i];
                }
                //Serial.println(atoi(receivedBytes));
                //Serial.println(atoi(temp));
                
            }
        }

        else if (rb == startMarker) {
            recvInProgress = true;
        }
    }
}

void showNewData() {
  dato1user=receivedBytes[0];
  dato2user=receivedBytes[2];
  dato3user=receivedBytes[4];
  dato4user=receivedBytes[5];
  dato5user=receivedBytes[6];
    if (newData == true) {
        Serial.print("This just in (HEX values)... ");
        for (byte n = 0; n < numReceived; n++) {
            Serial.print(receivedBytes[n]);
            Serial.print(' ');
        }
        Serial.println();
        
        newData = false;
    }
}
void moveToPosition(long position) {
  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(500);
  stepper.moveTo(position);
  while (stepper.distanceToGo() != 0) {
    stepper.run();
  }
}