#include "fgc/AxisSpeed.h"

#include <cmath>

namespace fgc {
namespace {

std::string num(double v) {
    char buf[32];
    std::snprintf(buf, sizeof(buf), "%.3g", v);
    return buf;
}

}  // namespace

double vmaxToDegPerSec(long vmax, double counts_per_deg, double clock_hz) {
    const double cpd = std::fabs(counts_per_deg);
    if (cpd <= 0.0 || clock_hz <= 0.0) return 0.0;
    return (static_cast<double>(vmax) * clock_hz / kTmcVmaxDiv) / cpd;
}

AxisSpeedPlan planAxisSpeed(double deg_per_s, double counts_per_deg, double clock_hz) {
    AxisSpeedPlan p;
    p.requested_deg_s = deg_per_s;

    // A speed is a magnitude; the direction sense lives in counts_per_deg's sign
    // and belongs to Geometry, not here.
    const double cpd = std::fabs(counts_per_deg);

    if (deg_per_s <= 0.0) {
        p.error = "speed must be > 0 deg/s, got " + num(deg_per_s);
        return p;
    }
    if (cpd < kMinPlausibleCountsPerDeg) {
        p.error = "counts_per_deg is " + num(counts_per_deg) +
                  ", which is not a calibrated drivetrain (an ungeared AS5047P alone reads 11.1) "
                  "-- calibrate the axis first ('gimbal heading setup'), or declare "
                  "[Motor.<axis>] travel_deg so homing can derive it";
        return p;
    }
    if (clock_hz <= 0.0) {
        p.error = "TMC clock must be > 0 Hz";
        return p;
    }

    const double counts_per_s = deg_per_s * cpd;
    const double exact        = counts_per_s * kTmcVmaxDiv / clock_hz;

    // Round rather than truncate: at low speeds truncation biases every axis slow,
    // and at 1 count/s it would round to zero and stop the gimbal outright.
    long vmax = std::lround(exact);

    if (vmax > kTmcVmaxMax) {
        vmax      = kTmcVmaxMax;
        p.clamped = true;
    }
    if (vmax < 1) {
        // Anything that rounds below one unit cannot be commanded at all. Failing
        // is right: silently substituting the slowest possible speed would look
        // like a working axis that never finishes a sweep.
        p.error = num(deg_per_s) + " deg/s is below what VMAX can express on this drivetrain (" +
                  num(cpd) + " counts/deg); the slowest commandable speed is " +
                  num(vmaxToDegPerSec(1, cpd, clock_hz)) + " deg/s";
        return p;
    }

    p.ok           = true;
    p.vmax         = vmax;
    p.actual_deg_s = vmaxToDegPerSec(vmax, cpd, clock_hz);
    return p;
}

}  // namespace fgc