#include <Stepper.h>
#include <Servo.h>
#include <Wire.h>  // LCD
#include <LiquidCrystal_I2C.h> // LCD
#include <EEPROM.h>

// EEPROM

// how many liquid is there? in (ml)
#define add_bot1 0
#define add_bot2 1
#define add_bot3 2
#define add_bot4 3


// WEIGHT SENSOR

typedef struct {
  int initialWeight;
  int weight;

  int pos;
  int neg;

  int getWeight() {
    weight = 2*(pos - neg) + 10 - initialWeight; // TODO transform function for 50ml?
    Serial.println("sensor" + weight);
    return weight;
  }

  void setInitialWeight() {
    initialWeight = getWeight();
  }
} s_weightSensor;

s_weightSensor weightSensor;

// STATES

typedef enum {
  INIT,
  CORE
} 
states;

states state = INIT;

// Database
const int MAX_QUANTITY = 6;
int currentCocktail = 0;

typedef struct {
  String name;
  int q1;
  int q2;
  int q3;
  int q4;

  String getName() {
    return name;
  }

  int _validate(int quantity) {
    if (quantity >= 0 && quantity < MAX_QUANTITY) {
      return quantity;
    }
    return 0;
  }

  int getQuantity(int num) {
    switch(num) {
    case 0:
      return _validate(q1);
      break;
    case 1:
      return _validate(q2);
      break;
    case 2:
      return _validate(q3);
      break;
    case 3:
      return _validate(q4);
      break;
    default:
      return 0;
      break;
    }
  }

  void insert(String c_name, int c_q1, int c_q2, int c_q3, int c_q4) {
    name = "EMPTY";
    q1 = 0;
    q2 = 0;
    q3 = 0;
    q4 = 0;
    if ((c_q1 + c_q2 + c_q3 + c_q4) <= MAX_QUANTITY) {
      name = c_name;
      q1 = c_q1;
      q2 = c_q2;
      q3 = c_q3;
      q4 = c_q4;
    }
  }

} 
cocktail;

// COCKTAILS RECIPES
const int NUM_COCKTAILS = 7;
cocktail cock[NUM_COCKTAILS];

void makeDatabase() { 
  cock[0].insert("CO1",1,1,1,1);
  cock[1].insert("CO2",0,0,0,1);
  cock[2].insert("CO3",1,0,0,0);
  cock[3].insert("CO4",0,2,0,1);
  cock[4].insert("CO5",0,0,0,4);
  cock[5].insert("CO6",1,0,0,3);
  cock[6].insert("CO7",0,0,0,5);
}

// Magnet Sensor

int magInPin = 13;

// servo
int servoPin = 10;
Servo servo;

// Stepper
int m1in1Pin = 8;
int m1in2Pin = 9;
int m1in3Pin = 10;
int m1in4Pin = 11;

Stepper motor1(200, m1in1Pin, m1in2Pin, m1in3Pin, m1in4Pin);

// secondary motor
int m2stepPin = 3;
int m2dirPin = 4;
int m2speed = 4000;
int m2turnAngle = 90;
float m2ratio = 5;
int m2direction = 1;
int m2x;
int m2readPin;

// LCD
int inPinButton = 0;
LiquidCrystal_I2C lcd(0x20,16,2);  // set the LCD address to 0x20 for a 16 chars and 2 line display

int lcd_key     = 0;
int adc_key_in  = 0;
#define btnRIGHT  0
#define btnUP     1
#define btnDOWN   2
#define btnLEFT   3
#define btnSELECT 4
#define btnNONE   5

// read the buttons
int getButton()
{
  adc_key_in = analogRead(inPinButton);      // read the value from the sensor 
  // my buttons when read are centered at these valies: 0, 144, 329, 504, 741
  // we add approx 50 to those values and check to see if we are close
  if (adc_key_in > 1000) return btnNONE; // We make this the 1st option for speed reasons since it will be the most likely result

  if (adc_key_in < 50)   return btnRIGHT;  
  if (adc_key_in < 195)  return btnUP; 
  if (adc_key_in < 380)  return btnDOWN; 
  if (adc_key_in < 555)  return btnLEFT; 
  if (adc_key_in < 790)  return btnSELECT;   

  return btnNONE;  // when all others fail, return this...
}


void lcdShow(char* texte, int line) {
  if (line < 1 or line > 2)
    line = 1;
  lcd.setCursor(0,line-1);
  lcd.print(texte);
  Serial.println(texte);

}

