#include <Servo.h>

Servo servoRPM;
Servo servoSpeed;

// RPM
int rpm = 0;
const int idleRPM = 800;
const int maxRPM = 8000;

// Speed
int speed = 0;
const int maxSpeed = 250;

// Simulation
int throttle = 0;
// clutch
int clutch = 0; // 100 - coupled
// gearbox
const double gearRatios[] = {0, 5.5, 3.52, 2.2, 1.72, 1.317, 1.0, 0.823, 0.64};
const int finalDrive = 3.55;
int outputRPM = 0;
int gearIndex = 0;
const int maxGearIndex = 8;
int minShiftDownRPM = 1000;
int maxShiftDownRPM = 4000;
int minShiftUpRPM = 1600;
int maxShiftUpRPM = 7000;

void updateRpmServo() {
  int pos = map(rpm, 0, maxRPM, 0, 180);
  servoRPM.write(pos);
}

void updateSpeedServo() {
  int pos = map(speed, 0, maxSpeed, 0, 180);
  servoSpeed.write(pos);
}

void updateSimulation() {
  // simulate power
  int targetRPM = idleRPM + ((maxRPM - idleRPM) * (throttle / 100.0));
  double gearFactor = 1.0 - (gearIndex / (maxSpeed + 1));
  double speedFactor = 1.0 - (min(max(speed, 1), maxSpeed - 1) / maxSpeed);
  int rpmDiff = (targetRPM - rpm) / (10.0 * gearFactor * speedFactor);
  rpm = rpm + rpmDiff;
  // Serial.println(rpm);

  // gear
  int shiftDownRPM = minShiftDownRPM + ((maxShiftDownRPM - minShiftDownRPM) * (throttle / 100));
  int shiftUpRPM = minShiftUpRPM + ((maxShiftUpRPM - minShiftUpRPM) * (throttle / 100));
  if(rpm > shiftUpRPM && gearIndex < maxGearIndex) {
    gearIndex++;
    Serial.println(gearIndex);
    rpm = outputRPM * finalDrive * gearRatios[gearIndex];
  } else if(rpm < shiftDownRPM && gearIndex > 1) {
    gearIndex--;
    Serial.println(gearIndex);
    rpm = outputRPM * finalDrive * gearRatios[gearIndex];
  }

  // rpm to speed
  outputRPM = rpm / gearRatios[gearIndex] / finalDrive;
  speed = outputRPM / 10;
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  servoRPM.attach(6);
  servoSpeed.attach(5);

  rpm = idleRPM;
  gearIndex = 1;

  // needle sweep
  servoRPM.write(180);
  servoSpeed.write(180);
  delay(600);
  servoRPM.write(0);
  servoSpeed.write(0);
  delay(600);
}

void loop() {
  // put your main code here, to run repeatedly:
  // Read potentiometers
  int valLeft = analogRead(1);
  int valRight = analogRead(0);

  // left
  // clutch = map(valLeft, 0, 1023, 0, maxRPM);
  // right
  throttle = map(valRight, 0, 1023, 0, 100);

  updateSimulation();

  // Convert val to RPM and Speed
  // rpm = map(valRPM, 0, 1023, 0, maxRPM);
  // speed = map(valSpeed, 0, 1023, 0, maxSpeed);

  // Serial.println(rpm);
  // Serial.println(speed);

  // Update Servos
  updateRpmServo();
  updateSpeedServo();

  delay(100);
}