#include <Servo.h>
#include "HX711.h"
#include <Keypad.h>
#include <LiquidCrystal.h>

#define myServoPin 8
#define DOUT 3
#define CLK 2

Servo myServo;
HX711 balance;

using namespace std;

const int intPin = 21;

const int DIR = A8;
const int STEP = A9;

const int v0 = 13;
const int rs = 9;   // Pin 9  on Arduino to pin 4  (RS) on LCD
const int en = 10;  // Pin 10  on Arduino to pin 6  (E)  on LCD
const int d4 = 4;   // Pin 4 on Arduino to pin 11 (D4) on LCD
const int d5 = 5;   // Pin 5 on Arduino to pin 12 (D5) on LCD
const int d6 = 6;   // Pin 6  on Arduino to pin 13 (D6) on LCD
const int d7 = 7;   // Pin 7  on Arduino to pin 14 (D7) on LCD

LiquidCrystal lcd(rs, en, d4, d5, d6, d7);

const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

byte x;

const int C1 = 31;
const int C2 = 33;
const int C3 = 35;
const int C4 = 37;
const int R1 = 23;
const int R2 = 25;
const int R3 = 27;
const int R4 = 29;

uint8_t colPins[COLS] = { C1, C2, C3, C4 }; // Pins connected to C1, C2, C3, C4
uint8_t rowPins[ROWS] = { R1, R2, R3, R4 }; // Pins connected to R1, R2, R3, R4

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

boolean blink = false;

volatile enum state {WAITING, LOOPING, INGRESING} current_state = WAITING;

void setupBalance() {
  balance.begin(DOUT, CLK);
  Serial.print("Leyendo ADC");
  Serial.println(balance.read());
  delay(200);
  Serial.println("No coloque peso, calculando tara");
  balance.set_scale(0.42);
  balance.tare(10);
  Serial.print("Listo");
}

int data[10];
long w = 0;
int wObj = 0;

void loopBalance() {

  w = balance.get_units(10);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Peso: ");
  lcd.print(w);
  lcd.print("g");
  delay(50);
}

int pos = 0;
int mdir = 0;

void loopServosMovement() {
  for (pos = 0; pos <= 125; pos += 1 ) {
    myServo.write(pos);
    loopMotor();
    delay(15);
  }
  for (pos = 125; pos >= 0; pos -= 1 ) {
    myServo.write(pos);
    loopMotor();
    delay(15);
  }
}

void loopMotor() {
  for (mdir = 0; mdir <= 2; mdir += 1 ) {
    digitalWrite(STEP, HIGH);
    delayMicroseconds(40);
    digitalWrite(STEP, LOW);
    delayMicroseconds(40);
  }
}

unsigned long preMilis = 0;
const long interval = 1500;

void keypadEvent(KeypadEvent key) {
  switch (keypad.getState()) {
    case PRESSED:
      switch (key) {
        case 'A': current_state = INGRESING; break;
        case 'C': current_state = LOOPING; break; // enter the function with "C"
        case 'B': current_state = WAITING; break; // exit the function with "D"
      }
      break;
  }
}

boolean ingresar;

void setup() {
  Serial.begin(9600);
  setupBalance();

  analogWrite(v0, 50);
  lcd.begin(16, 2);
  pinMode(DIR, OUTPUT);
  pinMode(STEP, OUTPUT);
  keypad.addEventListener(keypadEvent); //add an event listener for this keypad

  myServo.attach(myServoPin);

  myServo.write(0);
  Serial.println("Servos en posición");

  digitalWrite(DIR, LOW);
  digitalWrite(STEP, LOW);

  lcd.print("Seleccione boton");

  pinMode(intPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(intPin), stater, CHANGE);

  ingresar = false;
}

void stater() {
  Serial.print("Terminado");
  lcd.clear();
  lcd.print("Seleccione boton");
  current_state = WAITING;
}

void loop() {
  char key = keypad.getKey();
  unsigned long currentMilis = millis();

  if (key == 'B') {
    Serial.print(wObj);
    wObj = 0;
    lcd.noCursor();
    lcd.clear();
  }
  if (key == 'A') {
    lcd.clear();
  }
  if (key != NO_KEY) {
    if (ingresar && (key != 'A' && key != 'B' && key != 'C' && key != 'D' && key != '*' && key != '#' )) {
      x = key & 0x0F;
      data[x] = key - 48;
      wObj = (wObj * 10) + data[x];
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Ingresar: ");
      lcd.setCursor(0, 1);
      lcd.print(wObj);
    }
  }

  switch (current_state) {
    case WAITING:
      break;
    case LOOPING:
      loopBalance();
      if ((w < wObj )) {
        lcd.setCursor(0, 1);
        lcd.print("Limit: ");
        lcd.print(wObj);
        if (currentMilis - preMilis >= interval) {
          loopServosMovement();
          loopMotor();
        }
      } else {
        lcd.setCursor(0, 1);
        lcd.print("Limite");
        delay(1500);
      }
      break;
    case INGRESING:
      ingresar = true;
      lcd.setCursor(0, 0);
      lcd.print("Ingresar: ");
      break;
  }

}