#include <AccelStepper.h>
#include <LCD_I2C.h>
#include <Wire.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,
     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 status1 = 0;
int status2 = 0;
int status3 = 0;
int status4 = 0;


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()
{
  // Initialize I2C as slave with address 4
  Wire.begin(3);
  // Register a function to be called when data is received
  Wire.onReceive(receiveEvent);

  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;
}

void receiveEvent(int howMany)
{
  // Read the received bytes and store them in the status variables
  if (howMany >= 4) 
  {
    status1 = Wire.read();
    status2 = Wire.read();
    status3 = Wire.read();
    status4 = Wire.read();
  }
}

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 move_to(int32_t destino, int motor, bool homing)
{
  int pin;
  AccelStepper* m;

  switch (motor)
  {
    case 1: m = &m1; pin = FC1; break; // aponta para m1 // assigna o fc1 como fim de curso
    case 2: m = &m2; pin = FC2; break;// aponta para m2 // assigna o fc2 como fim de curso
    case 3: m = &m3; pin = FC3; break;// aponta para m3 // assigna o fc3 como fim de curso
    default: Serial.println("Problema na função"); return;
  }

  if (homing) // verifica flag homing
  {
    while (!digitalRead(pin))
    {
      m->setSpeed(8000);
      m->run();
    }
    delay(100);
    m->setCurrentPosition(0);
    m->stop();
  }

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

void cortar()
{

  Serial.println("Iniciando corte...");
  display("INIC. CORTE", "......AGUARDE....:", -1);


  move_to(distancia * 5, 2, false);

  delay(500);

  move_to(distancia * 2.5, 2, false);
  cutted = true;

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

  gestao_tubo(comprimento, false);
  ready_to_cut = false;

}



void handle_cut(int size, int multiplier, const char* message)  // funcao de suporte de "posicionar"
{
  if (atual > size)   // verifica se o stock é maior
  {

    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()
{

  if (atual > 0)
  {

    if (size1 || size2 || size3) //só executa isto se umas das teclas for pressionada
    {
      posicionar();
      in_position = true;
    }

    else if (!in_position)  // apresenta o stand by se estiver na posição inicial
    {
      display("STAND BY...", "......STOCK:", atual);
    }

    if (seleccionado > 0 && atual != seleccionado && cut_plc_signal && ready_to_cut)
    {
      cortar(); // verifica ordem do plc
      if (cutted)
      {
        in_position = false;
        while (digitalRead(OUT_PLC))
        {
          display("EJETANDO...", "ESPERANDO SAIDA..", -1);
        }
        delay(100);
        cutted = false;
        display("ESP. NV. DIM", "......STOCK:", atual);

      }
    }


  }

  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);
    }
  }
}