#include "fgc/TmcCurrent.h"

#include <algorithm>
#include <cmath>
#include <sstream>

namespace fgc {
namespace {

// Formatting helper: milliamps to one decimal, for warning text that has to name
// both what was asked for and what the hardware can actually do.
std::string ma(double v) {
    std::ostringstream os;
    os.setf(std::ios::fixed);
    os.precision(1);
    os << v;
    return os.str();
}

constexpr int kGsMin = 32;   // below this the datasheet disallows GLOBALSCALER
constexpr int kGsMax = 255;  // 0 would mean 256 (full scale) - never emitted
constexpr int kIrunMax = 31;

}  // namespace

double tmcRmsCurrentMa(int global_scaler, int irun, double r_sense_ohm, double v_fs) {
    if (r_sense_ohm <= 0.0) return 0.0;
    const double gs = (global_scaler <= 0) ? 256.0 : static_cast<double>(global_scaler);
    return (gs / 256.0) * ((irun + 1) / 32.0) * (v_fs / (r_sense_ohm + 0.02)) /
           std::sqrt(2.0) * 1000.0;
}

TmcCurrentPlan planTmcCurrent(double r_sense_ohm, double run_ma, double homing_ma,
                              double v_fs) {
    TmcCurrentPlan p;
    if (r_sense_ohm <= 0.0) {
        p.warnings.push_back("sense resistor must be positive, got " +
                             ma(r_sense_ohm) + " ohm");
        return p;
    }
    if (run_ma <= 0.0) {
        p.warnings.push_back("run current must be positive, got " + ma(run_ma) + " mA");
        return p;
    }
    if (homing_ma <= 0.0) {
        p.warnings.push_back("homing current must be positive, got " + ma(homing_ma) + " mA");
        return p;
    }

    // The most the bridge can deliver: GLOBALSCALER 256, IRUN 31.
    const double i_max = tmcRmsCurrentMa(0, kIrunMax, r_sense_ohm, v_fs);

    // Pick GLOBALSCALER so the RUN current sits at IRUN 31.
    double gs_ideal = 256.0 * run_ma / i_max;
    if (gs_ideal > kGsMax) {
        p.global_scaler = kGsMax;
        p.warnings.push_back("run current " + ma(run_ma) + " mA is above the " + ma(i_max) +
                             " mA this driver can deliver with a " + ma(r_sense_ohm) +
                             " ohm sense resistor; using the maximum instead");
    } else if (gs_ideal < kGsMin) {
        // GLOBALSCALER 1..31 is forbidden, so a very small run current has to be
        // reached by lowering IRUN instead, at some cost in microstep resolution.
        p.global_scaler = kGsMin;
        p.warnings.push_back("run current " + ma(run_ma) +
                             " mA needs GLOBALSCALER below the permitted minimum of 32; "
                             "holding it at 32 and reducing IRUN instead, which coarsens "
                             "microstepping");
    } else {
        p.global_scaler = static_cast<int>(std::lround(gs_ideal));
    }

    // Full-scale current at the chosen scaler, i.e. what IRUN 31 now means.
    const double full = tmcRmsCurrentMa(p.global_scaler, kIrunMax, r_sense_ohm, v_fs);

    auto irunFor = [&](double target) {
        return std::clamp(static_cast<int>(std::lround(32.0 * target / full)) - 1, 0, kIrunMax);
    };

    p.run_irun = irunFor(run_ma);

    if (homing_ma > run_ma) {
        // Not a feature: a homing current above the run current defeats the point
        // of having a separate one.
        p.homing_irun = p.run_irun;
        p.warnings.push_back("homing current " + ma(homing_ma) +
                             " mA is above the run current " + ma(run_ma) +
                             " mA; clamping the search to the run current");
    } else {
        p.homing_irun = irunFor(homing_ma);
        if (p.homing_irun > p.run_irun) p.homing_irun = p.run_irun;
    }

    p.run_ma    = tmcRmsCurrentMa(p.global_scaler, p.run_irun, r_sense_ohm, v_fs);
    p.homing_ma = tmcRmsCurrentMa(p.global_scaler, p.homing_irun, r_sense_ohm, v_fs);

    // The smallest non-zero step this scaler can express. Worth naming when a
    // homing target lands under it, because the result is then noticeably
    // stronger than asked for - the direction that matters for a yielding stop.
    const double step = full / 32.0;
    if (p.homing_irun == 0 && homing_ma < step * 0.5) {
        p.warnings.push_back("homing current " + ma(homing_ma) +
                             " mA is below the smallest step this driver can express (" +
                             ma(step) + " mA); using " + ma(p.homing_ma) + " mA");
    }

    auto driftWarn = [&](const char* what, double want, double got) {
        if (want > 0.0 && std::fabs(got - want) / want > 0.10)
            p.warnings.push_back(std::string(what) + " current " + ma(want) +
                                 " mA is not reachable; nearest is " + ma(got) + " mA");
    };
    driftWarn("run", run_ma, p.run_ma);
    if (homing_ma <= run_ma) driftWarn("homing", homing_ma, p.homing_ma);

    p.ok = true;
    return p;
}

}  // namespace fgc