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

#define FC1 2              // - fim de curso eixo 1
#define FC2 1              // - fim de curso eixo 2
#define FC3 0              // - fim de curso eixo 3
#define SIZE1 10           // - sinal PLC corte de 5 cm
#define SIZE2 11           // - sinal PLC corte de 10 cm
#define SIZE3 12           // - sinal PLC corte de 15 cm
#define LED 3              // - Led Amarelo Pisca Pisca
#define CUT 6              // BT RED Sinal do PLC da ordem de corte ao eixo 2
#define OUT_OF_STOCK 7           
#define IN_POSITION 5
#define OUT 4

#define CUTTED 8           // led vermelho barra cortada e armazem em posicao
#define TUBE_S1 13

LCD_I2C lcd(0x27, 16, 2);

AccelStepper m1(1, 9, 3);  // estruturas das librarias dos motores do eixo 1
AccelStepper m2(1, A3, A2);  // estruturas das librarias dos motores do eixo 2
AccelStepper m3(1, A1, A0);  // estruturas das librarias dos motores do eixo 3

bool zero, done, led, ready, size1, size2, size3, tubo_presente, out_of_stock, cut_done;  // variaveis booleanas
bool ready_to_cut, ready_to_store, reset,cutted,in_position;



int32_t distancia = -2000; // alterar aqui o valor de 5cm no eixo 1
int32_t new_position;
int barracm=0;
int eixo1=0;
int counter=0;
int atual=30;
int inicial=30;
int comprimento=0;
int seleccionado=01;
int consumido=0;


String displayTextLinha1 = "Esperando ordem...";
String displayTextLinha2 = "......";  

ISR(TIMER1_COMPA_vect) // interrupt ciclico
{
  OCR1A += 20000; 
  counter++; // timer para atualizar display
  
  swap_led();

 
 
  ready = !digitalRead(CUT);   // se o sinal de cortar for acionado
  size1 = !digitalRead(SIZE1); // tamanho de 5 cm for acionado
  size2 = !digitalRead(SIZE2); // tamanho de 10 cm for acionado
  size3 = !digitalRead(SIZE3); // tamanho de 15 cm for acionado
  tubo_presente = !digitalRead(TUBE_S1); // tubo na calha

  
  digitalWrite(CUTTED, cutted);
  digitalWrite(OUT_OF_STOCK, out_of_stock);
  digitalWrite(IN_POSITION, in_position); // saida a ser ligada quando tubo estiver pronto a ser cortado
 
}

void setup()
{  
  TCCR1A = 0;           // Init Timer1A
  TCCR1B = 0;           // Init Timer1B
  TCCR1B |= B00000100;  // Prescaler = 64
  OCR1A = 30000;        // Timer Compare1A Register
  TIMSK1 |= B00000010;  // Enable Timer COMPA Interrupt

   lcd.begin(); // If you are using more I2C devices using the Wire library use lcd.begin(false)
                 // this stop the library(LCD_I2C) from calling Wire.begin()
   lcd.backlight();

  pinMode(FC1, INPUT_PULLUP);
  pinMode(FC2, INPUT_PULLUP);
  pinMode(FC3, INPUT_PULLUP);
  pinMode(OUT, INPUT_PULLUP);

  pinMode(TUBE_S1, INPUT_PULLUP);

  pinMode(SIZE1, INPUT_PULLUP);
  pinMode(SIZE2, INPUT_PULLUP);
  pinMode(SIZE3, INPUT_PULLUP);
 
  //pinMode(READY_TO_STORE, OUTPUT);
 
  pinMode(CUTTED, OUTPUT);

  pinMode(LED, OUTPUT);
  pinMode(CUT, INPUT_PULLUP);
  pinMode(OUT_OF_STOCK, OUTPUT);

  Serial.begin(9600);

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

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

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

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

  zero_position(1);
  lcd.clear();
  lcd.print("homing e1 ok....");
  zero_position(2);
  lcd.clear();
  lcd.print("homing e2 ok....");
  zero_position(3);
  lcd.clear();
  lcd.print("homing e3 ok....");

  lcd.clear();

  //if ( !digitalRead(TUBE) ) {out_of_stock=false;}
  //else {out_of_stock=true;}
  out_of_stock=false;
  cut_done=true;
}

void display()
{

    if (counter==0)  // muda a informação do display no primeiro ciclo de interrupt
     {
     lcd.print(displayTextLinha1);
     lcd.setCursor(8, 1);
     lcd.print("STOCK:");
     lcd.print(atual);
     lcd.setCursor(1,0);

     lcd.backlight();
     counter++;
     }   

     if (counter > 5) // ao decimo atualiza/apaga
     {
       lcd.clear();
       counter=0;
     }   
}

