#include "lcd.h"
#include "lcd_dma.h"
#include "exmc_driver.h"
#include "systick.h"
#include <cstdio>
#include "font.h"

extern "C" uint32_t SystemCoreClock;

Lcd &Lcd::instance()
{
    static Lcd inst;
    return inst;
}

Lcd::Lcd()
    : lcddev_{0, 0, 0, 0, 0, 0, 0},
      point_color_(Color::WHITE),
      back_color_(Color::BLACK),
      id_string_{0}
{
}

Lcd::LcdRegs *Lcd::regs()
{
    return reinterpret_cast<LcdRegs *>(0x6C000000 | 0x000007FE);
}

void Lcd::rstOn() { gpio_bit_set(GPIOD, GPIO_PIN_12); }
void Lcd::rstOff() { gpio_bit_reset(GPIOD, GPIO_PIN_12); }
void Lcd::blkOn() { gpio_bit_set(GPIOD, GPIO_PIN_13); }
void Lcd::blkOff() { gpio_bit_reset(GPIOD, GPIO_PIN_13); }

uint16_t Lcd::width() const { return lcddev_.width_; }
uint16_t Lcd::height() const { return lcddev_.height_; }
uint16_t Lcd::id() const { return lcddev_.id_; }
const char *Lcd::idString() const { return id_string_; }
uint16_t Lcd::foreground() const { return point_color_; }
uint16_t Lcd::background() const { return back_color_; }
void Lcd::setForeground(uint16_t color) { point_color_ = color; }
void Lcd::setBackground(uint16_t color) { back_color_ = color; }

void Lcd::wrReg(uint16_t regval)
{
    regs()->LCD_REG = regval;
    __DSB();
}

void Lcd::wrData(uint16_t data)
{
    regs()->LCD_RAM = data;
    __DSB();
}

void Lcd::writeRam(uint16_t color)
{
    regs()->LCD_RAM = color;
    __DSB();
}

void Lcd::writeReg(uint16_t reg, uint16_t value)
{
    regs()->LCD_REG = reg;
    __DSB();
    regs()->LCD_RAM = value;
    __DSB();
}

void Lcd::writeRamPrepare()
{
    regs()->LCD_REG = lcddev_.wramcmd_;
    __DSB();
}

uint16_t Lcd::rdData()
{
    return regs()->LCD_RAM;
}

void Lcd::flush()
{
    __DSB();
    volatile uint16_t dummy = regs()->LCD_RAM;
    (void)dummy;
}

uint16_t Lcd::readReg(uint16_t reg)
{
    wrReg(reg);
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    uint16_t data = rdData();
    printf("[Lcd::readReg] 0x%04X -> 0x%04X\r\n", reg, data);
    return data;
}

void Lcd::readID()
{
    uint16_t id1, id2, id3;

    printf("[LCD ID] Start reading...\r\n");
    delay_1ms(50);
    writeReg(0x0000, 0x0001);
    delay_1ms(50);
    lcddev_.id_ = readReg(0x0000);
    printf("[LCD ID] reg0x0000: 0x%04X\r\n", lcddev_.id_);

    printf("[LCD ID] Reset LCD...\r\n");
    rstOn();
    delay_1ms(10);
    rstOff();
    delay_1ms(50);
    rstOn();
    delay_1ms(120);

    wrReg(0XDA00);
    id1 = rdData();
    printf("[LCD ID] ID1: 0x%04X\r\n", id1);

    wrReg(0XDB00);
    id2 = rdData();
    printf("[LCD ID] ID2: 0x%04X\r\n", id2);

    if (id2 == 0x8000)
        id2 = 0x80;
    else if ((id2 >> 8) == 0x80)
        id2 >>= 8;

    wrReg(0XDC00);
    id3 = rdData();
    printf("[LCD ID] ID3: 0x%04X\r\n", id3);

    lcddev_.id_ = (id2 << 8) | (id3 & 0xFF);
    printf("NT35510 LCD ID:%04X\r\n", lcddev_.id_);
    sprintf(id_string_, "LCD ID:%04X", lcddev_.id_);
}

void Lcd::ctrlLinesConfig()
{
    rcu_periph_clock_enable(RCU_GPIOD);
    gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_PULLUP, GPIO_PIN_13);
    gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_13);
    gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_12);
    gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12);
}

