#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

const int relayPin = 8; //rele KS2E-M-DC5
const int buttonLEFT = 6;
const int buttonRIGHT = 5;
const int buttonFORWARD = 7;
const int buttonBACK = 4;

//LED
const int ledRED = 8;
const int ledPINK = 8;

//Ultragarsinis jutiklis
#define TRIG_PIN A3      // ultragarsinio matuoklio įjungimo signalo kontaktas
#define ECHO_PIN A2      // ultragarsinio matuoklio duomenų kontaktas
uint16_t duration = 0;   // signalo trukmės kintamasis
uint32_t interval = 0;   // jutiklio apklausos kintamasis
float distance = 0;      // trukmes konvertavimo į atstumą kintamasis

//Servo variklis, valdantis čiaupo sklendę
#define ciaupo_pin 3 //input servo variklys
Servo ciaupas; // Servo variklio tipo objektas valdantis čiaupo sklendę.
float pos = 0.0; // kintamasis, kur bus saugomas parametro 'ciaupas' pozicija (laipsniais) - ciaupas uzdarytas
float step = 1.0; //kintamasis, kuris bus naudojamas parametro 'ciaupas' zingsniui

LiquidCrystal_I2C lcd(0x27, 20, 4); //didelis LCD ekranas
//LiquidCrystal_I2C lcd(0x27, 16, 2); //mazas LCD ekranas
//LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES); // lcd ekrano LiquidCrystal_I2C objektas su pradiniais parametrais:
//  I2C_ADDR adresas LCD ekrano pajungto per I2C sąsają (galima
// pajungti 128 unikalių įrenginių vienu metu)
// LCD_COLUMNS ekrano eilutės ilgis
// LCD_LINES ekrano eilučių skaičius


//FUNKCIJA - distancijos nustatymui
float cm;
float meter;
long readUltrasonicDistance(int TRIG_PIN, int ECHO_PIN)
{
  pinMode(TRIG_PIN, OUTPUT);  // Clear the trigger
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  pinMode(ECHO_PIN, INPUT);
  // the sound wave travel time in microseconds
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = (duration / 58.35); // atstumo iš signalo trukmes apskaičiavimas 1/58 - skirtas signalo trukmes konvertavimui į cm
  return distance;

}
/*
  long readUltrasonicDistance(char TRIG_PIN, char ECHO_PIN)
  {
  pinMode(TRIG_PIN, OUTPUT);  // Clear the trigger
  analogWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  analogWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  analogWrite(TRIG_PIN, LOW);
  //pinMode(ECHO_PIN, INPUT); //the sound wave travel time in microseconds
  pinMode(ECHO_PIN, INPUT_PULLUP); //slide-switch //the sound wave travel time in microseconds
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = (duration / 58.35); // atstumo iš signalo trukmes apskaičiavimas 1/58 - skirtas signalo trukmes konvertavimui į cm
  return distance;
  }
*/

//---MASINOS JUDEJIMAS----MYGTUKAIS---FORWARD(default)-BACK
static void distancijosNustatymasMygtukais(int buttonFORWARD, int buttonBACK) {
  //Arduino ultragarsinis atstumų jutiklis
  if (digitalRead(buttonFORWARD))
  {
    pos = 90; //servo ciaupas pasisuka tiesiai, 90 laipsniu kampu;
    analogWrite(ledRED, HIGH);
    analogWrite(ledPINK, LOW);
    ciaupas.write(pos);

    if ((millis() - interval) >= 250)   // millis() metodas nuskaito lokalų Arduino laikrodžio reikšmę ir if ligina ar praėjo 1/4 sec.
    {
      interval = millis();              // perrašome intervalo naują reikšmę
      digitalWrite(TRIG_PIN, HIGH);     // ultragarsinio jutiklio įjungimas
      delayMicroseconds(10);            // 10 milisekundžių trukmės pauzė
      digitalWrite(TRIG_PIN, LOW);      // ultragarsinio jutiklio išjungimas
      duration = pulseIn(ECHO_PIN, HIGH);// trukmės nuskaitymas iš ECHO_PIN kontakto
      distance = (duration / 58.35);     // atstumo iš signalo trukmes apskaičiavimas 1/58 - skirtas signalo trukmes konvertavimui į cm

      //vaziavimo kripties isvedimas
      lcd.setCursor(0, 1);
      lcd.print("I PRIEKI-> ");
      delay(1000);

      //distancijos isvedimas i lcd ekrana
      lcd.setCursor(0, 1);
      lcd.print("Atstumas-> ");
      delay(1000);
      lcd.print(distance);
      delay(1000);
      lcd.clear();

      //----------------------------
      //DISTANCIJOS isvedimas - cm, metrais pagal funkcija
      cm = readUltrasonicDistance(3, 2);
      //cm = 0.0344 / 2 * readUltrasonicDistance(3, 2);
      //cm = readUltrasonicDistance(A3, A2);
      meter = (cm * 100);

      lcd.setCursor(0, 0);
      lcd.print("Metrai");
      lcd.setCursor(4, 0);
      lcd.setCursor(12, 0);
      lcd.print("cm");
      lcd.setCursor(1, 1);
      lcd.print(meter, 1);
      lcd.setCursor(11, 1);
      lcd.print(cm, 1);
      lcd.setCursor(14, 1);
      delay(2000);
      lcd.clear();
    }
    if (distance < 100) // jei atstumas iki kliuties mazesnis nei 1 metras (100 cm) automatiskai isijungia atbuline pavara
    {
      digitalRead(buttonBACK);
      //distancijos isvedimas i lcd ekrana
      lcd.setCursor(0, 1);
      lcd.print("ATBULINE!-> ");
      delay(1000);
      lcd.print(distance);
      delay(1000);
      lcd.clear();
    }
  }
}