void zero_position(int motor)
{
  switch (motor) 
  {
    case 1:
      while (digitalRead(FC1) == false) 
      {
        m1.setSpeed(8000);
        m1.run();
      }
      delay(100);
      m1.setCurrentPosition(0);
      m1.stop();
      break;

    case 2:
      while (digitalRead(FC2) == false) 
      {
        m2.setSpeed(8000);
        m2.run();
      }
      delay(100);
      m2.setCurrentPosition(0);
      m2.stop();
      break;

    case 3:
      while (digitalRead(FC3) == false) 
      {
        m3.setSpeed(8000);
        m3.run();
      }
      delay(100);
      m3.setCurrentPosition(0);
      m3.stop();
      break;

    default:
      Serial.println("Problema na função zero_position");
      break;
  }
 
}

void move_to(int32_t destino, int motor)
{
  switch (motor) 
  {
    case 1:
      m1.moveTo(destino);
      m1.runToPosition();
      
      break;

    case 2:
      m2.moveTo(destino);
      m2.runToPosition();
      break;

    case 3:
      m3.moveTo(destino);
      m3.runToPosition();
      break;

    default:
      Serial.println("Problema na função move_to");
      break;
  }
}

bool cortar(bool cut)
{
  
  if (cut) 
  {
    Serial.println("Iniciando corte...");

    displayTextLinha1 = "Iniciando corte...";
    displayTextLinha2 = atual;

    display();

      move_to(distancia *5 , 2);
      delay(250);
      displayTextLinha1 = "recolha de serra";
      displayTextLinha2 = atual;
      display();

      delay(500);

      
      move_to(distancia *2.5 ,2);
      cutted = true;   
      Serial.println("Corte concluído.");
      displayTextLinha1 = "Corte concluido.";
      displayTextLinha2 = atual;
      
      
    
      gestao_tubo(comprimento,false);

      display();

      
      
      
      ready_to_cut=false;
      return cutted;
      
  }

  
}

void swap_led()
{
  digitalWrite(LED, led ? HIGH : LOW);
  led = !led;
}

void posicionar()
{ 
  
  ready_to_store = false;

  if (atual == seleccionado && (size1 || size2 || size3))  //verificar se já tem a medida, se sim não corta
  {
  displayTextLinha1 = "medida exata...";
  display();
  delay (500);
  atual=0;
  move_to(distancia * 5, 1); // 30cm
  
  }

  if (size1 && atual > 5) // medida de 5cm
  {
    
    new_position = distancia - (consumido * - distancia);
    move_to(new_position, 1);
    ready_to_cut = true;

    Serial.println("Barra pronta para Corte de 5 cm.");
    

    delay(500);

    move_to(distancia, 3); /// movimento armazem

    //ready_to_store = true;

    Serial.println("corte 5cm");
    displayTextLinha1 = "5cm: aguardando..";
    display();
    comprimento = 5; // comprimento cortado
    consumido=consumido+1;

    size1=false;
    delay(500);

    
  }

  if (size2 && atual > 10) // medida de 10cm
  {
   
   
    
    int32_t dez = distancia*2;
     new_position = dez - (consumido * - distancia);

    move_to(new_position, 1);
    ready_to_cut = true;
  
    Serial.println("Barra pronta para Corte de 10 cm.");

    delay(500);

    move_to(distancia *2 , 3); /// movimento armazem

   

    Serial.println("corte 10cm");
    displayTextLinha1 = "10cm: aguardando..";

    display();
 
    comprimento = 10; // comprimento cortado
    consumido=consumido+2;
    size2=false;
    delay(500);
  }

  if (size3 && atual > 15) // medida de 15cm
  {
   
    
    int32_t quinze = distancia*3;
     new_position = quinze - (consumido * - distancia);

    move_to(new_position, 1);
    ready_to_cut = true;
  
    Serial.println("Barra pronta para Corte de 15 cm.");

    delay(500);

    move_to(distancia *3, 3); /// movimento armazem


    
    Serial.println("15cm: aguardando ordem de corte");
    displayTextLinha1 = "15cm: aguardando..";

    display();

    comprimento = 15; // comprimento posicionado
    
    size3=false;
    consumido=consumido+3;
    delay(500);
    
  }

  seleccionado = comprimento;



}
  


  


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

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

}
void loop()
{
     

displayTextLinha2 = atual;
display();

if (atual > 0) 
{
 
  posicionar();
  

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


  


    }
if (cutted)
{

 while (digitalRead(OUT)==true)
   {
   displayTextLinha1 = "SAINDO...";
   display();
   
   }
   cutted=false;


displayTextLinha1 = "ESP. NOVA DIM";
display();

}

  
  
}

while (atual==0) 
{ 

  while (m1.currentPosition() != 0)
  {
  displayTextLinha1 = "homing...";
  display();
  move_to(0, 1); //E1 retorna a 0;
  }
    

  displayTextLinha1 = "esperando tubo...";
  display();

  out_of_stock = true; // diz ao plc que nao tem tubo 

 if (tubo_presente) // chegou tubo
 {
   out_of_stock = false;
   atual=30;
   displayTextLinha1 = "stock reposto";
  
 }
 
}

display();
    

    
}