//1+2+3+4+5+6+7+8+9+10
#include <Servo.h>
#include <Wire.h>   
#include <LiquidCrystal_I2C.h> 
#include <Encoder.h>
#include <avr/eeprom.h>

const int hcSrTrigPin = 7, hcSrEchoPin = 6;
const int encPin1 = 3, encPin2 = 4;
const int servoPin = 11;
const int potentiometerPin = A2, btnPin = 2;

unsigned long timeEnc, timeRadar, timeSpeed;
int prevEncVal = 0;
int btnClicked = 0;
int menu = 1, state = 1;
int angle, minAngle;
int distance, minDistance = 999;
int pot;

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servo;
Encoder enc(encPin1, encPin2);

void setup() {
  Serial.begin(9600);
  pinMode(hcSrTrigPin, OUTPUT);
  pinMode(hcSrEchoPin, INPUT);
  pinMode(btnPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(btnPin), handleBtn, CHANGE);
  lcd.begin(16, 2);
  lcd.clear();
  servo.attach(servoPin); 
  readEeprom();
}

void showLcd(){ 
  char buff[16];
  lcd.setCursor(0,0);
  if(menu == 1){
    if(state == 1) sprintf(buff, "Radar           ");
    if(state == 2) sprintf(buff, "Radar minDis:%3d", minDistance);
    lcd.print(buff);

    lcd.setCursor(0,1);
    sprintf(buff, "Kat:%3d D:%3dcm", angle, distance);
    lcd.print(buff);
  }
  else if(menu == 2){
    lcd.print("Ster czuj odleg ");

    lcd.setCursor(0,1);
    sprintf(buff, "Kat:%3d D:%3dcm", angle, distance);
    lcd.print(buff);
  }
  else if(menu == 3){
    lcd.print("Ster potencjomet");

    lcd.setCursor(0,1);
    sprintf(buff, "Kat:%3d P:%4d", angle, pot);
    lcd.print(buff);
  }
  else if(menu == 4){
    lcd.print("Lewo prawo      ");
  }
}

void showSerial(){
  char buff[100];
  sprintf(buff, "menu: %1d, state: %1d, btnClick: %1d, angle: %3d, distance: %3dcm, pot: %4d", menu, state, btnClicked, angle, distance, pot);
  Serial.println(buff);
}

void handleEnc(){
  if(millis() - timeEnc > 50){
    int encVal = enc.read();
    timeEnc = millis();

    if(encVal < prevEncVal){
      menu--;
      prevEncVal = encVal;
      lcd.clear();
    }
    else if(encVal > prevEncVal){
      menu++;
      prevEncVal = encVal;
      lcd.clear();
    }
    menu = constrain(menu, 1, 4);
  }
}

int measureDistance(){
    digitalWrite(hcSrTrigPin, HIGH);                 
    delayMicroseconds(10);                        
    digitalWrite(hcSrTrigPin, LOW);                   
    long impulseTime = pulseIn(hcSrEchoPin, HIGH);        
    return impulseTime/58.0; 
}

void handleHcSr(){
    int sum = 0;
    for(int i = 0; i < 3; i++){
      sum += measureDistance();
    }
    distance = sum / 3;
    distance = constrain(distance, 0, 350);

    if(menu == 2) {
      angle = map(distance, 0, 350, 0, 180);   
    }             
}

void handlePotentiometer(){
  pot = analogRead(potentiometerPin);
  if(menu == 3){
    angle = map(pot, 0, 1023, 0, 180);  
  }   
}

void handleServo(){ 
  if(menu != 4 && btnClicked != 1){
    servo.write(angle);
    if(menu == 1 && state == 1 && angle == 180){
      timeRadar = millis();
      state = 2;
      angle = minAngle;
    }
  }
}

void handleBtn(){
  btnClicked = 1;
  timeSpeed = millis();
  writeEeprom();
}

void handleRadarMode(){
  if(menu == 1 && state == 1){
    if(millis() - timeRadar > 200){
      timeRadar = millis();
      angle+=5;
      angle = constrain(angle, 0, 180);

      if(distance < minDistance){
        minDistance = distance;
        minAngle = angle;
      }
    }
  }
  else if(menu == 1 && state == 2){
    if(millis() - timeRadar > 5000){
      state = 1;
      minDistance = 999;
      angle = 0;
    }
  }
}

void handleSpeedMode(){
  if(menu == 4 || btnClicked == 1){
    speed();
  }
}

void speed(){
  if(millis() - timeSpeed > 1){
    timeSpeed = millis();
    servo.write(angle);
    angle+=10;
    angle = angle > 180 ? 0 : angle;

    if(btnClicked == 1 && angle == 0){
      btnClicked = 0;
    }
  }
}

void writeEeprom(){
  eeprom_write_block(&menu, 0, 2); 
  eeprom_write_block(&state, 10, 2); 
  eeprom_write_block(&angle, 20, 2); 
}

void readEeprom(){
  eeprom_read_block(&menu, 0, 2); 
  eeprom_read_block(&state, 10, 2); 
  eeprom_read_block(&angle, 20, 2); 

  menu = constrain(menu, 1, 4);
  state = constrain(state, 1, 2);
  angle = constrain(angle, 0, 180);
}

void loop() {
    handleEnc();
    handleHcSr();
    handlePotentiometer();
    handleRadarMode();
    handleServo();
    handleSpeedMode();
    showLcd();
    showSerial();
}