#include "LCD_driver.h"
#include "FreeRTOS.h"
#include "task.h"
#include <stdio.h>
#include <cmath>

static float hue_offset = 0.0f;
static const float HUE_STEP = 36.0f; // 10タップで360度回転

LCD_Driver::LCD_Driver() {}

void LCD_Driver::writeCommand(uint8_t cmd) {
    gpio_put(PIN_CS, 0);
    gpio_put(PIN_DC, 0);
    spi_write_blocking(SPI_PORT, &cmd, 1);
    gpio_put(PIN_CS, 1);
}

void LCD_Driver::writeData(uint8_t data) {
    gpio_put(PIN_CS, 0);
    gpio_put(PIN_DC, 1);
    spi_write_blocking(SPI_PORT, &data, 1);
    gpio_put(PIN_CS, 1);
}

void LCD_Driver::setAddrWindow(uint16_t x, uint16_t y, uint16_t w, uint16_t h) {
    writeCommand(0x2A);
    writeData(x >> 8); writeData(x & 0xFF);
    writeData((x + w - 1) >> 8); writeData((x + w - 1) & 0xFF);
    writeCommand(0x2B);
    writeData(y >> 8); writeData(y & 0xFF);
    writeData((y + h - 1) >> 8); writeData((y + h - 1) & 0xFF);
    writeCommand(0x2C);
}

void LCD_Driver::init() {
    printf("[LCD] Initializing Hardware SPI and Pins...\n");
    spi_init(SPI_PORT, 5 * 1000 * 1000);
    gpio_set_function(PIN_MISO, GPIO_FUNC_SPI);
    gpio_set_function(PIN_SCK, GPIO_FUNC_SPI);
    gpio_set_function(PIN_MOSI, GPIO_FUNC_SPI);

    gpio_init(PIN_CS);   gpio_set_dir(PIN_CS, GPIO_OUT);   gpio_put(PIN_CS, 1);
    gpio_init(PIN_DC);   gpio_set_dir(PIN_DC, GPIO_OUT);   gpio_put(PIN_DC, 0);
    gpio_init(PIN_RST);  gpio_set_dir(PIN_RST, GPIO_OUT);  gpio_put(PIN_RST, 1);

    printf("[LCD] Resetting Display...\n");
    gpio_put(PIN_RST, 0); sleep_ms(100);
    gpio_put(PIN_RST, 1); sleep_ms(200);

    writeCommand(0x01); sleep_ms(150);
    writeCommand(0x11); sleep_ms(200);
    writeCommand(0x3A); writeData(0x55);
    writeCommand(0x29);
}

void LCD_Driver::drawHSBBackground() {
    printf("[LCD] Drawing HSB Background (Brightness: 0.5-1.0)...\n");
    setAddrWindow(0, 0, 240, 320);
    gpio_put(PIN_CS, 0);
    gpio_put(PIN_DC, 1);

    for (int y = 0; y < 320; y++) {
        if (y % 40 == 0) {
            gpio_put(PIN_CS, 1);
            vTaskDelay(pdMS_TO_TICKS(1));
            setAddrWindow(0, y, 240, 320 - y);
            gpio_put(PIN_CS, 0);
            gpio_put(PIN_DC, 1);
        }

        // 明度(Value)を 0.5 ~ 1.0 の範囲に設定
        float v = 0.5f + ((float)y / 319.0f * 0.5f);

        for (int x = 0; x < 240; x++) {
            float h = fmodf(((float)x / 239.0f * 360.0f) + hue_offset, 360.0f);
            if (h < 0) h += 360.0f;

            uint16_t color = hsvToRgb565(h, 1.0f, v);
            uint8_t buf[2] = { (uint8_t)(color >> 8), (uint8_t)(color & 0xFF) };
            spi_write_blocking(SPI_PORT, buf, 2);
        }
    }
    gpio_put(PIN_CS, 1);
}

uint16_t LCD_Driver::hsvToRgb565(float h, float s, float v) {
    float r = 0, g = 0, b = 0;
    float c = v * s;
    float x_val = c * (1.0f - fabsf(fmodf(h / 60.0f, 2.0f) - 1.0f));
    float m = v - c;

    if (h < 60)        { r = c; g = x_val; b = 0; }
    else if (h < 120)  { r = x_val; g = c; b = 0; }
    else if (h < 180)  { r = 0; g = c; b = x_val; }
    else if (h < 240)  { r = 0; g = x_val; b = c; }
    else if (h < 300)  { r = x_val; g = 0; b = c; }
    else               { r = c; g = 0; b = x_val; }

    uint16_t r5 = (uint16_t)((r + m) * 31.0f + 0.5f);
    uint16_t g6 = (uint16_t)((g + m) * 63.0f + 0.5f);
    uint16_t b5 = (uint16_t)((b + m) * 31.0f + 0.5f);
    return (uint16_t)((r5 << 11) | (g6 << 5) | b5);
}

bool is_touched() {
    gpio_put(PIN_CS, 1);
    gpio_init(PIN_T_CS);
    gpio_set_dir(PIN_T_CS, GPIO_OUT);
    gpio_put(PIN_T_CS, 0);

    uint8_t cmd = 0xB1;
    uint8_t response[2] = {0, 0};
    spi_write_blocking(SPI_PORT, &cmd, 1);

    gpio_set_function(PIN_T_CS, GPIO_FUNC_SPI);
    spi_read_blocking(SPI_PORT, 0, response, 2);

    gpio_init(PIN_T_CS);
    gpio_set_dir(PIN_T_CS, GPIO_OUT);
    gpio_put(PIN_T_CS, 1);
    gpio_set_function(PIN_T_CS, GPIO_FUNC_SPI);

    uint16_t pressure = ((response[0] << 8) | response[1]) >> 3;
    if (pressure > 50) {
        printf("[Touch Debug] Pressure: %d\n", pressure);
    }
    return (pressure > 100);
}

void update_hue_on_tap() {
    hue_offset = fmodf(hue_offset + HUE_STEP, 360.0f);
    printf("[Update] Tap! Hue rotated 120 deg. Offset: %.1f\n", hue_offset);
}

void LCD_Driver::drawPixel(uint16_t x, uint16_t y, uint16_t color) {}
void LCD_Driver::drawString(uint16_t x, uint16_t y, const char* str, uint8_t size) {}
void LCD_Driver::drawNumber(uint16_t x, uint16_t y, const char* label, int num, uint8_t size) {}