#include <Servo.h>

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0;  


#include <LiquidCrystal_I2C.h>
//#include <HX711_ADC.h>
#include <Wire.h>


//INPUT
#define STOP_B 2      //tlacidlo prerusenia plnenia
#define MINUS_B 4     //tlacidlo minusu
#define PLUS_B 5      //tlacidlo plusu
#define ENTER_B 6    //tlacidlo enteru
#define HX711_dout 7  //mcu > HX711 dout pin
#define HX711_sck 8   //mcu > HX711 sck pin
#define DISK_POS A1
#define TERMISTOR A2 
#define GLASS_POS A3

//OUTPUT
#define BUZZER 9
#define HEATER 10
#define PUMP 11
#define dirPin 12
#define stepPin 13

//FUNKCIE
void first_screen(); //uvitacia obrazovka a nastavenie teploty
void second_screen(); //druha obrazovka, nastavovanie hmotnosti, aktualna hmostnost, aktualna teplota, aktualna davka
void third_screen(int ACT_TEMP, float ACT_WEIGHT, int ACT_GLASS);
void final_screen();
bool empty_glass();
int temp_reg();
void fill_glass(float &ACT_WEIGHT, int &ACT_GLASS);
void next_glass();
void ERROR_G();
void ERROR_F();
void first_setup();

//PREMENNE
int TEMP = 40;
int glass_N = 3;
const int steps_to_move = 1700;
const int num_of_glasses = 10;
float EMPTY_GLASS_WEGHT = 0;
float weight = 50.0;
float WEIGHTS[10] = {51.2,51.2,51.2,51.2,51.2,51.2,51.2,51.2,51.2,51.2};
const float BETA = 3950;
unsigned long t;
unsigned long time;
unsigned long INT_timer;
bool EMERGENCY = false;

//HX711_ADC LoadCell(HX711_dout, HX711_sck);
LiquidCrystal_I2C lcd(0x27,20,4); 

void setup()
{
  myservo.attach(11);
  Serial.begin(57600);

  //LCD
  lcd.init();                      
  lcd.backlight();

  //INPUT
  pinMode(STOP_B, INPUT);
  pinMode(MINUS_B, INPUT);    
  pinMode(PLUS_B, INPUT);      
  pinMode(ENTER_B, INPUT);   
  pinMode(DISK_POS, INPUT);
  pinMode(TERMISTOR, INPUT); 
  pinMode(GLASS_POS, INPUT);

  //OUTPUT
  pinMode(BUZZER, OUTPUT);
  pinMode(HEATER, OUTPUT);
  pinMode(PUMP, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin, OUTPUT);

  //DIGITALWRITE
  digitalWrite(BUZZER, LOW);
  digitalWrite(HEATER, LOW);
  digitalWrite(PUMP, LOW);
  digitalWrite(dirPin, LOW);
/*
  //LOAD CELL
  LoadCell.begin(); 
  float calibrationValue; // calibration value (see example file "Calibration.ino")
  calibrationValue = 1958.38; // uncomment this if you want to set the calibration value in the sketch
  long stabilizingtime = 2000; // preciscion right after power-up can be improved by adding a few seconds of stabilizing time
  boolean _tare = true; //set this to false if you don't want tare to be performed in the next step
  LoadCell.start(stabilizingtime, _tare);
  if (LoadCell.getTareTimeoutFlag()) {
    //Serial.println("Pripojte vahu");
    while (1);
  }
  else {
    LoadCell.setCalFactor(calibrationValue); // set calibration value (float)  
  }
  */
  //PRERUSENIA
  attachInterrupt(digitalPinToInterrupt(STOP_B), STOP_F, FALLING);

  
  
  first_screen();

  first_setup();

}


void loop()
{
  second_screen();
  int ACT_TEMP = 0;
  float ACT_WEIGHT = 0.0;
  int ACT_GLASS = 0;
  
  if(empty_glass()){ //kontrola pritomnosti prazdneho sklicka
    third_screen(ACT_TEMP, ACT_WEIGHT, ACT_GLASS);
    while(empty_glass()){
      ACT_TEMP = temp_reg(); //nacitanie a vzregulovanie teplotz  
      fill_glass(ACT_WEIGHT, ACT_GLASS);
      third_screen(ACT_TEMP, ACT_WEIGHT, ACT_GLASS); 
      delay(500);
      next_glass();   
    }
  }
  else if (ACT_GLASS == 0)ERROR_G(); //vzpisat eror o nepritomnosti skla
  delay(1000);

  final_screen();
  
}


/////////////////////////////////////// Funkcie /////////////////////////////////////////////////

////////////////////////////////////// Uvitacia obrazovka /////////////////////////////////////
void first_screen(){
   lcd.setCursor(3,0);
   lcd.print("VOLANSKY HONEY");
   lcd.setCursor(4,1);
   lcd.print("Vitaj Martin");
   lcd.setCursor(0,3);
   lcd.print(" Vyrobil Tomas Matej");
   delay(4000);
   lcd.clear();
}