void Lcd::fsmcConfig()
{
    exmc_norsram_parameter_struct nor_init_struct;
    exmc_norsram_timing_parameter_struct readWriteTiming;
    exmc_norsram_timing_parameter_struct writeTiming;

    ExmcBus &exmc_bus = ExmcBus::instance();
    exmc_bus.init();
    ExmcBus::norsram_lcd_pin_init();

    readWriteTiming.asyn_access_mode = EXMC_ACCESS_MODE_A;
    readWriteTiming.syn_data_latency = EXMC_DATALAT_2_CLK;
    readWriteTiming.syn_clk_division = EXMC_SYN_CLOCK_RATIO_2_CLK;
    readWriteTiming.bus_latency = 0;
    readWriteTiming.asyn_address_holdtime = 0;
    writeTiming = readWriteTiming;

    if (SystemCoreClock > 100000000)
    {
        printf("[FSMC] 168MHz timing\r\n");
        readWriteTiming.asyn_data_setuptime = 61;
        readWriteTiming.asyn_address_setuptime = 15;
        writeTiming.asyn_data_setuptime = 5;
        writeTiming.asyn_address_setuptime = 4;
    }
    else
    {
        printf("[FSMC] 16MHz timing\r\n");
        readWriteTiming.asyn_data_setuptime = 6;
        readWriteTiming.asyn_address_setuptime = 2;
        writeTiming.asyn_data_setuptime = 1;
        writeTiming.asyn_address_setuptime = 1;
    }

    nor_init_struct.norsram_region = EXMC_BANK0_NORSRAM_REGION3;
    nor_init_struct.address_data_mux = DISABLE;
    nor_init_struct.memory_type = EXMC_MEMORY_TYPE_SRAM;
    nor_init_struct.databus_width = EXMC_NOR_DATABUS_WIDTH_16B;
    nor_init_struct.burst_mode = DISABLE;
    nor_init_struct.nwait_polarity = EXMC_NWAIT_POLARITY_LOW;
    nor_init_struct.asyn_wait = DISABLE;
    nor_init_struct.wrap_burst_mode = DISABLE;
    nor_init_struct.nwait_config = EXMC_NWAIT_CONFIG_BEFORE;
    nor_init_struct.memory_write = ENABLE;
    nor_init_struct.nwait_signal = DISABLE;
    nor_init_struct.extended_mode = ENABLE;
    nor_init_struct.write_mode = EXMC_ASYN_WRITE;
    nor_init_struct.read_write_timing = &readWriteTiming;
    nor_init_struct.write_timing = &writeTiming;
    exmc_norsram_init(&nor_init_struct);
    exmc_norsram_enable(EXMC_BANK0_NORSRAM_REGION3);
}

void Lcd::hvgaConfig()
{
    lcddev_.width_ = 480;
    lcddev_.height_ = 800;
    lcddev_.setxcmd_ = 0X2A00;
    lcddev_.setycmd_ = 0X2B00;
    lcddev_.wramcmd_ = 0X2C00;
}

