#include <Servo.h>
#include <LiquidCrystal_I2C.h>
const int buttonPin = 2; // Pin for button
const int testPin1 = 3; // Pin for continuity testing (contact 1)
const int testPin2 = 4; // Pin for continuity testing (contact 2)
const int testPin3 = 5; // Pin for continuity testing (contact 3)
const int testPin4 = 6; // Pin for continuity testing (contact 4)
const int testPin5 = 7; // Pin for continuity testing (contact 5)
const int servoPin1 = 9;
const int servoPin2 = 10;
const int servoPin3 = 11;
const int trigPin = 8;  // Pin to trigger the ultrasonic sensor
const int echoPin = 12; // Pin to read the echo
int initServoPos = 0;
int finalServoPos = 90;
int buttonState = 0; // Variable for reading button status
Servo Servo1;
Servo Servo2;
Servo Servo3;

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(testPin1, INPUT); // Set test pin 1 as input
  pinMode(testPin2, INPUT); // Set test pin 2 as input
  pinMode(testPin3, INPUT); // Set test pin 3 as input
  pinMode(testPin4, INPUT); // Set test pin 4 as input
  pinMode(testPin5, INPUT); // Set test pin 5 as input
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buttonPin, INPUT); 

  Servo1.attach(servoPin1);
  Servo2.attach(servoPin2);
  Servo3.attach(servoPin3);

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

  // Print something
  lcd.setCursor(3, 0);
  lcd.print("Hello, world!");
  lcd.setCursor(2, 1);
  lcd.print("Wokwi Online IoT");
  lcd.setCursor(5, 2);
  lcd.print("Simulator");
  lcd.setCursor(7, 3);
  lcd.print("Enjoy!");
}

void loop() {
  // put your main code here, to run repeatedly:

  buttonState = digitalRead(buttonPin);

  if (buttonState == HIGH) {             // If the button is pressed
    Serial.println("Button Pressed! Measuring distance now.");
    int dist = measureDist(trigPin, echoPin) + 0.5;
    lcdPrint(String(dist)+"mm",0,1);
    delay(500);
  }

  int contState = digitalRead(testPin1);
  if (contState == HIGH) {
    Serial.println("Continuity Detected!"); // Continuity exists
  } else {
    //Serial.println("No Continuity!"); // No connection
  }
  
  delay(100); // Wait for 0.5 second before checking again

  //Servo1.write(initServoPos);
  //clearLine(0);
  //lcd.setCursor(0, 0);
  //lcd.print("Servo 1 moved");
  //delay(1000);

  //Servo1.write(finalServoPos);
  //clearLine(0);
  //lcd.setCursor(0, 0);
  //lcd.print("Servo 1 reset");
  //delay(1000);


  // Start a new measurement:
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the result:
  int duration = pulseIn(echoPin, HIGH);
  //Serial.print("Distance in CM: ");
  //Serial.println(duration / 58);
  
}

void clearLine(int line) {
  lcd.setCursor(0, line);       // Move cursor to the beginning of the line
  lcd.print("                    "); // Print 16 spaces (for a 16-column display)
}

void lcdPrint(String msg, int line, int col) {
  if (col == 0) {
    lcd.setCursor(0, line);
  } else {
    lcd.setCursor(10, line);
  }
  lcd.print(msg);
}

float measureDist(int trig, int echo) {  // uses ultrasonic sensor to measure distance in mm
  digitalWrite(trig, HIGH);
  delayMicroseconds(5);
  digitalWrite(trig, LOW);

  float duration = pulseIn(echo, HIGH);
  float dist = duration/5.8;
  Serial.print("Dist: ");
  Serial.println(dist);
  return dist;
}