#include "track_core/model.hpp"

#include <cmath>
#include <cstdio>

namespace track_core {
namespace {
template <class... Ts>
struct overloads : Ts... {
    using Ts::operator()...;
};
} // namespace

std::string Color::hex() const {
    char buffer[8]{};
    static_cast<void>(std::snprintf(buffer, sizeof(buffer), "#%02X%02X%02X", r, g, b));
    return buffer;
}

expected<unit, TrackError> TrackPidConfig::validate(bool allow_empty) const {
    const auto speed_suppression_ok =
        !speed_suppression.has_value() ||
        (std::isfinite(speed_suppression->ratio_min) &&
         std::isfinite(speed_suppression->sigma_m) &&
         speed_suppression->ratio_min > 0.0F &&
         speed_suppression->ratio_min < 1.0F &&
         speed_suppression->sigma_m > 0.0F);
    if (!speed_suppression_ok) {
        return unexpected<TrackError>{TrackError::invalid_arg};
    }
    if (schemas.empty()) {
        if (allow_empty) {
            return {};
        }
        return unexpected<TrackError>{TrackError::invalid_arg};
    }
    for (const auto &schema : schemas) {
        const auto ok = std::visit(overloads{
                                       [](const TrackPidSegment &pid) {
                                           return pid.duration_s > 0 &&
                                                  std::isfinite(pid.min_speed_m_s) &&
                                                  std::isfinite(pid.max_speed_m_s) &&
                                                  std::isfinite(pid.kp) &&
                                                  std::isfinite(pid.ki) &&
                                                  std::isfinite(pid.kd) &&
                                                  std::isfinite(pid.slew_rate_limit) &&
                                                  (!pid.fine_tune.has_value() ||
                                                   (std::isfinite(pid.fine_tune->gain_scale) &&
                                                    pid.fine_tune->gain_scale >= 0.0F)) &&
                                                  pid.min_speed_m_s >= 0.0F &&
                                                  pid.max_speed_m_s >= 0.0F &&
                                                  pid.min_speed_m_s <= pid.max_speed_m_s &&
                                                  pid.kp >= 0.0F &&
                                                  pid.ki >= 0.0F &&
                                                  pid.kd >= 0.0F &&
                                                  pid.slew_rate_limit >= 0.0F;
                                       },
                                       [](const TrackConstantSegment &constant) {
                                           return constant.duration_s > 0 &&
                                                  std::isfinite(constant.speed_m_s) &&
                                                  constant.speed_m_s >= 0.0F;
                                       }},
                                   schema.segment);
        if (!ok) {
            return unexpected<TrackError>{TrackError::invalid_arg};
        }
    }
    return {};
}

expected<unit, TrackError> TrackPidSetTuning::validate() const {
    const auto fine_tune_ok =
        !fine_tune.has_value() ||
        (std::isfinite(fine_tune->gain_scale) && fine_tune->gain_scale >= 0.0F);
    if (!std::isfinite(min_speed_m_s) ||
        !std::isfinite(max_speed_m_s) ||
        !std::isfinite(kp) ||
        !std::isfinite(ki) ||
        !std::isfinite(kd) ||
        !std::isfinite(slew_rate_limit) ||
        !fine_tune_ok ||
        min_speed_m_s < 0.0F ||
        max_speed_m_s < 0.0F ||
        min_speed_m_s > max_speed_m_s ||
        kp < 0.0F ||
        ki < 0.0F ||
        kd < 0.0F ||
        slew_rate_limit < 0.0F) {
        return unexpected<TrackError>{TrackError::invalid_arg};
    }
    return {};
}

void TrackPidSetTuning::apply_to(TrackPidSegment &segment) const {
    segment.min_speed_m_s = min_speed_m_s;
    segment.max_speed_m_s = max_speed_m_s;
    segment.kp = kp;
    segment.ki = ki;
    segment.kd = kd;
    segment.slew_rate_limit = slew_rate_limit;
    segment.fine_tune = fine_tune;
}

} // namespace track_core