void Lcd::initialCode()
{
    writeReg(0xF000, 0x55);
    writeReg(0xF001, 0xAA);
    writeReg(0xF002, 0x52);
    writeReg(0xF003, 0x08);
    writeReg(0xF004, 0x01);
    writeReg(0xB000, 0x0D);
    writeReg(0xB001, 0x0D);
    writeReg(0xB002, 0x0D);
    writeReg(0xB600, 0x34);
    writeReg(0xB601, 0x34);
    writeReg(0xB602, 0x34);
    writeReg(0xB100, 0x0D);
    writeReg(0xB101, 0x0D);
    writeReg(0xB102, 0x0D);
    writeReg(0xB700, 0x34);
    writeReg(0xB701, 0x34);
    writeReg(0xB702, 0x34);
    writeReg(0xB200, 0x00);
    writeReg(0xB201, 0x00);
    writeReg(0xB202, 0x00);
    writeReg(0xB800, 0x24);
    writeReg(0xB801, 0x24);
    writeReg(0xB802, 0x24);
    writeReg(0xBF00, 0x01);
    writeReg(0xB300, 0x0F);
    writeReg(0xB301, 0x0F);
    writeReg(0xB302, 0x0F);
    writeReg(0xB900, 0x34);
    writeReg(0xB901, 0x34);
    writeReg(0xB902, 0x34);
    writeReg(0xB500, 0x08);
    writeReg(0xB501, 0x08);
    writeReg(0xB502, 0x08);
    writeReg(0xC200, 0x03);
    writeReg(0xBA00, 0x24);
    writeReg(0xBA01, 0x24);
    writeReg(0xBA02, 0x24);
    writeReg(0xBC00, 0x00);
    writeReg(0xBC01, 0x78);
    writeReg(0xBC02, 0x00);
    writeReg(0xBD00, 0x00);
    writeReg(0xBD01, 0x78);
    writeReg(0xBD02, 0x00);
    writeReg(0xBE00, 0x00);
    writeReg(0xBE01, 0x64);
    writeReg(0xD100, 0x00);
    writeReg(0xD101, 0x33);
    writeReg(0xD102, 0x00);
    writeReg(0xD103, 0x34);
    writeReg(0xD104, 0x00);
    writeReg(0xD105, 0x3A);
    writeReg(0xD106, 0x00);
    writeReg(0xD107, 0x4A);
    writeReg(0xD108, 0x00);
    writeReg(0xD109, 0x5C);
    writeReg(0xD10A, 0x00);
    writeReg(0xD10B, 0x81);
    writeReg(0xD10C, 0x00);
    writeReg(0xD10D, 0xA6);
    writeReg(0xD10E, 0x00);
    writeReg(0xD10F, 0xE5);
    writeReg(0xD110, 0x01);
    writeReg(0xD111, 0x13);
    writeReg(0xD112, 0x01);
    writeReg(0xD113, 0x54);
    writeReg(0xD114, 0x01);
    writeReg(0xD115, 0x82);
    writeReg(0xD116, 0x01);
    writeReg(0xD117, 0xCA);
    writeReg(0xD118, 0x02);
    writeReg(0xD119, 0x00);
    writeReg(0xD11A, 0x02);
    writeReg(0xD11B, 0x01);
    writeReg(0xD11C, 0x02);
    writeReg(0xD11D, 0x34);
    writeReg(0xD11E, 0x02);
    writeReg(0xD11F, 0x67);
    writeReg(0xD120, 0x02);
    writeReg(0xD121, 0x84);
    writeReg(0xD122, 0x02);
    writeReg(0xD123, 0xA4);
    writeReg(0xD124, 0x02);
    writeReg(0xD125, 0xB7);
    writeReg(0xD126, 0x02);
    writeReg(0xD127, 0xCF);
    writeReg(0xD128, 0x02);
    writeReg(0xD129, 0xDE);
    writeReg(0xD12A, 0x02);
    writeReg(0xD12B, 0xF2);
    writeReg(0xD12C, 0x02);
    writeReg(0xD12D, 0xFE);
    writeReg(0xD12E, 0x03);
    writeReg(0xD12F, 0x10);
    writeReg(0xD130, 0x03);
    writeReg(0xD131, 0x33);
    writeReg(0xD132, 0x03);
    writeReg(0xD133, 0x6D);
    writeReg(0xD200, 0x00);
    writeReg(0xD201, 0x33);
    writeReg(0xD202, 0x00);
    writeReg(0xD203, 0x34);
    writeReg(0xD204, 0x00);
    writeReg(0xD205, 0x3A);
    writeReg(0xD206, 0x00);
    writeReg(0xD207, 0x4A);
    writeReg(0xD208, 0x00);
    writeReg(0xD209, 0x5C);
    writeReg(0xD20A, 0x00);
    writeReg(0xD20B, 0x81);
    writeReg(0xD20C, 0x00);
    writeReg(0xD20D, 0xA6);
    writeReg(0xD20E, 0x00);
    writeReg(0xD20F, 0xE5);
    writeReg(0xD210, 0x01);
    writeReg(0xD211, 0x13);
    writeReg(0xD212, 0x01);
    writeReg(0xD213, 0x54);
    writeReg(0xD214, 0x01);
    writeReg(0xD215, 0x82);
    writeReg(0xD216, 0x01);
    writeReg(0xD217, 0xCA);
    writeReg(0xD218, 0x02);
    writeReg(0xD219, 0x00);
    writeReg(0xD21A, 0x02);
    writeReg(0xD21B, 0x01);
    writeReg(0xD21C, 0x02);
    writeReg(0xD21D, 0x34);
    writeReg(0xD21E, 0x02);
    writeReg(0xD21F, 0x67);
    writeReg(0xD220, 0x02);
    writeReg(0xD221, 0x84);
    writeReg(0xD222, 0x02);
    writeReg(0xD223, 0xA4);
    writeReg(0xD224, 0x02);
    writeReg(0xD225, 0xB7);
    writeReg(0xD226, 0x02);
    writeReg(0xD227, 0xCF);
    writeReg(0xD228, 0x02);
    writeReg(0xD229, 0xDE);
    writeReg(0xD22A, 0x02);
    writeReg(0xD22B, 0xF2);
    writeReg(0xD22C, 0x02);
    writeReg(0xD22D, 0xFE);
    writeReg(0xD22E, 0x03);
    writeReg(0xD22F, 0x10);
    writeReg(0xD230, 0x03);
    writeReg(0xD231, 0x33);
    writeReg(0xD232, 0x03);
    writeReg(0xD233, 0x6D);
    writeReg(0xD300, 0x00);
    writeReg(0xD301, 0x33);
    writeReg(0xD302, 0x00);
    writeReg(0xD303, 0x34);
    writeReg(0xD304, 0x00);
    writeReg(0xD305, 0x3A);
    writeReg(0xD306, 0x00);
    writeReg(0xD307, 0x4A);
    writeReg(0xD308, 0x00);
    writeReg(0xD309, 0x5C);
    writeReg(0xD30A, 0x00);
    writeReg(0xD30B, 0x81);
    writeReg(0xD30C, 0x00);
    writeReg(0xD30D, 0xA6);
    writeReg(0xD30E, 0x00);
    writeReg(0xD30F, 0xE5);
    writeReg(0xD310, 0x01);
    writeReg(0xD311, 0x13);
    writeReg(0xD312, 0x01);
    writeReg(0xD313, 0x54);
    writeReg(0xD314, 0x01);
    writeReg(0xD315, 0x82);
    writeReg(0xD316, 0x01);
    writeReg(0xD317, 0xCA);
    writeReg(0xD318, 0x02);
    writeReg(0xD319, 0x00);
    writeReg(0xD31A, 0x02);
    writeReg(0xD31B, 0x01);
    writeReg(0xD31C, 0x02);
    writeReg(0xD31D, 0x34);
    writeReg(0xD31E, 0x02);
    writeReg(0xD31F, 0x67);
    writeReg(0xD320, 0x02);
    writeReg(0xD321, 0x84);
    writeReg(0xD322, 0x02);
    writeReg(0xD323, 0xA4);
    writeReg(0xD324, 0x02);
    writeReg(0xD325, 0xB7);
    writeReg(0xD326, 0x02);
    writeReg(0xD327, 0xCF);
    writeReg(0xD328, 0x02);
    writeReg(0xD329, 0xDE);
    writeReg(0xD32A, 0x02);
    writeReg(0xD32B, 0xF2);
    writeReg(0xD32C, 0x02);
    writeReg(0xD32D, 0xFE);
    writeReg(0xD32E, 0x03);
    writeReg(0xD32F, 0x10);
    writeReg(0xD330, 0x03);
    writeReg(0xD331, 0x33);
    writeReg(0xD332, 0x03);
    writeReg(0xD333, 0x6D);
    writeReg(0xD400, 0x00);
    writeReg(0xD401, 0x33);
    writeReg(0xD402, 0x00);
    writeReg(0xD403, 0x34);
    writeReg(0xD404, 0x00);
    writeReg(0xD405, 0x3A);
    writeReg(0xD406, 0x00);
    writeReg(0xD407, 0x4A);
    writeReg(0xD408, 0x00);
    writeReg(0xD409, 0x5C);
    writeReg(0xD40A, 0x00);
    writeReg(0xD40B, 0x81);
    writeReg(0xD40C, 0x00);
    writeReg(0xD40D, 0xA6);
    writeReg(0xD40E, 0x00);
    writeReg(0xD40F, 0xE5);
    writeReg(0xD410, 0x01);
    writeReg(0xD411, 0x13);
    writeReg(0xD412, 0x01);
    writeReg(0xD413, 0x54);
    writeReg(0xD414, 0x01);
    writeReg(0xD415, 0x82);
    writeReg(0xD416, 0x01);
    writeReg(0xD417, 0xCA);
    writeReg(0xD418, 0x02);
    writeReg(0xD419, 0x00);
    writeReg(0xD41A, 0x02);
    writeReg(0xD41B, 0x01);
    writeReg(0xD41C, 0x02);
    writeReg(0xD41D, 0x34);
    writeReg(0xD41E, 0x02);
    writeReg(0xD41F, 0x67);
    writeReg(0xD420, 0x02);
    writeReg(0xD421, 0x84);
    writeReg(0xD422, 0x02);
    writeReg(0xD423, 0xA4);
    writeReg(0xD424, 0x02);
    writeReg(0xD425, 0xB7);
    writeReg(0xD426, 0x02);
    writeReg(0xD427, 0xCF);
    writeReg(0xD428, 0x02);
    writeReg(0xD429, 0xDE);
    writeReg(0xD42A, 0x02);
    writeReg(0xD42B, 0xF2);
    writeReg(0xD42C, 0x02);
    writeReg(0xD42D, 0xFE);
    writeReg(0xD42E, 0x03);
    writeReg(0xD42F, 0x10);
    writeReg(0xD430, 0x03);
    writeReg(0xD431, 0x33);
    writeReg(0xD432, 0x03);
    writeReg(0xD433, 0x6D);
    writeReg(0xD500, 0x00);
    writeReg(0xD501, 0x33);
    writeReg(0xD502, 0x00);
    writeReg(0xD503, 0x34);
    writeReg(0xD504, 0x00);
    writeReg(0xD505, 0x3A);
    writeReg(0xD506, 0x00);
    writeReg(0xD507, 0x4A);
    writeReg(0xD508, 0x00);
    writeReg(0xD509, 0x5C);
    writeReg(0xD50A, 0x00);
    writeReg(0xD50B, 0x81);
    writeReg(0xD50C, 0x00);
    writeReg(0xD50D, 0xA6);
    writeReg(0xD50E, 0x00);
    writeReg(0xD50F, 0xE5);
    writeReg(0xD510, 0x01);
    writeReg(0xD511, 0x13);
    writeReg(0xD512, 0x01);
    writeReg(0xD513, 0x54);
    writeReg(0xD514, 0x01);
    writeReg(0xD515, 0x82);
    writeReg(0xD516, 0x01);
    writeReg(0xD517, 0xCA);
    writeReg(0xD518, 0x02);
    writeReg(0xD519, 0x00);
    writeReg(0xD51A, 0x02);
    writeReg(0xD51B, 0x01);
    writeReg(0xD51C, 0x02);
    writeReg(0xD51D, 0x34);
    writeReg(0xD51E, 0x02);
    writeReg(0xD51F, 0x67);
    writeReg(0xD520, 0x02);
    writeReg(0xD521, 0x84);
    writeReg(0xD522, 0x02);
    writeReg(0xD523, 0xA4);
    writeReg(0xD524, 0x02);
    writeReg(0xD525, 0xB7);
    writeReg(0xD526, 0x02);
    writeReg(0xD527, 0xCF);
    writeReg(0xD528, 0x02);
    writeReg(0xD529, 0xDE);
    writeReg(0xD52A, 0x02);
    writeReg(0xD52B, 0xF2);
    writeReg(0xD52C, 0x02);
    writeReg(0xD52D, 0xFE);
    writeReg(0xD52E, 0x03);
    writeReg(0xD52F, 0x10);
    writeReg(0xD530, 0x03);
    writeReg(0xD531, 0x33);
    writeReg(0xD532, 0x03);
    writeReg(0xD533, 0x6D);
    writeReg(0xD600, 0x00);
    writeReg(0xD601, 0x33);
    writeReg(0xD602, 0x00);
    writeReg(0xD603, 0x34);
    writeReg(0xD604, 0x00);
    writeReg(0xD605, 0x3A);
    writeReg(0xD606, 0x00);
    writeReg(0xD607, 0x4A);
    writeReg(0xD608, 0x00);
    writeReg(0xD609, 0x5C);
    writeReg(0xD60A, 0x00);
    writeReg(0xD60B, 0x81);
    writeReg(0xD60C, 0x00);
    writeReg(0xD60D, 0xA6);
    writeReg(0xD60E, 0x00);
    writeReg(0xD60F, 0xE5);
    writeReg(0xD610, 0x01);
    writeReg(0xD611, 0x13);
    writeReg(0xD612, 0x01);
    writeReg(0xD613, 0x54);
    writeReg(0xD614, 0x01);
    writeReg(0xD615, 0x82);
    writeReg(0xD616, 0x01);
    writeReg(0xD617, 0xCA);
    writeReg(0xD618, 0x02);
    writeReg(0xD619, 0x00);
    writeReg(0xD61A, 0x02);
    writeReg(0xD61B, 0x01);
    writeReg(0xD61C, 0x02);
    writeReg(0xD61D, 0x34);
    writeReg(0xD61E, 0x02);
    writeReg(0xD61F, 0x67);
    writeReg(0xD620, 0x02);
    writeReg(0xD621, 0x84);
    writeReg(0xD622, 0x02);
    writeReg(0xD623, 0xA4);
    writeReg(0xD624, 0x02);
    writeReg(0xD625, 0xB7);
    writeReg(0xD626, 0x02);
    writeReg(0xD627, 0xCF);
    writeReg(0xD628, 0x02);
    writeReg(0xD629, 0xDE);
    writeReg(0xD62A, 0x02);
    writeReg(0xD62B, 0xF2);
    writeReg(0xD62C, 0x02);
    writeReg(0xD62D, 0xFE);
    writeReg(0xD62E, 0x03);
    writeReg(0xD62F, 0x10);
    writeReg(0xD630, 0x03);
    writeReg(0xD631, 0x33);
    writeReg(0xD632, 0x03);
    writeReg(0xD633, 0x6D);
    writeReg(0xF000, 0x55);
    writeReg(0xF001, 0xAA);
    writeReg(0xF002, 0x52);
    writeReg(0xF003, 0x08);
    writeReg(0xF004, 0x00);
    writeReg(0xB100, 0xCC);
    writeReg(0xB101, 0x00);
    writeReg(0xB600, 0x05);
    writeReg(0xB700, 0x70);
    writeReg(0xB701, 0x70);
    writeReg(0xB800, 0x01);
    writeReg(0xB801, 0x03);
    writeReg(0xB802, 0x03);
    writeReg(0xB803, 0x03);
    writeReg(0xBC00, 0x02);
    writeReg(0xBC01, 0x00);
    writeReg(0xBC02, 0x00);
    writeReg(0xC900, 0xD0);
    writeReg(0xC901, 0x02);
    writeReg(0xC902, 0x50);
    writeReg(0xC903, 0x50);
    writeReg(0xC904, 0x50);
    writeReg(0x3500, 0x00);
    writeReg(0x3A00, 0x55);

    wrReg(0x1100);
    delay_1ms(120);
    wrReg(0x2900);
    delay_1ms(200);
}

