#include <Servo.h>

//L298N Motor Driver 1
#define dvigalo_Pin_A 2    // pin a
#define dvigalo_Pin_B 3    // pin b
#define dvigalo_Pin_PWM 4  // pin PWM

#define pumpa_1_Pin_A 5    // pin a
#define pumpa_1_Pin_B 6    // pin b
#define pumpa_1_Pin_PWM 7  // pin PWM

//L298N Motor Driver 2
#define pumpa_2_Pin_A 8     // pin a
#define pumpa_2_Pin_B 9     // pin b
#define pumpa_2_Pin_PWM 10  // pin PWM

#define pumpa_3_Pin_A 11    // pin a
#define pumpa_3_Pin_B 12    // pin b
#define pumpa_3_Pin_PWM 13  // pin PWM

//servo
#define servo_Pin 44
Servo servo;

//Encoder
#define Pin_Up 23
#define Pin_Down 25

bool Up;
bool Down;
bool UpP;
bool DownP;

int izbira_pijace = 0;
int nastavitev_casa = 1000;

//stikala
bool up_stikalo;    // pin 22
bool down_stikalo;  // pin 24

#define pin_up_stikalo 62
#define pin_down_stikalo 63

//tipka
bool tipka = 0;   // pin 26
bool tipkaP = 0;  // prejšnje stanje tipke

//spremenljivke
int zacetni_stevec = 0;
bool zacetek = 0;

void setup() {

  pinMode(dvigalo_Pin_A, OUTPUT);
  pinMode(dvigalo_Pin_B, OUTPUT);
  pinMode(dvigalo_Pin_PWM, OUTPUT);

  pinMode(pumpa_1_Pin_A, OUTPUT);
  pinMode(pumpa_1_Pin_B, OUTPUT);
  pinMode(pumpa_1_Pin_PWM, OUTPUT);

  pinMode(pumpa_2_Pin_A, OUTPUT);
  pinMode(pumpa_2_Pin_B, OUTPUT);
  pinMode(pumpa_2_Pin_PWM, OUTPUT);

  pinMode(pumpa_3_Pin_A, OUTPUT);
  pinMode(pumpa_3_Pin_B, OUTPUT);
  pinMode(pumpa_3_Pin_PWM, OUTPUT);

  pinMode(servo_Pin, OUTPUT);

  pinMode(63, INPUT);
  pinMode(62, INPUT);
  pinMode(26, INPUT_PULLUP);

  pinMode(Pin_Up, INPUT);
  pinMode(Pin_Down, INPUT);

  pinMode(pin_up_stikalo, INPUT_PULLUP);
  pinMode(pin_down_stikalo, INPUT_PULLUP);

  servo.attach(servo_Pin);
  servo.write(0);

  Serial.begin(9600);
}


void loop() {

  tipka = digitalRead(26);

  if (( tipka == 0 ) && ( tipkaP == 1 )) {
    zacetni_stevec =  zacetni_stevec + 1;
    Serial.println("tipka");
  }
  tipkaP = tipka;


  switch (zacetni_stevec) {
    case 1:  //izbira pijače
      Encoder_izbira_pijace();
      //Serial.println("zacetni_stevec = 1");
      break;

    case 2:
      Encoder_cas_tocenja();
      //  Serial.println("zacetni_stevec = 2");
      break;

    case 3:
      zacetek = 1;
      // Serial.println("zacetni_stevec = 3");
      break;
  }

  while (zacetek == 1) {  //zacetek delovanja

    down_stikalo = digitalRead(pin_down_stikalo);  //spust dvigala dokler ne zadane stikala

    while (down_stikalo == 1) {
      digitalWrite(dvigalo_Pin_A, 1);
      digitalWrite(dvigalo_Pin_B, 0);
      analogWrite(dvigalo_Pin_PWM, 50);

      down_stikalo = digitalRead(pin_down_stikalo);
    }
    digitalWrite(dvigalo_Pin_A, 0);  //ustavi dvigalo
    digitalWrite(dvigalo_Pin_B, 0);

    delay(1000);

    servo.write(90);  //servo obrne cevko nad kozarc

    delay(1000);

    switch (izbira_pijace) {
      case 0:
        digitalWrite(pumpa_1_Pin_A, 1);
        digitalWrite(pumpa_1_Pin_B, 0);
        analogWrite(pumpa_1_Pin_PWM, 100);
        break;

      case 1:
        digitalWrite(pumpa_2_Pin_A, 1);
        digitalWrite(pumpa_2_Pin_B, 0);
        analogWrite(pumpa_2_Pin_PWM, 100);
        break;

      case 2:
        digitalWrite(pumpa_3_Pin_A, 1);
        digitalWrite(pumpa_3_Pin_B, 0);
        analogWrite(pumpa_3_Pin_PWM, 100);
        break;
    }

    delay(nastavitev_casa);

    digitalWrite(pumpa_1_Pin_A, 0);
    digitalWrite(pumpa_1_Pin_B, 0);
    digitalWrite(pumpa_2_Pin_A, 0);
    digitalWrite(pumpa_2_Pin_B, 0);
    digitalWrite(pumpa_3_Pin_A, 0);
    digitalWrite(pumpa_3_Pin_B, 0);

    delay(1000);

    servo.write(0);

    delay(1000);

    up_stikalo = digitalRead(pin_up_stikalo);

    while (up_stikalo == 1) {
      digitalWrite(dvigalo_Pin_A, 0);
      digitalWrite(dvigalo_Pin_B, 1);
      analogWrite(dvigalo_Pin_PWM, 50);

      up_stikalo = digitalRead(pin_up_stikalo);
    }

    digitalWrite(dvigalo_Pin_A, 0);
    digitalWrite(dvigalo_Pin_B, 0);

    delay(1000);

    zacetek = 0;
    zacetni_stevec = 0;

    izbira_pijace = 0;
    nastavitev_casa = 1000;

    break;
  }

}

void Encoder_izbira_pijace() {

  Up = digitalRead(Pin_Up);
  Down = digitalRead(Pin_Down);

  if (( Up == 0 ) && ( UpP == 1 )) {
    izbira_pijace =   izbira_pijace + 1;

    if (izbira_pijace > 2) {
      izbira_pijace = 2;
    }

    Serial.print("pijaca");
    Serial.println(izbira_pijace);
  }
  UpP = Up;

  if (( Down == 0 ) && ( DownP == 1 )) {
    izbira_pijace =  izbira_pijace - 1;

    if (izbira_pijace < 0) {
      izbira_pijace = 0;
    }

    Serial.print("pijaca");
    Serial.println(izbira_pijace);
  }
  DownP = Down;

}

void Encoder_cas_tocenja() {

  Up = digitalRead(Pin_Up);
  Down = digitalRead(Pin_Down);

  if (( Up == 0 ) && ( UpP == 1 )) {
    nastavitev_casa = nastavitev_casa + 1000;

    if (nastavitev_casa > 5000) {
      nastavitev_casa = 5000;
    }

    Serial.print("cas = ");
    Serial.println(nastavitev_casa);

  }
  UpP = Up;

  if (( Down == 0 ) && ( DownP == 1 )) {
    nastavitev_casa = nastavitev_casa - 1000;

    if (nastavitev_casa < 1000) {
      nastavitev_casa = 1000;
    }

    Serial.print("cas = ");
    Serial.println(nastavitev_casa);
  }
  DownP = Down;


}