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

// Inputs
#define FC1 4
#define FC2 3
#define FC3 2

#define E1F 23
#define E1R 25

#define E2F 27
#define E2R 29

#define E3F 31
#define E3R 33


#define SIZE1_PLC A0
#define SIZE2_PLC A1
#define SIZE3_PLC A2
#define OUT_PLC A5
#define CUT_PLC A3
#define GERINDO_SOBRAS A13
#define ORDEM_START A4
#define ORDEM_STOP A5
#define SOBRAS A6

// Outputs
#define HOMING_OK_PLC A8
#define OUT_OF_STOCK_PLC A9
#define IN_POSITION_PLC A10
#define CUTTED_PLC A11
#define HOMING1_OK_PLC A12

LCD_I2C lcd(0x27, 16, 2);
AccelStepper m1(1, 6, 5);
AccelStepper m2(1, 8, 7);
AccelStepper m3(1, 10, 9);

bool done, led, cut_plc_signal, size1, size2, size3, tubo_presente, homing_ok, ordem_start, ordem_stop, homing1_ok_plc,
  e1_frente, e1_tras, e2_frente,e2_tras, e3_frente, e3_tras,
  out_of_stock, cut_done, ready_to_cut, ready_to_store, reset, 
  cutted, in_position, tick, sobras, gerindo_sobras;

int32_t distancia = -1000;
int32_t distancia_corte_total = -1000;
int32_t distancia_max_e1 = -6000; // distancia * 6
int32_t armazem_dez = -1000;
int32_t armazem_quinze = -2000;
int32_t armazem_vinte = -3000;
int32_t armazem_sobras = -4000;

int32_t new_position;
int barracm = 0;
int eixo1 = 0;
int counter = 0;
int atual = -1;
int comprimento = 0;
int seleccionado = 0;
int consumido = 0;

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

  e1_frente = !digitalRead(E1F);
  e1_tras = !digitalRead(E1R);
  e2_frente = !digitalRead(E2F);
  e2_tras = !digitalRead(E2R);
  e3_frente = !digitalRead(E3F);
  e3_tras = !digitalRead(E3R);

  ordem_start = !digitalRead(ORDEM_START);
  ordem_stop = !digitalRead(ORDEM_STOP);
  sobras = !digitalRead(SOBRAS);

  digitalWrite(CUTTED_PLC, cutted);
  digitalWrite(OUT_OF_STOCK_PLC, out_of_stock);
  digitalWrite(IN_POSITION_PLC, in_position);
  digitalWrite(HOMING_OK_PLC, homing_ok);
  digitalWrite(HOMING1_OK_PLC, homing1_ok_plc);
  digitalWrite(GERINDO_SOBRAS, gerindo_sobras);

  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(E1F, INPUT_PULLUP);
  pinMode(E1R, INPUT_PULLUP);
  pinMode(E2F, INPUT_PULLUP);
  pinMode(E2R, INPUT_PULLUP);
  pinMode(E3F, INPUT_PULLUP);
  pinMode(E3R, INPUT_PULLUP);
  
  pinMode(SOBRAS, INPUT_PULLUP);
  pinMode(SIZE1_PLC, INPUT_PULLUP);
  pinMode(SIZE2_PLC, INPUT_PULLUP);
  pinMode(SIZE3_PLC, INPUT_PULLUP);
  pinMode(ORDEM_START, INPUT_PULLUP);
  pinMode(ORDEM_STOP, INPUT_PULLUP);
  pinMode(CUTTED_PLC, OUTPUT);
  pinMode(CUT_PLC, INPUT_PULLUP);
  pinMode(OUT_OF_STOCK_PLC, OUTPUT);
  pinMode(HOMING_OK_PLC, OUTPUT);

  Serial.begin(9600);

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

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("inicio....");
  lcd.setCursor(0, 1);
  lcd.print("AG. START PLC");
  lcdblink();

  out_of_stock = false;
  cut_done = true;
  ordem_start = false;
  homing_ok = false;
 
}