void Lcd::blockWrite(uint16_t xStart, uint16_t xEnd, uint16_t yStart, uint16_t yEnd)
{
    __DSB();
    regs()->LCD_REG = 0x2A00;
    __DSB();
    regs()->LCD_RAM = (xStart >> 8) & 0xFF;
    __DSB();
    regs()->LCD_REG = 0x2A01;
    __DSB();
    regs()->LCD_RAM = xStart & 0xFF;
    __DSB();
    regs()->LCD_REG = 0x2A02;
    __DSB();
    regs()->LCD_RAM = (xEnd >> 8) & 0xFF;
    __DSB();
    regs()->LCD_REG = 0x2A03;
    __DSB();
    regs()->LCD_RAM = xEnd & 0xFF;
    __DSB();
    regs()->LCD_REG = 0x2B00;
    __DSB();
    regs()->LCD_RAM = (yStart >> 8) & 0xFF;
    __DSB();
    regs()->LCD_REG = 0x2B01;
    __DSB();
    regs()->LCD_RAM = yStart & 0xFF;
    __DSB();
    regs()->LCD_REG = 0x2B02;
    __DSB();
    regs()->LCD_RAM = (yEnd >> 8) & 0xFF;
    __DSB();
    regs()->LCD_REG = 0x2B03;
    __DSB();
    regs()->LCD_RAM = yEnd & 0xFF;
    __DSB();
}

