#include <Arduino.h>
#include <Servo.h>

Servo yaw;
Servo pitch;

double YAW_MAX = 7;
double PITCH_MAX = 7;

double zero_height;

float read_float();

void setup() {
  delay(1000);
  // yaw.attach(9);    // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
  // pitch.attach(10); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE

  Serial.println("Level the TVC and press 1.");
  read_float();

  // Do a full range of movement of the servos
  for (double pct = -100.0; pct <= 100.0; pct += 1.0) {
    double yaw_pos = pct * YAW_MAX;
    double pitch_pos = pct * PITCH_MAX;

    yaw.write(yaw_pos);
    // pitch.write(pitch_pos);
    delay(15);
  }

  Serial.print("Please input the point height in millimeters: ");
  String zero_string = read_float();
}

void loop() {
  Serial.print("Enter Yaw Coord: ");
  float yaw_pos = read_float();

  Serial.println();
  Serial.print("Enter Pitch Coord: ");
  float pitch_pos = read_float();
  Serial.println();

  Serial.println("Moving servos...");

  // yaw.write(yaw_pos);
  // pitch.write(pitch_pos);

  Serial.print("Calculating current height");
  for (int i = 0; i < 5; i++) {
    Serial.print(".");
    delay(i * 100);
  }
  Serial.println("");

  float height = sin(yaw_pos) + sin(pitch_pos) + zero_height;

  Serial.println("Please verify current height is correct: " + String(height) +
                 "\n\n\n\n\n");
}

float read_float() {
  while (Serial.available() == 0) {
    delay(100);
  }
  float num = Serial.parseFloat();
  Serial.print(num);
  return num;
}