////////////////////////////////////// Druha obrazovka /////////////////////////////////////
/////////////// Nastavovanie teploty a pozadovanej hmotnosti ///////////////////////////////
void second_screen(){
    EMERGENCY = false;
    lcd.clear();
    int n = 1;  
    bool enter = false;
    while(n<3){
      lcd.setCursor(1,0);
      lcd.print("ZAKLADNE NASTAVENIA");
      lcd.setCursor(0,2);
      if(n == 1) lcd.print("TEPLOTA OHREVU: ");
      else lcd.print("Teplota ohrevu: ");
      lcd.print(TEMP);
      lcd.print((char)223);
      lcd.print("C");
      lcd.setCursor(0,3);
      if(n == 2) lcd.print("HMOTNOST DAVKY: ");
      else lcd.print("Hmotnost davky: ");
      if(weight<100.0) lcd.setCursor(17,3);
      else lcd.setCursor(16,3);
      lcd.print(roundf(weight));
      lcd.print("g");
      enter = false;
      while(enter == false){
        if(n == 1){ 
          if(digitalRead(PLUS_B)){
            TEMP++;
            enter = true;
            //while(digitalRead(PLUS_B))
            delay(200);
          }
          if(digitalRead(MINUS_B)){
            TEMP--;
            enter = true;
            delay(200);
          }
          if(digitalRead(ENTER_B)){
            n = 2;
            enter = true;
            delay(200);
          }
        }
        else if(n == 2){
          if(digitalRead(PLUS_B)){
            weight++;
            enter = true;
            delay(200);
          }
          if(digitalRead(MINUS_B)){
            weight--;
            enter = true;
            delay(200);
          }
          if(digitalRead(ENTER_B)){
            n = 3;
            enter = true;
            delay(200);
          }
        } 
      }
    }
    lcd.clear();
    lcd.setCursor(1,0);
    lcd.print("Spustit davkovanie?");
    lcd.setCursor(6,2);
    lcd.print("ANO / NIE");
    while (1){
      if(digitalRead(ENTER_B)) {
        delay(100);
        break;
      }
      else if(digitalRead(STOP_B)){
        delay(100);
        second_screen();
        break;
      }
    }

}

////////////////////////////////////// Tretia obrazovka /////////////////////////////////////
/////////////// Zobrazuje nadavkovanu hmotnost a cislo skla ///////////////////////////////
void third_screen(int ACT_TEMP, float ACT_WEIGHT, int ACT_GLASS){
  lcd.clear();
    lcd.setCursor(2,0);
    lcd.print("PREBIEHA PLNENIE");
    lcd.setCursor(1,1);
    lcd.print("Akt. teplota: ");
    lcd.print(ACT_TEMP);
    lcd.print((char)223);
    lcd.print("C");
    lcd.setCursor(0,2);
    lcd.print("Akt. hmotnost: ");
    lcd.print(ACT_WEIGHT,1);
    lcd.print("g");
    lcd.setCursor(1,3);
    lcd.print("Cislo davky: ");
    lcd.print(ACT_GLASS);
    lcd.print("/10");
  delay(2000);
  //lcd.clear();
}


////////////////////////////////////// Posledna obrazovka /////////////////////////////////////
/////////////// Zobrazuje vsetky nadavkovane hmotnosti pre kontrolu ///////////////////////////
void final_screen(){
    lcd.clear();
    lcd.setCursor(2,1);
    lcd.print("PLNENIE DOKONCENE");
    lcd.setCursor(2,3);
    lcd.print("VYSLEDKY PLNENIA");
    tone(BUZZER, 2000, 1500); // Plays 262Hz tone for 0.250 seconds
    delay(3000);
    //digitalWrite(BUZZER, LOW);
    lcd.clear();
    for(int r = 0; r < 4; r++){
        lcd.setCursor(0,r);
        lcd.print((r*3)+1);
        lcd.print(".");
    }
    int r = 0;
    int m = 0;
    int i = 0;
    lcd.setCursor(2,0);
    while((glass_N-i)>0){
        lcd.print(" ");
        if(weight<95) lcd.print(WEIGHTS[i],1);
        else lcd.print(WEIGHTS[i],0);
        lcd.print("g");
        //i++;
        if(i == 2) lcd.setCursor(2,1);
        else if (i == 5) lcd.setCursor(2,2);
        else if (i == 8) lcd.setCursor(3,3);
        i++;
    }
    while(!digitalRead(ENTER_B)){}
    
}



bool empty_glass(){ //bude kontrolovat pritomnost skla a jeho vahu ak bude pritomne a praydne tak vzstup true
 //optickym snimacom kontrolovat pritomnost skla
 //prostrednictvom vahy kontrolovat stav naplnenia
 /*
 float ACT_WEIGHT = 0.0;
  if(!digitalRead(GLASS_POS)){
    for(int i = 0; i < 5; i++){   
      if(LoadCell.update()){
        ACT_WEIGHT = LoadCell.getData();
      }
    }
    if(ACT_WEIGHT > (EMPTY_GLASS_WEGHT+10)) return false;
  }
  else return false;
*/
return true;
}

