#include "../res/5x7_dotmatrix_charset.h"
#include "config.hpp"
#include "core/font.hpp"
#include "core/helpers.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;
}


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}}
#else
      {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);
}

enum letter_id {
  LETTER_A,
  LETTER_B,
  LETTER_C,
  LETTER_D,
  LETTER_E,
  LETTER_F,
  LETTER_G,
  LETTER_H,
  LETTER_I,
  LETTER_J,
  LETTER_K,
  LETTER_L,
  LETTER_M,
  LETTER_N,
  LETTER_O,
  LETTER_P,
  LETTER_Q,
  LETTER_R,
  LETTER_S,
  LETTER_T,
  LETTER_U,
  LETTER_V,
  LETTER_W,
  LETTER_X,
  LETTER_Y,
  LETTER_Z,
  LETTER_a,
  LETTER_b,
  LETTER_c,
  LETTER_d,
  LETTER_e,
  LETTER_f,
  LETTER_g,
  LETTER_h,
  LETTER_i,
  LETTER_j,
  LETTER_k,
  LETTER_l,
  LETTER_m,
  LETTER_n,
  LETTER_o,
  LETTER_p,
  LETTER_q,
  LETTER_r,
  LETTER_s,
  LETTER_t,
  LETTER_u,
  LETTER_v,
  LETTER_w,
  LETTER_x,
  LETTER_y,
  LETTER_z,
  LETTER_0,
  LETTER_1,
  LETTER_2,
  LETTER_3,
  LETTER_4,
  LETTER_5,
  LETTER_6,
  LETTER_7,
  LETTER_8,
  LETTER_9
};

namespace font {
void putchar(const char c, uint16_t x, uint16_t y, const draw::color_t color) {
  uint8_t char_index;
  if (c >= 'a' && c <= 'z') {
  char_index = (c - 'a') + LETTER_a;
} else if (c >= 'A' && c <= 'Z') {
  char_index = (c - 'A') + LETTER_A;
} else if (c >= '0' && c <= '9') {
  char_index = (c - '0') + LETTER_0;
} else {
  return; // unsupported character
}
  const uint16_t character_bit_offset = char_index * FONT_WIDTH;
  const uint16_t x_orig = x;
  for (uint8_t i = 0; i < FONT_HEIGHT; i++) {
    x = x_orig;
    for (uint8_t j = 0; j < FONT_WIDTH; j++) {
      if (get_Nth_bit_in_grid(_5x7_dotmatrix_charset,
                              static_cast<std::remove_const_t<decltype(character_bit_offset)>>(
                                  character_bit_offset + j),
                              static_cast<std::remove_const_t<decltype(character_bit_offset)>>(i),
                              static_cast<std::remove_const_t<decltype(character_bit_offset)>>(
                                  _5X7_DOTMATRIX_CHARSET_WIDTH))) {
        draw::pixel(x, y, color);
      }
      x++;
    }
    y++;
  }
}
void puts(const std::string &s, const uint16_t x, const uint16_t y, const draw::color_t color) {
  for (size_t i = 0; i < s.length(); i++) {
    putchar(s[i], x+((FONT_WIDTH+CONFIG_CHARACTER_SPACING)*i), y, color);
  }
}
} // namespace font