void Lcd::clear(uint16_t color)
{
    uint32_t totalpoint = (uint32_t)lcddev_.width_ * lcddev_.height_;
    blockWrite(0, lcddev_.width_ - 1, 0, lcddev_.height_ - 1);
    writeRamPrepare();
    for (uint32_t i = 0; i < totalpoint; i++)
        writeRam(color);
    __DSB();
}

void Lcd::clear(Color color)
{
    clear(static_cast<uint16_t>(color));
}

void Lcd::diagnosticPattern()
{
    printf("[Diagnostic] ===== Step-by-step diagnostic =====\r\n");

    printf("\r\n[Diag 1] Read register 0x0000 (Manufacturer ID)...\r\n");
    uint16_t id = readReg(0x0000);
    printf("  [Diag 1] Result: ID=0x%04X\r\n", id);
    delay_1ms(500);

    printf("\r\n[Diag 2] Read register 0x0A00 (Display status)...\r\n");
    uint16_t disp = readReg(0x0A00);
    printf("  [Diag 2] Result: DISP_STATUS=0x%04X\r\n", disp);
    delay_1ms(500);

    printf("\r\n[Diag 3] Write a small block to top-left (0,0)-(50,50)...\r\n");
    printf("  Sending BlockWrite + 2500 pixels...\r\n");
    blockWrite(0, 50, 0, 50);
    writeRamPrepare();
    for (uint32_t i = 0; i < 51 * 51; i++)
    {
        writeRam(Color::RED);
    }
    __DSB();
    delay_1ms(1000);
    printf("  [Diag 3] Done, wait 1s...\r\n");
    delay_1ms(1000);

    printf("\r\n[Diag 4] Re-read register 0x0000 to confirm EXMC still works...\r\n");
    id = readReg(0x0000);
    printf("  [Diag 4] Result: ID=0x%04X\r\n", id);
    delay_1ms(500);

    printf("\r\n[Diag 5] Write a small block to center (200,200)-(250,250)...\r\n");
    blockWrite(200, 250, 200, 250);
    writeRamPrepare();
    for (uint32_t i = 0; i < 51 * 51; i++)
    {
        writeRam(Color::GREEN);
    }
    __DSB();
    delay_1ms(1000);
    printf("  [Diag 5] Done\r\n");

    printf("\r\n[Diag 6] Write a small block to bottom (200,750)-(250,799)...\r\n");
    blockWrite(200, 250, 750, 799);
    writeRamPrepare();
    for (uint32_t i = 0; i < 51 * 50; i++)
    {
        writeRam(Color::BLUE);
    }
    __DSB();
    delay_1ms(1000);
    printf("  [Diag 6] Done\r\n");

    printf("\r\n[Diag 7] Re-read register 0x0000...\r\n");
    id = readReg(0x0000);
    printf("  [Diag 7] Result: ID=0x%04X\r\n", id);

    printf("\r\n[Diag 8] Read register 0x0A00...\r\n");
    disp = readReg(0x0A00);
    printf("  [Diag 8] Result: DISP_STATUS=0x%04X\r\n", disp);

    printf("[Diagnostic] ===== Diagnostic complete =====\r\n");
}