//----------------SERVO VALDYMAS------------MYGTUKAIS--RIGHT-LEFT
static void servoCiaupoValdymasMygtukais(int buttonRIGHT, int buttonLEFT) {
  if (digitalRead(buttonRIGHT)) // RIGHT - patikriname WHITE mygtuko input
  {
    //posukio isvedimas
    lcd.setCursor(0, 1);
    lcd.print("I DESNE-> ");
    delay(1000);
    lcd.clear();

    for (step = 1; step < 5; step++)
    {
      {
        ciaupas.write(pos);
        pos += step; // padidaname ciaupo 'pos' parametra step (zingsnio) reiksme
        delay(5); // su 5ms uzdelsimu
      }
    }
  }

  else if (digitalRead(buttonLEFT)) // LEFT - patikriname GREY mygtuko input
  {

    //posukio isvedimas
    lcd.setCursor(0, 1);
    lcd.print("I KAIRE-> ");
    delay(1000);
    lcd.clear();

    for (step = 4; step < 10; step++)
    {
      {
        ciaupas.write(pos);
        pos -= step; // sumaziname ciaupo 'pos' parametra step (zingsnio) reiksme
        delay(5); // su 5ms uzdelsimu
      }
    }
  }
}

void setup() {
  // testas mygtukų nuspaudimo atsako įverčių nuskaitymui iš arduino testui reikalinga biblioteka Wire.h

  Wire.begin();                     // įjungiame laidinių magistralių signalų valdymą
  Serial.begin(9600);               // sereal consoles nustatymas
  Serial.println("\nI2C Scanner");  // pranešimų į konsolė išvedimas


  pinMode(TRIG_PIN, OUTPUT);          // ultragarsinio jutiklio įjungimo pino nustatymas kaip OUTPUT
  pinMode(ECHO_PIN, INPUT);           // ultragarsinio jutiklio kontakto nustatymas kaip INPUT jo nuskaitymui
  pinMode(relayPin, OUTPUT);         //rele KS2E-M-DC5
  //----------------------

  pinMode(ECHO_PIN, INPUT_PULLUP); //slide-switch

  ciaupas.attach(ciaupo_pin); // Servo variklio valdymo kontakto priskyrimas
  ciaupas.write(pos); // Servo variklio pradinė padėtis
  //----------------------------

  //Pranesimo isvedimas i LCD ekrana
  Serial.begin(9600);
  lcd.init();	// ekrano inicijavimas
  lcd.backlight();// ekrano apšvietimo įjungimas
  lcd.print(">Valdymo pultas<");
  delay(3000);
  lcd.clear();
}

void loop() {
/*
  {
    // TESTAS mygtukų nuspaudimo atsako įverčių nuskaitymui iš arduino
      byte error, address;
      int nDevices;
      Serial.println("Scanning...");
      nDevices = 0;
      for (address = 1; address < 127; address++ )
      {
      // The i2c_scanner uses the return value of
      // the Write.endTransmisstion to see if
      // a device did acknowledge to the address.
      Wire.beginTransmission(address);
      error = Wire.endTransmission();
      if (error == 0)
      {
        Serial.print("I2C device found at address 0x");
        if (address < 16)
          Serial.print("0");
        Serial.print(address, HEX);
        Serial.println(" !");
        nDevices++;
      }
      else if (error == 4)
      {
        Serial.print("Unknow error at address 0x");
        if (address < 16)
          Serial.print("0");
        Serial.println(address, HEX);
      }
      }
      if (nDevices == 0)
      Serial.println("No I2C devices found\n");
      else
      Serial.println("done\n");
      delay(5000); // wait 5 seconds for next scan
      }
    //----------------------------
    /*
      //Arduino ultragarsinis atstumų jutiklis
      if ((millis() - interval) >= 1000)   // millis() metodas nuskaito lokalų Arduino laikrodžio reikšmę ir if ligina ar praėjo 1 sec.
      {
        interval = millis();              // perrašome intervalo naują reikšmę
        digitalWrite(TRIG_PIN, HIGH);     // ultragarsinio jutiklio įjungimas
        delayMicroseconds(10);            // 10 milisekundžių trukmės pauzė
        digitalWrite(TRIG_PIN, LOW);      // ultragarsinio jutiklio išjungimas
        duration = pulseIn(ECHO_PIN, HIGH);// trukmės nuskaitymas iš ECHO_PIN kontakto
        distance = (duration / 58.35);     // atstumo iš signalo trukmes apskaičiavimas 1/58 - skirtas signalo trukmes konvertavimui į cm
      }
    */

    //FUNCIJA: ---MASINOS JUDEJIMAS----MYGTUKAIS---FORWARD(default)-BACK
    distancijosNustatymasMygtukais(7, 4);


    //FUNKCIJA: ----------------SERVO VALDYMAS------------MYGTUKAIS--RIGHT-LEFT
    servoCiaupoValdymasMygtukais(5, 6);
  }