#include "config.hpp"
#include "hal/draw.hpp"
#include <array>
#include <cmath>
#include <cstdint>

struct color_stop {
  double value;
  draw::color_t color;
};

draw::color_t interpolateColor(double value,
                               const std::array<color_stop, 11> &stops) {
  if (value <= stops.front().value)
    return stops.front().color;

  if (value >= stops.back().value)
    return stops.back().color;

  for (size_t i = 0; i < stops.size() - 1; ++i) {
    if (value >= stops[i].value && value <= stops[i + 1].value) {

      double t =
          (value - stops[i].value) / (stops[i + 1].value - stops[i].value);

      draw::color_t c0 = stops[i].color;
      draw::color_t c1 = stops[i + 1].color;

      draw::color_t result;
      result.r = static_cast<uint8_t>(std::round(c0.r + t * (c1.r - c0.r)));
      result.g = static_cast<uint8_t>(std::round(c0.g + t * (c1.g - c0.g)));
      result.b = static_cast<uint8_t>(std::round(c0.b + t * (c1.b - c0.b)));

      return result;
    }
  }

  // Fallback
  return stops.back().color;
}

#ifdef CONFIG_METRIC
draw::color_t get_altitude_color(uint32_t value) {
  static constexpr std::array<color_stop, 11> stops = {{
#ifdef CONFIG_METRIC
      {0, {220, 90, 20}},
      {150, {245, 115, 25}},
      {300, {250, 150, 30}},
      {600, {245, 180, 20}},
      {1200, {225, 200, 20}},
      {1800, {175, 215, 30}},
      {2400, {100, 225, 50}},
      {3000, {40, 210, 110}},
      {6000, {40, 175, 215}},
      {9000, {50, 110, 240}},
      {12000, {140, 50, 220}}
#endif
#ifndef CONFIG_METRIC
      {0, {220, 90, 20}},      // Brown/Dark Orange
      {500, {245, 115, 25}},   // Orange
      {1000, {250, 150, 30}},  // Light Orange
      {2000, {245, 180, 20}},  // Yellow-Orange
      {4000, {225, 200, 20}},  // Yellow
      {6000, {175, 215, 30}},  // Yellow-Green
      {8000, {100, 225, 50}},  // Light Green
      {10000, {40, 210, 110}}, // Teal/Cyan
      {20000, {40, 175, 215}}, // Cyan-Blue
      {30000, {50, 110, 240}}, // Blue
      {40000, {140, 50, 220}}  // Purple
#endif
  }};
  return interpolateColor(value, stops);
}
#endif