void Lcd::setCursor(uint16_t x, uint16_t y)
{
    writeReg(0x2A00, x >> 8);
    writeReg(0x2A01, x & 0xFF);
    writeReg(0x2A02, (lcddev_.width_ - 1) >> 8);
    writeReg(0x2A03, (lcddev_.width_ - 1) & 0xFF);
    writeReg(0x2B00, y >> 8);
    writeReg(0x2B01, y & 0xFF);
    writeReg(0x2B02, (lcddev_.height_ - 1) >> 8);
    writeReg(0x2B03, (lcddev_.height_ - 1) & 0xFF);
}

void Lcd::setScanDir(ScanDir dir)
{
    uint16_t regval = 0;
    uint16_t dirreg = 0;

    switch (dir)
    {
    case L2R_U2D:
        regval |= (0 << 7) | (0 << 6) | (0 << 5);
        break;
    case L2R_D2U:
        regval |= (1 << 7) | (0 << 6) | (0 << 5);
        break;
    case R2L_U2D:
        regval |= (0 << 7) | (1 << 6) | (0 << 5);
        break;
    case R2L_D2U:
        regval |= (1 << 7) | (1 << 6) | (0 << 5);
        break;
    case U2D_L2R:
        regval |= (0 << 7) | (0 << 6) | (1 << 5);
        break;
    case U2D_R2L:
        regval |= (0 << 7) | (1 << 6) | (1 << 5);
        break;
    case D2U_L2R:
        regval |= (1 << 7) | (0 << 6) | (1 << 5);
        break;
    case D2U_R2L:
        regval |= (1 << 7) | (1 << 6) | (1 << 5);
        break;
    }
    regval &= ~(1 << 3);

    dirreg = 0X3600;
    writeReg(dirreg, regval);
    writeReg(0x2A00, 0);
    writeReg(0x2A01, 0);
    writeReg(0x2A02, (lcddev_.width_ - 1) >> 8);
    writeReg(0x2A03, (lcddev_.width_ - 1) & 0xFF);
    writeReg(0x2B00, 0);
    writeReg(0x2B01, 0);
    writeReg(0x2B02, (lcddev_.height_ - 1) >> 8);
    writeReg(0x2B03, (lcddev_.height_ - 1) & 0xFF);
}

RetCode Lcd::init()
{
    printf("[Lcd] init start\r\n");
    ctrlLinesConfig();
    fsmcConfig();

    rstOff();
    delay_1ms(100);
    rstOn();
    delay_1ms(100);

    lcddev_.id_ = 0x5510;
    sprintf(id_string_, "LCD ID:%04X", lcddev_.id_);
    printf("[Lcd] Skip ReadID, force ID=0x5510\r\n");

    hvgaConfig();
    initialCode();

    writeReg(0xC200, 0x01);
    delay_1ms(10);

    writeReg(0x3A00, 0x55);
    delay_1ms(10);

    wrReg(0x1300);
    delay_1ms(10);

    writeReg(0x3600, 0x0000);
    delay_1ms(10);

    setScanDir(ScanDir::DFT_SCAN_DIR);
    delay_1ms(10);
    blkOn();

    LcdDma::instance().init();
    printf("[Lcd] init done\r\n");

    return RET_OK;
}

void Lcd::drawPoint(uint16_t x, uint16_t y)
{
    setCursor(x, y);
    writeRamPrepare();
    writeRam(point_color_);
}

void Lcd::fastDrawPoint(uint16_t x, uint16_t y, uint16_t color)
{
    setCursor(x, y);
    writeRamPrepare();
    writeRam(color);
}

uint16_t Lcd::readPoint(uint16_t x, uint16_t y)
{
    uint16_t color;

    if (lcddev_.id_ == 0x5510)
    {
        wrReg(lcddev_.setxcmd_);
        wrData(x >> 8);
        wrReg(lcddev_.setxcmd_ + 1);
        wrData(x & 0xFF);
        wrReg(lcddev_.setycmd_);
        wrData(y >> 8);
        wrReg(lcddev_.setycmd_ + 1);
        wrData(y & 0xFF);
    }
    else
    {
        setCursor(x, y);
    }

    wrReg(0x2E00);
    color = rdData();
    return color;
}