int temp_reg(){
  int analogValue = analogRead(TERMISTOR);
  float celsius = 1 / (log(1 / (1023. / analogValue - 1)) / BETA + 1.0 / 298.15) - 273.15;
  Serial.print("Temperature: ");
  Serial.print(celsius);
  Serial.println(" ℃");
  delay(1000);
  if((TEMP-celsius)>10) analogWrite(HEATER, 255);
  else if((TEMP-celsius)>8) analogWrite(HEATER, 200);
  else if((TEMP-celsius)>6) analogWrite(HEATER, 150);
  else if((TEMP-celsius)>4) analogWrite(HEATER, 100);
  else if((TEMP-celsius)>2) analogWrite(HEATER, 50);
  else if((TEMP-celsius)<0) analogWrite(HEATER, 0);
  
  return roundf(celsius);
}

void fill_glass(float &ACT_WEIGHT, int &ACT_GLASS){
  /*
  int i =0;
  ACT_WEIGHT = LoadCell.getData();
  if (LoadCell.update()) newDataReady = true;
  if (newDataReady) {
    newDataReady = 0;
    t = millis(); 
  }

  delay(200);
  LoadCell.tare();
  delay(200);

  //prva etapa plnenia
  digitalWrite(PUMP, HIGH);
  t = millis();
  while(ACT_WEIGHT<(weight*0.3)){   
    if(LoadCell.update()){
      ACT_WEIGHT = LoadCell.getData();
    }
  }
  digitalWrite(PUMP, LOW);
  
  //if(EMERGENCY) second_screen();
  
  int time1 = millis()-t;
  delay(200);

  while(i<30){   
    if(LoadCell.update()){
      ACT_WEIGHT = LoadCell.getData();
      //Serial.println(ACT_WEIGHT);
      i++;
    }
  }
  
  //druha etapa plnenia
  float podiel1 = ACT_WEIGHT/weight; //teray poynam ya akz cas time1 sa kolko nadobz naplnilo
  float poediel2 = (1-podiel1-0.15); //0.15 percenta objemu naplnime nakoniec =
  int time2 = (podiel2*time1)/podiel1; //vypocet casu druheho procesu plnenia
  float mass2 = 0.00;
  if(time2>(2*time1)) ERROR_F();
  i = 0;
  digitalWrite(PUMP, HIGH);
  delay(time2);
  digitalWrite(PUMP, LOW);
  
  while(i<30){   
    if(LoadCell.update()){
      mass2 = LoadCell.getData();
      //Serial.println(mass2);
      i++;
    }
  }
  if(!(mass2>ACT_WEIGHT*1.5)) ERROR_F();
  
  
  //tretia etapa plnenia 
  int pause_time = roundf((weight-mass2)/((mass2-ACT_WEIGHT)/time2)); //vypocet casu pre posledny cyklus plnenia
  digitalWrite(PUMP, HIGH); 
  delay(pause_time);
  digitalWrite(PUMP, LOW);
  delay(400);
  ACT_WEIGHT = LoadCell.getData();
  WEIGHTS[ACT_GLASS] = ACT_WEIGHT;
  if(!(ACT_WEIGHT>weight)) ERROR_F();
  */
  digitalWrite(PUMP,HIGH);
  delay(1500);
  digitalWrite(PUMP,LOW);
  ACT_GLASS ++; 

}

void next_glass(){
  //najprv dostat disk na pozadovanu hodnotu
  /*
  while (analogRead(DISK_POS)>50){
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(800);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(800);
  }

   while (analogRead(DISK_POS)<950){
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(800);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(800);
  }
  */
}

///////////////////////////////////ERROR v pripade nepritomnosti skla v plnicke/////////////
void ERROR_G(){
  //pridat bzucanie
  lcd.clear();
  lcd.setCursor(2,1);
  lcd.print("V plnicke nie je");
  lcd.setCursor(1,2);
  lcd.print("umiestnene sklicko.");
  while(!digitalRead(ENTER_B)){}
}
 /////////////////////////////////// Chybove hlasenie pre prazdny zasobnik na mast ////////////////
void ERROR_F(){
  //pridat bzucanie
  lcd.clear();
  lcd.setCursor(2,1);
  lcd.print("V plnicke nie je");
  lcd.setCursor(2,2);
  lcd.print("dostatok masti.");
  while(!digitalRead(ENTER_B)){}
}

void STOP_F(){
  if((millis()-INT_timer)>200){
  EMERGENCY = true;
  digitalWrite(PUMP, LOW);
  INT_timer=millis();
  }
}

void first_setup(){
  //vycentrovanie rotacneho disku
  while (analogRead(DISK_POS)>800){
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(800);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(800);
  }

  //vyziadanie prazdnych skiel
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.print("Je nutne umiestnit");
  lcd.setCursor(2,1);
  lcd.print("prazdne nadoby do");
  lcd.setCursor(6,2);
  lcd.print("plnicky.");

  while(digitalRead(GLASS_POS)){}

  delay (2000);
  /*
  for(int i = 0; i < 5; i++){   
    if(LoadCell.update()){
      EMPTY_GLASS_WEIGHT = LoadCell.getData();
    }
  }
  */
}