#include <EEPROM.h>
#include <Servo.h>

Servo servo;
int gear_angles[] = {1, 20, 30, 43, 65, 87, 102};
int current_gear = 1;
int trim_mode = 4;
int next_button = 2;
int prev_button = 3;

void setup() {
  servo.attach(9);
  pinMode(next_button, INPUT_PULLUP);
  pinMode(prev_button, INPUT_PULLUP);
  pinMode(trim_mode, INPUT_PULLUP);
  int saved_gear = EEPROM.read(0);
  if (saved_gear > 0 && saved_gear <= 7) {
    current_gear = saved_gear;
    servo.write(gear_angles[current_gear - 1]);
  }
}

void loop() {
  if (digitalRead(trim_mode) == LOW) {
    calibrateMode();
  } else {
    shiftMode();
  }
}

void shiftMode() {
  if (digitalRead(next_button) == LOW && current_gear < 7) {
    current_gear++;
    servo.write(gear_angles[current_gear - 1]);
  }
  if (digitalRead(prev_button) == LOW && current_gear > 1) {
    current_gear--;
    servo.write(gear_angles[current_gear - 1]);
  }
  EEPROM.write(0, current_gear);
  delay(100);
}

void calibrateMode() {
  int servo_angle = gear_angles[current_gear - 1];
  if (digitalRead(next_button) == LOW) {
    servo_angle++;
    servo.write(servo_angle);
  }
  if (digitalRead(prev_button) == LOW) {
    servo_angle--;
    servo.write(servo_angle);
  }
  gear_angles[current_gear - 1] = servo_angle;
  delay(100);
}