#pragma once
#include <Arduino.h>

namespace EngineConsts {
constexpr float GEAR_RATIO[6] = {
  3.31f,
  1.95f,
  1.28f,
  0.97f,
  0.75f,
  -3.66f
};

constexpr float FINAL_DRIVE = 3.74f;   // przekładnia różnicowa
constexpr float WHEEL_RADIUS = 0.30f;  // m
constexpr float RPM_IDLE = 800.0f;     // obroty jałowe
constexpr float RPM_MAX = 7000.0f;     // maksymalne obroty

constexpr float ENGINE_FORCE_FACTOR = 3000.0f;  // N przy maks. RPM i wciśniętym sprzęgle
constexpr float BRAKE_FORCE_MAX = 5000.0f;      // N przy pełnym pedale hamulca
constexpr float DRAG_COEFF = 0.3f;              // Cd
constexpr float FRONTAL_AREA = 2.2f;            // m^2
}

class Engine {
public:
  Engine(uint8_t throttlePin,
         uint8_t clutchPin,
         uint8_t brakePin);

  void setCarMass(float mass) { _carMass = mass; }

  void update(float dt, uint8_t gearIdx);

  int getRPM() const;
  int getSpeedKmh() const;

  uint8_t rpmToPwm(float rpm);
  void setMotor(uint8_t inA, uint8_t inB, uint8_t ena, float rpm, bool reverse = false);

private:
  const uint8_t _throttlePin;
  const uint8_t _clutchPin;
  const uint8_t _brakePin;

  float _engineRPM = EngineConsts::RPM_IDLE;
  float _speedMs = 0.0f;
  float _carMass = 1200.0f;

  float readNormalized(uint8_t pin) const;
};