#include "Engine.h"
using namespace EngineConsts;

Engine::Engine(uint8_t throttlePin,
               uint8_t clutchPin,
               uint8_t brakePin)
  : _throttlePin(throttlePin), _clutchPin(clutchPin), _brakePin(brakePin) {}

void Engine::begin() {
}

float Engine::readNormalized(uint8_t pin) const {
  return analogRead(pin) / 1023.0f;
}

void Engine::update(float dt, uint8_t gearIdx) {
  // Serial.println("----------------");

  const float throttle = readNormalized(_throttlePin);
  // Serial.print("Throttle: ");
  // Serial.println(throttle);

  const float clutch = readNormalized(_clutchPin);
  // Serial.print("Clutch: ");
  // Serial.println(clutch);

  const float brake = readNormalized(_brakePin);
  // Serial.print("Brake: ");
  // Serial.println(brake);

  const float targetRPM = RPM_IDLE + throttle * (RPM_MAX - RPM_IDLE);
  // Serial.print("targetRPM: ");
  // Serial.println(targetRPM);

  const float wheelRPM = (_speedMs / (2.0f * PI * WHEEL_RADIUS)) * 60.0f;
  // Serial.print("wheelRPM: ");
  // Serial.println(wheelRPM);

  const float gearRatio = GEAR_RATIO[gearIdx];
  // Serial.print("gearRatio: ");
  // Serial.println(gearRatio);

  const float drivenRPM = wheelRPM * fabs(gearRatio) * FINAL_DRIVE;
  // Serial.print("drivenRPM: ");
  // Serial.println(drivenRPM);

  _engineRPM = clutch * drivenRPM + (1.0f - clutch) * targetRPM;
  // Serial.print("_engineRPM: ");
  // Serial.println(_engineRPM);

  _engineRPM = constrain(_engineRPM, RPM_IDLE, 7500.0f);
  // Serial.print("_engineRPM (constrain): ");
  // Serial.println(_engineRPM);

  const float engineForce = (_engineRPM / RPM_MAX) * (1.0f - clutch) * ENGINE_FORCE_FACTOR;
  // Serial.print("engineForce: ");
  // Serial.println(engineForce);

  const float brakeForce = brake * BRAKE_FORCE_MAX;
  // Serial.print("brakeForce: ");
  // Serial.println(brakeForce);

  const float dragForce = 0.5f * DRAG_COEFF * FRONTAL_AREA * (_speedMs * _speedMs);
  // Serial.print("dragForce: ");
  // Serial.println(dragForce);

  const int direction = (gearIdx == -1) ? -1 : 1;
  // Serial.print("direction: ");
  // Serial.println(direction);

  const float netForce = direction * engineForce - brakeForce - dragForce;
  // Serial.print("netForce: ");
  // Serial.println(netForce);

  const float acceleration = netForce / CAR_MASS;
  // Serial.print("acceleration: ");
  // Serial.println(acceleration);

  _speedMs += acceleration * dt;
  if (_speedMs < 0.0f) _speedMs = 0.0f;
  // Serial.print("_speedMs: ");
  // Serial.println(_speedMs);

  // delay(1000);
}

int Engine::getRPM() const {
  return static_cast<int>(_engineRPM + 0.5f);
}

int Engine::getSpeedKmh() const {
  return static_cast<int>((_speedMs * 3.6f) + 0.5f);
}