#include <AccelStepper.h>
#include <LCD_I2C.h>

// Inputs
#define FC1 2              
#define FC2 1              
#define FC3 0              
#define SIZE1_PLC 7           
#define SIZE2_PLC 6           
#define SIZE3_PLC 5 
#define OUT_PLC 3
#define CUT_PLC 4 
#define TUBE_S1_PLC 8

// Outputs
#define OUT_OF_STOCK_PLC 13            
#define IN_POSITION_PLC 11
#define CUTTED_PLC 12           

LCD_I2C lcd(0x27, 16, 2);

AccelStepper m1(1, 10, 9);  
AccelStepper m2(1, A3, A2);  
AccelStepper m3(1, A1, A0);  

bool zero, done, led, cut_plc_signal, size1, size2, size3, tubo_presente, cutting,
out_of_stock, cut_done, ready_to_cut, ready_to_store, reset, cutted, in_position, tick;

int32_t distancia = -2000; 
int32_t new_position;
int barracm = 0;
int eixo1 = 0;
int counter = 0;
int atual = 0;
int comprimento = 0;
int seleccionado = 0;
int consumido = 0;
int cutsteps;

// Motor states
enum MotorState { IDLE, HOMING, MOVING };
MotorState motorStates[3] = { IDLE, IDLE, IDLE };

// Target positions
int32_t targetPositions[3];
AccelStepper* motors[3] = { &m1, &m2, &m3 };
int motorPins[3] = {FC1,FC2,FC3 };

ISR(TIMER1_COMPA_vect) 
{
  OCR1A += 50000; 
  counter++;  
  cut_plc_signal = !digitalRead(CUT_PLC);   
  size1 = !digitalRead(SIZE1_PLC); 
  size2 = !digitalRead(SIZE2_PLC); 
  size3 = !digitalRead(SIZE3_PLC); 
  tubo_presente = !digitalRead(TUBE_S1_PLC); 

  digitalWrite(CUTTED_PLC, cutted);
  digitalWrite(OUT_OF_STOCK_PLC, out_of_stock);
  digitalWrite(IN_POSITION_PLC, in_position); 

  tick = true;
}

void setup()
{  
  TCCR1A = 0;           
  TCCR1B = 0;           
  TCCR1B |= B00000100;  
  OCR1A = 50000;        
  TIMSK1 |= B00000010;  

  lcd.begin();
  lcd.backlight();

  pinMode(FC1, INPUT_PULLUP);
  pinMode(FC2, INPUT_PULLUP);
  pinMode(FC3, INPUT_PULLUP);
  pinMode(OUT_PLC, INPUT_PULLUP);
  pinMode(TUBE_S1_PLC, INPUT_PULLUP);
  pinMode(SIZE1_PLC, INPUT_PULLUP);
  pinMode(SIZE2_PLC, INPUT_PULLUP);
  pinMode(SIZE3_PLC, INPUT_PULLUP);
  pinMode(CUTTED_PLC, OUTPUT);
  pinMode(CUT_PLC, INPUT_PULLUP);
  pinMode(OUT_OF_STOCK_PLC, OUTPUT);

  Serial.begin(9600);

  m1.setMaxSpeed(20000);
  m1.setAcceleration(8000);
  m2.setMaxSpeed(20000);
  m2.setAcceleration(8000);
  m3.setMaxSpeed(20000);
  m3.setAcceleration(8000);

  lcd.setCursor(0, 0);
  lcd.print("INICIO....");
  delay(2000);

  move_to(0, 1, true);
  lcd.clear();
  lcd.print("HOMING E1 ok....");
  move_to(0, 2, true);
  lcd.clear();
  lcd.print("HOMING E2 ok....");
  move_to(0, 3, true);
  lcd.clear();
  lcd.print("HOMING E3 ok....");

  lcd.clear();

  out_of_stock = false;
  cut_done = true;
  cutsteps=1;

  
}

void display(const char* message1, const char* message2, int value)
{
  if (tick)
  {
    tick = false;

    lcd.clear();
    lcd.backlight();
    lcd.setCursor(0, 0);
    lcd.print(message1);
    lcd.setCursor(0, 1);
    
    if (value > 0)
    {
       lcd.print(message2);
       lcd.print(value);
    }
    else 
    {
      lcd.print(message2);
    }
  }
}

void move_to(int32_t destino, int motor, bool homing) 
{
    if (motor < 1 || motor > 3) {
        Serial.println("Problema na função");
        return;
    }

    int index = motor - 1;
    targetPositions[index] = destino;

    if (homing) 
    {
        motorStates[index] = HOMING;
       
    
        while (!digitalRead(motorPins[motor-1])) 
        {
            motors[index]->setSpeed(8000);
            motors[index]->run();
        }
        delay(100);
        motors[index]->setCurrentPosition(0);
        motors[index]->stop();
    
    } 
    else 
    {
        motors[index]->moveTo(targetPositions[index]);
        motorStates[index] = MOVING;
        Serial.print("Moving motor ");
        Serial.print(motor);
        Serial.print(" to position ");
        Serial.println(destino);
    }
}