void setup() {
  //// clear EEPROM
  //EEPROM.write(add_bot1, 0);
  //EEPROM.write(add_bot2, 0);
  //EEPROM.write(add_bot3, 0);
  //EEPROM.write(add_bot4, 0);

  // Weight sensor
  weightSensor.setInitialWeight();
  
  // Stepper setup

  // This line is for Leonardo's, it delays the serial interface
  // until the terminal window is opened
  while (!Serial);

  Serial.begin(9600);
  //motor1.setCurrentPosition(motor1.currentPosition());
  //motor1.setSpeed(20);

  // Magnet Sensor
  pinMode(magInPin, INPUT);

  // servo setup
  servo.attach(servoPin);
  servo.write(100);
  
  // Motor 2
  pinMode(m2stepPin, OUTPUT);
  pinMode(m2dirPin, OUTPUT);
  m2readPin = A1;
  
  // LCD setup
  lcd.begin();                      // initialize the lcd 

  // Print a message to the LCD.
  lcd.backlight();

  // Populate database
  makeDatabase();

  //// Initialize quantities for the first time.
  //bottlesMenus(1,1,1,1);
}

void doStepMotor(int steps) {
  Serial.println(digitalRead(m2dirPin));
  for (int i = 0; i < steps/1.8*79/14.0; i++)
  {
  digitalWrite(m2stepPin,HIGH);
  delayMicroseconds(m2speed*2);     
  digitalWrite(m2stepPin,LOW); 
  }
  
}

void moveMotors(float steps) {
  digitalWrite(m2dirPin,m2direction); // Enables the motor to move in a particular direction
    if (steps < 0) {
      digitalWrite(m2dirPin,0);
      doStepMotor(-steps);
    }
    else {
      digitalWrite(m2dirPin, 1);
      doStepMotor(steps);
    }
}

int getMagnet() {
  Serial.println(digitalRead(magInPin)); // TODO remove
  return digitalRead(magInPin); 
}


String getSelectionText() {
  return (String) currentCocktail + ". " + cock[currentCocktail].getName() + " :";
}
String getSelectionQuantitiesText() {
  return (String) cock[currentCocktail].getQuantity(0) + "/"
    + cock[currentCocktail].getQuantity(1) +"/"
    + cock[currentCocktail].getQuantity(2) + "/"
    + cock[currentCocktail].getQuantity(3);
}

int getQuantityInBottle(int num) {
  if (num == 1) return EEPROM.read(add_bot1);
  if (num == 2) return EEPROM.read(add_bot2);
  if (num == 3) return EEPROM.read(add_bot3);
  return EEPROM.read(add_bot4);
}

void setQuantityInBottle(int num, int quantity) {
  int bot = add_bot1;
  if (num == 1) bot = add_bot1;
  if (num == 2) bot = add_bot2;
  if (num == 3) bot = add_bot3;
  if (num == 4) bot = add_bot4;

  EEPROM.write(bot, quantity);
}

void decrementBottle5(int num) {
  int quantity = 0;
  boolean ret = false;
  int bot = add_bot1;
  if (num == 1) bot = add_bot1;
  if (num == 2) bot = add_bot2;
  if (num == 3) bot = add_bot3;
  if (num == 4) bot = add_bot4;
  quantity = EEPROM.read(bot) - 5;
  if (quantity < 0)
    quantity = 0;
  EEPROM.write(bot,quantity);
} 

void bottlesMenus(int w1, int w2, int w3, int w4) {
  int q1 = getQuantityInBottle(1);
  int q2 = getQuantityInBottle(2);
  int q3 = getQuantityInBottle(3);
  int q4 = getQuantityInBottle(4);
  int bot = add_bot1;

  String text1 = "";
  String text2 = "";
  int button = btnNONE;

  int newQuantity = 75;

  while ((q1 < 5*w1) || (q2 < 5*w2) || (q3 < 5*w3) || (q4 < 5*w4)) { // While one of the bottles is empty
    if ((q1 < 5*w1)) {
      text1 = (String) "Refill bottle 1.";
      bot = add_bot1;
    } 
    else if ((q2 < 5*w2)) {
      text1 = (String) "Refill bottle 2.";
      bot = add_bot2;
    } 
    else if ((q3 < 5*w3)) {
      text1 = (String) "Refill bottle 3.";
      bot = add_bot3;
    } 
    else {
      text1 = (String) "Refill bottle 4.";
      bot = add_bot4;
    }
    Serial.println(bot);

    lcdShow((char*) text1.c_str(), 1);
    text2 = (String) "Quantity : " + newQuantity;
    lcdShow((char*) text2.c_str(), 2);
    while ((button = getButton()) != btnSELECT) {
      // buttons
      switch(button) { 
      case btnLEFT:
      case btnUP:
        text2 = (String) "                ";
        lcdShow((char*) text2.c_str(), 2);
        newQuantity += 5;
        if (newQuantity > 150)
          newQuantity = 150;
        text2 = (String) "Quantity : " + newQuantity;
        lcdShow((char*) text2.c_str(), 2);
        delay(500);
        break;
      case btnRIGHT:
      case btnDOWN:
        text2 = (String) "                ";
        lcdShow((char*) text2.c_str(), 2);
        newQuantity -= 5;
        if (newQuantity < 0)
          newQuantity = 0;
        text2 = (String) "Quantity : " + newQuantity;
        lcdShow((char*) text2.c_str(), 2);
        delay(500);
        break;
      case btnSELECT:
      case btnNONE:
      default:
        break;
      }
    }

    setQuantityInBottle(bot + 1, newQuantity);

    text1 = (String) "Bottle " + (bot+1) + " Filled";
    text2 = (String) "                ";
    lcdShow((char*) text1.c_str(), 1);
    lcdShow((char*) text2.c_str(), 2);
    delay(2000);
    lcd.clear();

    q1 = getQuantityInBottle(1);
    q2 = getQuantityInBottle(2);
    q3 = getQuantityInBottle(3);
    q4 = getQuantityInBottle(4);
  }
}


