#include "vehicle.h"

void Vehicle::step(double dt, double wheel_torque_nm) {
    double drive_force_n = wheel_torque_nm / kWheelRadiusM;
    double drag_force_n = kDragCoeff * m_speed_mps * m_speed_mps;

    double net_force_n = drive_force_n - drag_force_n;
    double accel_mps2 = net_force_n / kMassKg;

    m_speed_mps += accel_mps2 * dt;
    if (m_speed_mps < 0.0) m_speed_mps = 0.0; // no reverse yet
}

double Vehicle::speed_mps() const {
    return m_speed_mps;
}

double Vehicle::wheel_rpm() const {
    double wheel_rad_per_s = m_speed_mps / kWheelRadiusM;
    return wheel_rad_per_s * 60.0 / (2.0 * 3.14159265358979);
}