void Lcd::showChar(uint16_t x, uint16_t y, uint8_t num, uint8_t size, uint8_t mode)
{
    uint8_t temp, t1, t;
    uint16_t y0 = y;
    uint8_t csize = (size / 8 + ((size % 8) ? 1 : 0)) * (size / 2);

    num = num - ' ';
    if (!mode)
    {
        for (t = 0; t < csize; t++)
        {
            if (size == 12)
                temp = asc2_1206[num][t];
            else
                temp = asc2_1608[num][t];

            for (t1 = 0; t1 < 8; t1++)
            {
                if (temp & 0x80)
                    fastDrawPoint(x, y, point_color_);
                else
                    fastDrawPoint(x, y, back_color_);
                temp <<= 1;
                y++;

                if (y >= lcddev_.height_)
                    return;

                if ((y - y0) == size)
                {
                    y = y0;
                    x++;
                    break;
                }
            }
        }
    }
    else
    {
        for (t = 0; t < csize; t++)
        {
            if (size == 12)
                temp = asc2_1206[num][t];
            else
                temp = asc2_1608[num][t];

            for (t1 = 0; t1 < 8; t1++)
            {
                if (temp & 0x80)
                    fastDrawPoint(x, y, point_color_);
                temp <<= 1;
                y++;

                if (y >= lcddev_.height_)
                    return;

                if ((y - y0) == size)
                {
                    y = y0;
                    x++;
                    break;
                }
            }
        }
    }
}

void Lcd::showString(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint8_t size, uint8_t mode, uint8_t *p)
{
    while (*p != '\0')
    {
        if (x + size / 2 > lcddev_.width_)
        {
            x = 0;
            y += size;
        }

        if (y + size > lcddev_.height_)
            return;

        if (*p == '\n')
        {
            x = 0;
            y += size;
            p++;
            continue;
        }

        showChar(x, y, *p, size, mode);
        x += size / 2;
        p++;
    }
}