void showMenus() {
  int button = btnNONE;
  String text1 = getSelectionText();
  String text2 = getSelectionQuantitiesText();
  String text3;
  String text4;
  currentCocktail = 0; // TODO REMOVE
  return; // TODO REMVOVE
  while ((button = getButton()) != btnSELECT) {
    getMagnet();
    switch(button) { 
    case btnLEFT:
      text3 = (String) "Bottles (cl):";
      text4 = (String) getQuantityInBottle(1) + "/"
        + getQuantityInBottle(2) + "/"
        + getQuantityInBottle(3) + "/"
        + getQuantityInBottle(4);
      lcd.clear();
      lcdShow((char*) text3.c_str(), 1);
      lcdShow((char*) text4.c_str(), 2);
      delay(3000);
      lcd.clear();
      break;
    case btnUP:
      currentCocktail--;
      if (currentCocktail < 0) {
        currentCocktail = NUM_COCKTAILS - 1; 
      }
      text1 =  getSelectionText();
      text2 = getSelectionQuantitiesText();
      lcd.clear();
      break;
    case btnRIGHT:
      EEPROM.write(add_bot1, 0);
      EEPROM.write(add_bot2, 0);
      EEPROM.write(add_bot3, 0);
      EEPROM.write(add_bot4, 0);
      bottlesMenus(1,1,1,1);
      break;
    case btnDOWN:
      currentCocktail++;
      if (currentCocktail >= NUM_COCKTAILS) {
        currentCocktail = 0; 
      }
      text1 =  getSelectionText();
      text2 = getSelectionQuantitiesText();
      lcd.clear();
      break;
    case btnSELECT:
    case btnNONE:
    default:
      break;
    }
    lcdShow((char*) text1.c_str(), 1);
    lcdShow((char*) text2.c_str(), 2);
    delay(500);
  }
  delay(500);
  bottlesMenus(cock[currentCocktail].getQuantity(0)
    ,cock[currentCocktail].getQuantity(1)
    ,cock[currentCocktail].getQuantity(2)
    ,cock[currentCocktail].getQuantity(3));
  text1 = (String) "Making " + cock[currentCocktail].getName();
  text2 = (String) "                ";
  lcdShow((char*) text1.c_str(), 1);
  lcdShow((char*) text2.c_str(), 2);
}

const int MAX_servo = 60; // To be tuned
void getLiquid() {
  servo.write(100);
  delay(2000);
  servo.write(40);
  delay(2000);
  servo.write(100);
  delay(1000);
}

const int gearRatio = 79/14.0; // big over little
const int steps = 90; // 

void makeCocktail() {
  Serial.println("makeCocktail");
  int sum = cock[currentCocktail].getQuantity(0)
    + cock[currentCocktail].getQuantity(1)
      + cock[currentCocktail].getQuantity(2)
        + cock[currentCocktail].getQuantity(3);
  int saved = 4;

  for (int i = 0; i < 4; i++) {
    Serial.println("next bottle");
    int quantity = cock[currentCocktail].getQuantity(i);
    if (quantity > 0) { 
      for (int j = 0; j < quantity; j++) { // get liquid as many time as needed
        getLiquid();
        sum--;
        decrementBottle5(i + 1);
      }
    }
    if (i < 3) {
      moveMotors(steps);
    }
    if (sum == 0) {
      saved = i;
      break;
    }
  }

  moveMotors(-steps*saved);
  
  lcd.clear();
  lcdShow("Please DRINK!", 1);
  delay(3000);
  
  Serial.println("cocktail made");
}

void loop() {
  switch(state) {
  case INIT:
    showMenus();
    state = CORE;
    break;
  case CORE:
    makeCocktail();
    state = INIT;
    lcd.clear();
    break;
  default:
    state = INIT;
    break;
  }
}