// Still in progress, mapping is not correct yet

#include <ESP32Servo.h>

const int buttonPin = 19;  // the number of the pushbutton pin
const int ledPin =  18;    // the number of the LED pin

// variable for storing the pushbutton status 
int buttonState = 0;

#define POT_PIN_H A0 
#define POT_PIN_V A1 
#define POT_PIN_F A2

#define SERVO_PIN_L 7
#define SERVO_PIN_R 6 
#define SERVO_PIN_F 5 

Servo servoL;
Servo servoR;
Servo servoF;

int potValueH, potValueV, potValueF;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Init ...");

 // initialize the pushbutton pin as an input
  pinMode(buttonPin, INPUT);
  // initialize the LED pin as an output
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH);

  servoL.attach(SERVO_PIN_L);
  servoR.attach(SERVO_PIN_R);
  servoF.attach(SERVO_PIN_F);

  servoL.write(90);
  servoR.write(90);
  servoF.write(90);

  Serial.println("Running ...");

}

void loop() {

  // read the state of the pushbutton value
  buttonState = digitalRead(buttonPin);

  // check if the pushbutton is pressed.
  // if it is, the buttonState is HIGH
  if (buttonState == HIGH) {
    // turn LED off
    digitalWrite(ledPin, LOW);

    potValueH = analogRead(POT_PIN_H);
    potValueV = analogRead(POT_PIN_V);
    potValueF = analogRead(POT_PIN_F);

    int servoPosL = map(potValueH, 0, 4096, 0, 180);
    int servoPosR = map(potValueV, 0, 4096, 0, 180);
    int servoPosF = map(potValueF, 0, 4096, 0, 180);

    servoL.write(servoPosL);
    servoR.write(servoPosR);
    servoF.write(servoPosF);

    Serial.println("--");

    Serial.print("H=");
    Serial.println(potValueH);
    Serial.print("SL=");
    Serial.println(servoPosL);
    Serial.print("V=");
    Serial.println(potValueV);
    Serial.print("SR=");
    Serial.println(servoPosR);
    Serial.print("F=");
    Serial.println(potValueF);
    Serial.print("SF=");
    Serial.println(servoPosF);

    Serial.println("--");
  } else {
    // turn LED on
    digitalWrite(ledPin, HIGH);
    Serial.println("Safety engaged!");
    Serial.println(buttonState);
  }

  // put your main code here, to run repeatedly:
  delay(100); // this speeds up the simulation
}