void Lcd::drawLine(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
{
    uint16_t t;
    int xerr = 0, yerr = 0, delta_x, delta_y, distance;
    int incx, incy;

    delta_x = x2 - x1;
    delta_y = y2 - y1;

    if (delta_x > 0)
        incx = 1;
    else if (delta_x == 0)
        incx = 0;
    else
    {
        incx = -1;
        delta_x = -delta_x;
    }

    if (delta_y > 0)
        incy = 1;
    else if (delta_y == 0)
        incy = 0;
    else
    {
        incy = -1;
        delta_y = -delta_y;
    }

    if (delta_x > delta_y)
        distance = delta_x;
    else
        distance = delta_y;

    for (t = 0; t <= distance; t++)
    {
        fastDrawPoint(x1, y1, point_color_);
        xerr += delta_x;
        yerr += delta_y;

        if (xerr > distance)
        {
            xerr -= distance;
            x1 += incx;
        }

        if (yerr > distance)
        {
            yerr -= distance;
            y1 += incy;
        }
    }
}

void Lcd::drawRectangle(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
{
    drawLine(x1, y1, x2, y1);
    drawLine(x1, y1, x1, y2);
    drawLine(x1, y2, x2, y2);
    drawLine(x2, y1, x2, y2);
}

void Lcd::fillRectangle(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
{
    uint32_t i, total;

    if (x1 > x2)
    {
        uint16_t t = x1;
        x1 = x2;
        x2 = t;
    }
    if (y1 > y2)
    {
        uint16_t t = y1;
        y1 = y2;
        y2 = t;
    }

    total = (uint32_t)(x2 - x1 + 1) * (y2 - y1 + 1);
    blockWrite(x1, x2, y1, y2);
    writeRamPrepare();
    for (i = 0; i < total; i++)
        writeRam(point_color_);
}

void Lcd::fill(uint16_t xStart, uint16_t yStart, uint16_t xEnd, uint16_t yEnd, uint16_t color)
{
    uint32_t i, total;

    if (xStart > xEnd)
    {
        uint16_t t = xStart;
        xStart = xEnd;
        xEnd = t;
    }
    if (yStart > yEnd)
    {
        uint16_t t = yStart;
        yStart = yEnd;
        yEnd = t;
    }

    total = (uint32_t)(xEnd - xStart + 1) * (yEnd - yStart + 1);
    blockWrite(xStart, xEnd, yStart, yEnd);
    writeRamPrepare();
    for (i = 0; i < total; i++)
        writeRam(color);
    __DSB();
}

void Lcd::clearDMA(uint16_t color)
{
    LcdDma::instance().fillBuffer(color);
    uint32_t total = (uint32_t)lcddev_.width_ * lcddev_.height_;

    blockWrite(0, lcddev_.width_ - 1, 0, lcddev_.height_ - 1);
    writeRamPrepare();
    flushFromSdram(total);
}

void Lcd::clearDMA(Color color)
{
    clearDMA(static_cast<uint16_t>(color));
}

void Lcd::fillDMA(uint16_t xStart, uint16_t yStart, uint16_t xEnd, uint16_t yEnd, uint16_t color)
{
    if (xStart > xEnd)
    {
        uint16_t t = xStart;
        xStart = xEnd;
        xEnd = t;
    }
    if (yStart > yEnd)
    {
        uint16_t t = yStart;
        yStart = yEnd;
        yEnd = t;
    }

    uint32_t pixelCount = (uint32_t)(xEnd - xStart + 1) * (yEnd - yStart + 1);

    LcdDma::instance().fillBuffer(color);
    blockWrite(xStart, xEnd, yStart, yEnd);
    writeRamPrepare();
    flushFromSdram(pixelCount);
}

void Lcd::flushFromSdram(uint32_t pixelCount)
{
    uint16_t *src = LcdDma::instance().getBuffer();
    uint32_t count = pixelCount;
    while (count--) {
        regs()->LCD_RAM = *src++;
        if ((count & 0x0F) == 0) {
            __DSB();
        }
    }
    __DSB();
}

void Lcd::showNum(uint16_t x, uint16_t y, uint32_t num, uint8_t len, uint8_t size)
{
    uint8_t buf[12];
    uint8_t i;

    for (i = 0; i < len; i++)
    {
        buf[i] = '0' + (num % 10);
        num /= 10;
    }
    for (i = 0; i < len; i++)
        showChar(x + (len - 1 - i) * (size / 2), y, buf[i], size, 0);
}

void Lcd::showIntNum(uint16_t x, uint16_t y, int32_t num, uint8_t len, uint8_t size)
{
    uint8_t buf[14];
    uint8_t i, neg = 0;

    if (num < 0)
    {
        neg = 1;
        num = -num;
    }

    for (i = 0; i < len; i++)
    {
        buf[i] = '0' + (num % 10);
        num /= 10;
    }
    if (neg)
    {
        for (i = 0; i < len; i++)
            showChar(x + (len - i) * (size / 2), y, buf[i], size, 0);
        showChar(x, y, '-', size, 0);
    }
    else
    {
        for (i = 0; i < len; i++)
            showChar(x + (len - 1 - i) * (size / 2), y, buf[i], size, 0);
    }
}

void Lcd::showFloatNum(uint16_t x, uint16_t y, float num, uint8_t intLen, uint8_t decLen, uint8_t size)
{
    uint16_t x_offset = 0;
    int32_t intPart = (int32_t)num;
    uint32_t decPart;
    uint8_t i;

    if (num < 0)
    {
        showChar(x, y, '-', size, 0);
        x_offset = size / 2;
        intPart = -intPart;
    }

    showIntNum(x + x_offset, y, intPart, intLen, size);
    x_offset += intLen * (size / 2);

    showChar(x + x_offset, y, '.', size, 0);
    x_offset += size / 2;

    if (num < 0)
        num = -num;
    decPart = (uint32_t)((num - (int32_t)num) * 1000000);

    for (i = 0; i < decLen; i++)
        decPart /= 10;

    showNum(x + x_offset, y, decPart, decLen, size);
}

void Lcd::drawCircle(uint16_t x0, uint16_t y0, uint16_t r)
{
    int x = 0, y = r;
    int d = 3 - 2 * r;

    while (x <= y)
    {
        fastDrawPoint(x0 + x, y0 + y, point_color_);
        fastDrawPoint(x0 - x, y0 + y, point_color_);
        fastDrawPoint(x0 + x, y0 - y, point_color_);
        fastDrawPoint(x0 - x, y0 - y, point_color_);
        fastDrawPoint(x0 + y, y0 + x, point_color_);
        fastDrawPoint(x0 - y, y0 + x, point_color_);
        fastDrawPoint(x0 + y, y0 - x, point_color_);
        fastDrawPoint(x0 - y, y0 - x, point_color_);

        if (d < 0)
            d = d + 4 * x + 6;
        else
        {
            d = d + 4 * (x - y) + 10;
            y--;
        }
        x++;
    }
}

void Lcd::fillCircle(uint16_t x0, uint16_t y0, uint16_t r)
{
    int x = 0, y = r;
    int d = 3 - 2 * r;

    while (x <= y)
    {
        drawLine(x0 - x, y0 + y, x0 + x, y0 + y);
        drawLine(x0 - y, y0 + x, x0 + y, y0 + x);
        drawLine(x0 - x, y0 - y, x0 + x, y0 - y);
        drawLine(x0 - y, y0 - x, x0 + y, y0 - x);

        if (d < 0)
            d = d + 4 * x + 6;
        else
        {
            d = d + 4 * (x - y) + 10;
            y--;
        }
        x++;
    }
}

void Lcd::showImage(uint16_t x, uint16_t y, uint16_t width, uint16_t height, const uint8_t *p)
{
    uint32_t i, j;
    uint16_t color;
    const uint16_t *img = (const uint16_t *)p;

    if (x + width > lcddev_.width_ || y + height > lcddev_.height_)
        return;

    blockWrite(x, x + width - 1, y, y + height - 1);
    writeRamPrepare();

    for (i = 0; i < height; i++)
    {
        for (j = 0; j < width; j++)
        {
            color = *img++;
            writeRam(color);
        }
    }
}

void Lcd::showSmallImage(uint16_t x, uint16_t y, uint16_t width, uint16_t height, const uint8_t *p)
{
    uint32_t i, j;
    uint16_t color;
    uint8_t r, g, b;

    if (x + width > lcddev_.width_ || y + height > lcddev_.height_)
        return;

    blockWrite(x, x + width - 1, y, y + height - 1);
    writeRamPrepare();

    for (i = 0; i < height; i++)
    {
        for (j = 0; j < width; j++)
        {
            uint8_t pixel = *p++;
            r = (pixel >> 5) & 0x07;
            g = (pixel >> 2) & 0x07;
            b = pixel & 0x03;

            color = ((r << 13) & 0xF800) | ((g << 8) & 0x07E0) | ((b << 3) & 0x001F);
            writeRam(color);
        }
    }
}

void Lcd::displayOn()
{
    wrReg(0x2900);
    delay_1ms(50);
}

void Lcd::displayOff()
{
    wrReg(0x2800);
    delay_1ms(10);
}

void Lcd::backlight(uint8_t brightness)
{
    if (brightness)
        blkOn();
    else
        blkOff();
}