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

#define PIN_VERT A1
#define PIN_HORZ A0
#define PIN_FIRE 3
#define PIN_LED 13

#define PIN_SERVO_E A3
#define PIN_SERVO_A A2

Servo servo_elevation, servo_azimuth;
float servo_step = 5.0;
float elevation = 0.0;
float azimuth = 0.0;

LiquidCrystal_I2C lcd(0x27, 20, 4); // set the LCD address to 0x27 for a 16 chars and 2 line display

volatile bool fire_button_pressed = false;
int ammo = 10;

int read_axis(uint8_t pin) {
  return map(analogRead(pin), 0, 1023, -1, 1);
}

void write(Servo& servo, float deg) {
  servo.write(deg + 90.0f);
}

float clamp(float v, float min_v, float max_v) {
  return max(min_v, min(max_v, v));
}

void setup()
{
  pinMode(PIN_VERT, INPUT);
  pinMode(PIN_HORZ, INPUT);
  pinMode(PIN_FIRE, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PIN_FIRE), on_fire_pressed, RISING);

  servo_elevation.attach(PIN_SERVO_E);
  write(servo_elevation, elevation);

  servo_azimuth.attach(PIN_SERVO_A);
  write(servo_azimuth, azimuth);

  lcd.init();
}

void on_fire_pressed()
{
  fire_button_pressed = true;
}

void loop() {

  if(fire_button_pressed && ammo > 0) {
    fire_button_pressed = false;
    ammo--;
    digitalWrite(PIN_LED, HIGH);
    tone(2, 100, 50);
    digitalWrite(PIN_LED, LOW);
  }

  int vert = read_axis(PIN_VERT);
  int horz = -read_axis(PIN_HORZ);

  if (vert != 0)
  {
    elevation += vert * servo_step;
    elevation = clamp(elevation, -90, 90);
    write(servo_elevation, elevation);
  }

  if (horz != 0)
  {
    azimuth += horz * servo_step;
    azimuth = clamp(azimuth, -90, 90);
    write(servo_azimuth, azimuth);
  }

  lcd.setCursor(0, 0);
  lcd.print("Azimuth: " + String(azimuth) + "   ");
  lcd.setCursor(0, 1);
  lcd.print("Elevation: " + String(elevation) + "   ");
  lcd.setCursor(0, 2);
  lcd.print("Ammo: " + String(ammo) + "   ");

  delay(5);
}