void display(const char* message1, const char* message2, int value) {
  if (tick) { // atualiza no interrupt
    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);
    }

    lcd.setCursor(1, 0);
  }
}

void homing_all() 
{
  lcd.clear();
  lcd.print("HOMING....");
  move_to(0, 1, true);
  lcd.clear();
  homing1_ok_plc=true;
  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();
  homing_ok=true;
}

void move_to(int32_t destino, int motor, bool homing) {
  int pin;
  AccelStepper* m;


 homing_ok=false; // homing reset


  switch (motor) 
  {
    case 1: m = &m1; pin = FC1; homing1_ok_plc=false; break;
    case 2: m = &m2; pin = FC2; break;
    case 3: m = &m3; pin = FC3; break;
    default: Serial.println("Problema na função"); return;
  }

  if (homing) 
  {
    lcd.clear();
    display("homing....", "ocupado....",-1);
    while (!digitalRead(pin)) 
    {
      m->setSpeed(8000);
      m->run();
    }
    delay(100);
    m->setCurrentPosition(0);
    m->stop();
  }

  m->moveTo(destino);
  m->runToPosition();
}

void cortar() // manda cortar e regista consumo
{
  Serial.println("Iniciando corte...");
  display("INIC. CORTE", "......AGUARDE....:", -1);

  move_to(distancia_corte_total, 2, false);
  delay(500);
  move_to(0, 2, false);
  cutted = true;

  Serial.println("Corte concluído.");
  display("CORTE CONCLUIDO",  "......STOCK:", atual);
 
  gestao_tubo(comprimento, false);

  ready_to_cut = false;
}

void lcdblink() {
  for (int i = 0; i < 10; i++) {
    lcd.noBacklight();
    delay(200);
    lcd.backlight();
    delay(200);
  }
}

void posicionar() {
  ready_to_store = false;

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

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

  if (atual == seleccionado) 
  {
    display("MEDIDA EXATA...", "", -1);
    delay(1000);
    atual = 0;
    move_to(distancia_max_e1 , 1, false);
  }

  if (size1) {
    handle_store(10, "10cm: AGUARDANDO..");
    size1 = false;
    gerindo_sobras=false;
  }
  if (size2) {
    handle_store(15, "15cm: AGUARDANDO..");
    size2 = false;
    gerindo_sobras=false;
  }
  if (size3) {
    handle_store(20, "20cm: AGUARDANDO..");
    size3 = false;
    gerindo_sobras=false;
  }
}

void handle_store(int size, const char* message) 
{
  if (atual > size) 
  {
    int multiplier = size / 5;
    int32_t new_position = (distancia * multiplier) + (consumido * distancia);
    
    move_to(new_position, 1, false);
    ready_to_cut = true;
    Serial.println(new_position);
    Serial.println(consumido);
    

    Serial.print("Barra pronta para Corte de ");
    Serial.print(size);
    Serial.println(" cm.");
    delay(100);

    switch (size)  // movimentar armazem
    {
      case 5:  move_to(armazem_sobras, 3, false); break;
      case 10: move_to(armazem_dez, 3, false);  break;
      case 15: move_to(armazem_quinze, 3, false); break;
      case 20: move_to(armazem_vinte, 3, false);  break;
      default: break;
    }

    display(message, "......STOCK:", atual);
    Serial.print("pronto p/ corte ");
    Serial.print(size);
    Serial.println("cm");
    comprimento = 5 * multiplier; // calculo de tubo gasto
    delay(100);
  }
}

void ejetar_automaticamente() 
{
  if (atual==10) 
  {
    gerindo_sobras=true;
    ready_to_cut = false;
    display("EXATA 10CM", atual, -1);
   
    lcdblink();
    atual = 0;
    
    move_to(armazem_dez, 3, false);
    move_to(distancia_max_e1, 1, false);
  } 
  else if (atual == 5) 
  {
    gerindo_sobras=true;
    ready_to_cut = false;
    display("SOBRA DE 5CM", atual, -1);
    lcdblink();
    atual = 0;
    move_to(armazem_sobras, 3, false);
    move_to(distancia_max_e1, 1, false);
  }
}

