#include <cmath>
#include <cstdlib>
#include <iostream>
#include <vector>

#include "track_core/memory_strip.hpp"
#include "track_core/pid_program.hpp"
#include "track_core/scheme_decoder.hpp"

namespace {

void require(bool condition, const char *message) {
    if (!condition) {
        std::cerr << "FAIL: " << message << '\n';
        std::exit(1);
    }
}

void require_eq_u32(std::uint32_t actual, std::uint32_t expected, const char *message) {
    if (actual != expected) {
        std::cerr << "FAIL: " << message << ": actual=" << actual << " expected=" << expected << '\n';
        std::exit(1);
    }
}

void require_near(float actual, float expected, float tolerance, const char *message) {
    if (std::fabs(actual - expected) > tolerance) {
        std::cerr << "FAIL: " << message << ": actual=" << actual << " expected=" << expected << '\n';
        std::exit(1);
    }
}

track_core::TrackInfo running_info(track_core::Color color = track_core::Color::green()) {
    return {
        .kind = track_core::SchemeKind::speed_input_time_segmented_mileage_free,
        .color = color,
        .id = 1,
        .is_running = true,
        .num_segments = 1,
    };
}

void test_circular_render_wraps() {
    track_core::TrackConfig config{
        .draw_kind = track_core::TrackDrawKind::circular,
        .line_length_m = 10.0F,
        .active_line_length_m = 4.0F,
        .head_offset_m = 0.0F,
        .line_leds_num = 20,
    };
    const track_core::TrackReport report{
        .id = 1,
        .state = track_core::TrackState::run,
        .mileage_m = 8.5F,
        .speed_m_s = 1.0F,
    };

    const auto plan = track_core::make_track_render_plan(config, running_info(), report);

    require_eq_u32(static_cast<std::uint32_t>(plan.span_count), 2, "circular span count");
    require_eq_u32(plan.spans[0].start_led, 17, "circular span 0 start");
    require_eq_u32(plan.spans[0].led_count, 3, "circular span 0 count");
    require(plan.spans[0].color == track_core::Color::green(), "circular span 0 color");
    require_eq_u32(plan.spans[1].start_led, 0, "circular span 1 start");
    require_eq_u32(plan.spans[1].led_count, 5, "circular span 1 count");
}

void test_linear_render_forward_and_memory_strip() {
    track_core::TrackConfig config{
        .draw_kind = track_core::TrackDrawKind::linear,
        .line_length_m = 10.0F,
        .active_line_length_m = 4.0F,
        .head_offset_m = 0.0F,
        .line_leds_num = 20,
    };
    const track_core::TrackReport report{
        .id = 2,
        .state = track_core::TrackState::run,
        .mileage_m = 5.0F,
        .speed_m_s = 2.0F,
    };
    track_core::MemoryStrip strip(config.line_leds_num);

    const auto plan = track_core::make_track_render_plan(config, running_info(track_core::Color::red()), report);
    const auto applied = track_core::apply_render_plan(strip, plan);

    require(applied == track_core::TrackError::ok, "linear plan applies");
    require_eq_u32(static_cast<std::uint32_t>(plan.span_count), 4, "linear span count");
    require_eq_u32(plan.spans[0].start_led, 10, "linear span 0 start");
    require_eq_u32(plan.spans[0].led_count, 2, "linear span 0 count");
    require(plan.spans[0].color == track_core::Color::red(), "linear span 0 color");
    require_eq_u32(plan.spans[1].start_led, 12, "linear span 1 start");
    require(plan.spans[1].color == track_core::Color::blue(), "linear span 1 color");
    require_eq_u32(plan.spans[2].start_led, 6, "linear span 2 start");
    require(plan.spans[2].color == track_core::Color::cyan(), "linear span 2 color");
    require_eq_u32(plan.spans[3].start_led, 8, "linear span 3 start");
    require(plan.spans[3].color == track_core::Color::red(), "linear span 3 color");
}

void test_linear_render_reverse() {
    track_core::TrackConfig config{
        .draw_kind = track_core::TrackDrawKind::linear,
        .line_length_m = 10.0F,
        .active_line_length_m = 5.0F,
        .head_offset_m = 0.0F,
        .line_leds_num = 20,
    };
    const track_core::TrackReport report{
        .id = 3,
        .state = track_core::TrackState::run,
        .mileage_m = 15.0F,
        .speed_m_s = 2.0F,
    };

    const auto plan = track_core::make_track_render_plan(config, running_info(track_core::Color::red()), report);

    require_eq_u32(static_cast<std::uint32_t>(plan.span_count), 4, "reverse span count");
    require_eq_u32(plan.spans[0].start_led, 5, "reverse span 0 start");
    require_eq_u32(plan.spans[0].led_count, 2, "reverse span 0 count");
    require(plan.spans[0].color == track_core::Color::blue(), "reverse span 0 color");
    require_eq_u32(plan.spans[1].start_led, 7, "reverse span 1 start");
    require_eq_u32(plan.spans[1].led_count, 3, "reverse span 1 count");
    require(plan.spans[1].color == track_core::Color::red(), "reverse span 1 color");
    require_eq_u32(plan.spans[2].start_led, 10, "reverse span 2 start");
    require_eq_u32(plan.spans[2].led_count, 3, "reverse span 2 count");
    require(plan.spans[2].color == track_core::Color::red(), "reverse span 2 color");
    require_eq_u32(plan.spans[3].start_led, 13, "reverse span 3 start");
    require_eq_u32(plan.spans[3].led_count, 2, "reverse span 3 count");
    require(plan.spans[3].color == track_core::Color::cyan(), "reverse span 3 color");
}

void test_memory_strip_bounds() {
    track_core::MemoryStrip strip(4);

    require(strip.fill(0, 0, track_core::Color::red()) == track_core::TrackError::ok, "zero fill");
    require(strip.fill(2, 8, track_core::Color::blue()) == track_core::TrackError::ok, "clamped fill");
    require(strip.pixels()[2] == track_core::Color::blue(), "clamped fill pixel 2");
    require(strip.pixels()[3] == track_core::Color::blue(), "clamped fill pixel 3");
    require(strip.fill(4, 1, track_core::Color::red()) == track_core::TrackError::range, "out-of-range fill");
}

void test_scheme_decoder() {
    const std::vector<std::uint8_t> data{
        static_cast<std::uint8_t>(track_core::SchemeKind::speed_input_time_segmented_mileage_free),
        static_cast<std::uint8_t>(track_core::AccelerationProfile::instant),
        2,
        0, 0, 0,
        51, 10, 0,
    };

    const auto decoded = track_core::decode_scheme(7, track_core::Color::white(), data);

    require(decoded.has_value(), "decode ST scheme");
    require(decoded->id == 7, "decoded id");
    require(decoded->kind == track_core::SchemeKind::speed_input_time_segmented_mileage_free, "decoded kind");
    const auto *segments = std::get_if<std::vector<track_core::STSegment>>(&decoded->segments);
    require(segments != nullptr, "decoded ST segment vector");
    require_eq_u32(static_cast<std::uint32_t>(segments->size()), 2, "decoded ST segment count");
    require_near((*segments)[1].speed_m_s, 2.0F, 0.01F, "decoded ST speed");
    require_eq_u32((*segments)[1].time_since_start_s, 10, "decoded ST time");
}

void test_pid_program_constant() {
    track_core::TrackPidConfig config;
    config.schemas = {
        track_core::TrackPidSchema{track_core::TrackConstantSegment{
            .duration_s = 2,
            .speed_m_s = 1.5F,
        }},
    };
    const auto start = track_core::clock::time_point(std::chrono::seconds(0));
    track_core::TrackPidProgramState state(start, config);

    require_near(state.next(start + std::chrono::seconds(1), std::nullopt), 1.5F, 0.0001F, "constant PID speed");
    require(!state.is_finished(start + std::chrono::seconds(1)), "constant PID not finished");
    require(state.is_finished(start + std::chrono::seconds(2)), "constant PID finished");
}

} // namespace

int main() {
    test_circular_render_wraps();
    test_linear_render_forward_and_memory_strip();
    test_linear_render_reverse();
    test_memory_strip_bounds();
    test_scheme_decoder();
    test_pid_program_constant();
    std::cout << "track-core tests passed\n";
    return 0;
}