void motor_handler()
{
    for (int i = 0; i < 3; i++) 
    {
        switch (motorStates[i]) 
        {
            case IDLE:
                // Do nothing
                break;

            case HOMING:
               
                
                break;

            case MOVING:
                if (motors[i]->distanceToGo() != 0) 
                {
                    motors[i]->run();
                } 
                break;
        }
    }
}

void cortar()
{
 


 if (m2.currentPosition()==-500 || m2.currentPosition()==-1000) { cutting=false; }

 switch (cutsteps)
 {

  case 1: 
    {
      if (!cutting)
      {
      Serial.println("Iniciando corte...");
      display("INIC. CORTE", "......AGUARDE....:", -1);

      move_to(-1000, 2, false);
      cutsteps++;
      cutting=true;
      
      }
    } break;

 case 2: 
    {
      if (!cutting)
      {
      Serial.println("Iniciando corte...");
      display("INIC. CORTE", "......AGUARDE....:", -1);

      move_to(-500, 2, false);
      cutsteps++;
      cutting=true;

      }
    } break;


 }
    
    
 

 

 //if (m2.currentPosition()== distancia * 2.5) // voltou
 //{
 //   Serial.println("Corte concluído.");
 //   display("CORTE CONCLUIDO",  "......STOCK:", atual);
 //  
 //   gestao_tubo(comprimento, false);
 //   ready_to_cut = false;
 //   cutting=false;
 //}
    
    
    

    
}


void handle_cut(int size, int multiplier, const char* message)
{
  if (atual > size)
  {
    int32_t new_position = (distancia * multiplier) - (consumido * -distancia);
    move_to(new_position, 1, false);
    ready_to_cut = true;
    Serial.print("Barra pronta para Corte de ");
    Serial.print(size);
    Serial.println(" cm.");
    delay(100);
    move_to(distancia * multiplier, 3, false); 
    display(message, "......STOCK:", atual);
    Serial.print("corte ");
    Serial.print(size);
    Serial.println("cm");
    comprimento = 5 * multiplier; // calculo de tubo gasto
    delay(100);
    
  }
}

void posicionar() 
{ 
  ready_to_store = false;

  if (size1) 
  {
    seleccionado = 5;
  } 
  else if (size2) 
  {
    seleccionado = 10;
  } 
  else if (size3) 
  {
    seleccionado = 15;
  }

  Serial.println(atual);
  Serial.println(seleccionado);

  if (atual == seleccionado) 
  {
    display("MEDIDA EXATA...", "", -1);
    delay(500);
    atual = 0;
    move_to(distancia * 5, 1, false); 
  }

  if (size1) 
  {
    handle_cut(5, 1, "5cm: AGUARDANDO..");
    size1 = false;
  } 
  else if (size2) 
  {
    handle_cut(10, 2, "10cm: AGUARDANDO..");
    size2 = false;
  } 
  else if (size3) 
  {
    handle_cut(15, 3, "15cm: AGUARDANDO..");
    size3 = false;
  }
}

void gestao_tubo(int baixa, bool tubonovo)
{
  if (tubonovo) { atual = 30; }
  if (baixa > 0) 
  {
    atual -= baixa;
  }
  Serial.print("Tubo disponivel: "); 
  Serial.println(atual);
}

void loop()
{
  motor_handler(); // TEM QUE ESTAR PRESENTE NO LOOP!!


  if (atual > 0) 
  {
    if (size1 || size2 || size3)
    {
     

      posicionar();

     

      in_position = true;

    }
    else if (!in_position)
    {
      display("STAND BY...", "......STOCK:", atual);
    }

    if (seleccionado > 0 && atual != seleccionado && cut_plc_signal && ready_to_cut)
    {
      
      cortar();

      if (cutted)
      {
        in_position = false;
        while (digitalRead(OUT_PLC))
        {
          display("EJETANDO...", "ESPERANDO SAIDA..", -1);
        }
        delay(100);
        cutted = false;
        display("ESP. NV. DIM", "......STOCK:", atual);
      }
    }

     if (cutting) { cortar(); } // continuar movimento da serra apos ordemm de corte

    
  }
 while (atual==0) 
 { 
   out_of_stock = true; 
   while (m1.currentPosition() != 0)
   {
     display("HOMING...", "AGUARDE...", -1);
     move_to(0, 1, false); 
   }
   display("ESPERANDO TUBO...", "" ,-1);
   if (tubo_presente) 
   {
     out_of_stock = false;
     atual = 30;
     display("TUBO PRESENTE...", "", -1);
   }
 }





 

  
}