void gestao_tubo(int baixa, bool tubonovo)  // dar baixa e repor
{
  if (tubonovo) 
  {
    atual = 30;
  }

  if (baixa > 0) 
  {
    atual -= baixa;
  }
  Serial.print("Tubo disponivel:"); Serial.println(atual);

   switch (seleccionado)  // dar baixa
    {
      case 5:  ; break;
      case 10: consumido += 2;  break;
      case 15: consumido += 3; break;
      case 20: consumido += 4;  break;
      default: break;
    }
}

void reset_system(bool stop) 
{
  m1.stop();
  m2.stop();
  m3.stop();

  atual = -1;
  comprimento = 0;
  seleccionado = 0;
  consumido = 0;
  //homing_ok = false;
  //homing1_ok_plc = false;
  out_of_stock = true;
  cut_done = true;
  ordem_start = false;
  in_position = false;
  gerindo_sobras=false;

if (stop)
{
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("STOP");
  lcd.setCursor(0, 1);
  lcd.print("retirar tubo...");
  lcdblink();


}
  

}

void manual()
{
    if (!digitalRead(E1F) && digitalRead(E1R)) // move e1 se so m1 for pressionado
     {
      m1.setSpeed(-8000);
      m1.run();
     }

    if (digitalRead(E1F) && !digitalRead(E1R))
     {
      m1.setSpeed(8000);
      m1.run();
     }

     if (!digitalRead(E2F) && digitalRead(E2R)) // move e1 se so m1 for pressionado
     {
      m2.setSpeed(-8000);
      m2.run();
     }

     if (digitalRead(E2F) && !digitalRead(E2R))
     {
      m2.setSpeed(8000);
      m2.run();
     }

    

     if (!digitalRead(E3F) && digitalRead(E3R)) // move e3 frente
     {
      m3.setSpeed(-8000);
      m3.run();
     }

     if (!digitalRead(E3R) &&  digitalRead(E3F) ) // move e3 frente
     {
      m3.setSpeed(8000);
      m3.run();
     }


}

void loop() 
{
  if (ordem_stop) 
  {
    reset_system(true); // reset com stop
    setup();
    return;
  }

 
 manual(); 
   
   


  if (ordem_start && !ordem_stop && !homing_ok) 
  {
    
    homing_all();
    atual = 0;
   
   
  }

  

  if (atual > 0 && !ordem_stop) 
  {
    ejetar_automaticamente();
    
    manual();
    if (size1 || size2 || size3) 
    {
      posicionar();
      in_position = true;
    } 
    
    else if (!in_position) 
    {
      display("esperando ordem...", "......STOCK:", atual);
     
      
    }

    if (seleccionado > 0 && atual != seleccionado && cut_plc_signal && ready_to_cut) 
    {
      cortar();
      if (cutted) 
      {
        in_position = false;
        
          display("Corte OK...", "ESPERANDO SAIDA..", -1);
        
        lcdblink();
     

        cutted = false;
        display("ESP. NV. DIM", "......STOCK:", atual);
      }
    }
  }

  if (atual == 0)
   {
    out_of_stock = true;
    while (m1.currentPosition() != 0) 
    {
      display("HOMING... E1", "AGUARDE...", -1);
      move_to(0, 1, true); // homing de fim d curso
      homing1_ok_plc = true;
    }
    reset_system(false); // voltar ao inicio
    display(".............", "def. med. a cortar", -1);
    gestao_tubo(0, true); //repor stock 
    lcdblink(); // serve de wait para plc
    lcdblink();
    out_of_stock = false;
   
    
    
    
  }
}