// --- Pin Definitions for ESP8266 NodeMCU ---
// 74HC595 Control Pins
const int SER_PIN   = 14; // ESP8266 GPIO14 (NodeMCU D5)
const int SRCLK_PIN = 12; // ESP8266 GPIO12 (NodeMCU D6)
const int RCLK_PIN  = 13; // ESP8266 GPIO13 (NodeMCU D7)

// LCD Control Pins (Directly connected to ESP8266, will go through Level Shifter IC2)
const int LCD_CS_PIN    = 16; // ESP8266 GPIO16 (NodeMCU D0)
const int LCD_RS_PIN    = 5;  // ESP8266 GPIO5  (NodeMCU D1)
const int LCD_RESET_PIN = 4;  // ESP8266 GPIO4  (NodeMCU D2)
const int LCD_WR_PIN    = 0;  // ESP8266 GPIO0  (NodeMCU D3)
const int LCD_RD_PIN    = 2;  // ESP8266 GPIO2  (NodeMCU D4) - For STROBING read only

// --- HD66766 Register Definitions ---
#define REG_OSC_START           0x00 // Start Oscillation
#define REG_DRIVER_OUTPUT_CTRL  0x01 // Driver Output Control
#define REG_AC_DRIVE_CTRL       0x02 // LCD-Driving-Waveform Control
#define REG_POWER_CTRL_1        0x03 // Power Control 1 (BS, BT, DC, AP, SLP)
#define REG_CONTRAST_CTRL       0x04 // Contrast Control (VR, CT)
#define REG_ENTRY_MODE          0x05 // Entry Mode (AM, I/D, SPR, HWM, LG) -> MADCTL equivalent
#define REG_DISPLAY_CTRL        0x07 // Display Control (D, REV, B/W, SPT, VLE)
#define REG_FRAME_CYCLE_CTRL    0x0B // Frame Cycle Control (DIV, RTN)
#define REG_POWER_CTRL_2        0x0C // Power Control 2 (VC)
#define REG_H_RAM_ADDR_POS      0x16 // Horizontal RAM Address Position (Window X: HEA, HSA)
#define REG_V_RAM_ADDR_POS      0x17 // Vertical RAM Address Position (Window Y: VEA, VSA)
#define REG_RAM_WRITE_MASK      0x20 // RAM Write Data Mask (WM)
#define REG_RAM_ADDR_SET        0x21 // RAM Address Set (AC pointer)
#define REG_GRAM_DATA_RW        0x22 // Write/Read Data to/from GRAM

// --- RGB565 Color Definitions ---
const uint16_t COLOR_BLACK   = 0x0000;
const uint16_t COLOR_BLUE    = 0x001F;
const uint16_t COLOR_RED     = 0xF800;
const uint16_t COLOR_GREEN   = 0x07E0;
const uint16_t COLOR_CYAN    = 0x07FF;
const uint16_t COLOR_MAGENTA = 0xF81F;
const uint16_t COLOR_YELLOW  = 0xFFE0;
const uint16_t COLOR_WHITE   = 0xFFFF;

// Helper function to convert 24-bit RGB to 16-bit RGB565
uint16_t colorToRGB565(uint8_t r, uint8_t g, uint8_t b) {
  return ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
}

// Helper function to put data on 74HC595 outputs
void shift_out_to_lcd_data_bus(uint8_t dataByte) {
  digitalWrite(RCLK_PIN, LOW); 
  for (int i = 7; i >= 0; i--) {
    digitalWrite(SER_PIN, (dataByte >> i) & 0x01);
    digitalWrite(SRCLK_PIN, LOW);
    digitalWrite(SRCLK_PIN, HIGH);
  }
  digitalWrite(RCLK_PIN, HIGH); 
  delayMicroseconds(2); // Ensure data is latched and settled on 595 outputs
}

// Sends one raw byte with WR pulse (assumes /CS and RS are already set)
void hd66766_send_one_byte_raw(uint8_t byte_val) {
  shift_out_to_lcd_data_bus(byte_val); 
  delayMicroseconds(2); // Let data settle through TXS0108E to LCD inputs

  digitalWrite(LCD_WR_PIN, LOW);
  delayMicroseconds(2); // WR low pulse width
  digitalWrite(LCD_WR_PIN, HIGH); 
  delayMicroseconds(2); // WR recovery time
}

// --- HD66766 16-bit Operations (Corrected /CS Handling) ---
void hd66766_write_to_ir(uint16_t index_val) {
  digitalWrite(LCD_CS_PIN, LOW);    
  digitalWrite(LCD_RS_PIN, LOW);    

  hd66766_send_one_byte_raw((index_val >> 8) & 0xFF); 
  hd66766_send_one_byte_raw(index_val & 0xFF);        

  digitalWrite(LCD_CS_PIN, HIGH);   
  delayMicroseconds(2); // Recovery time after /CS high
}

void hd66766_write_to_wdr(uint16_t data_val) {
  digitalWrite(LCD_CS_PIN, LOW);    
  digitalWrite(LCD_RS_PIN, HIGH);   

  hd66766_send_one_byte_raw((data_val >> 8) & 0xFF); 
  hd66766_send_one_byte_raw(data_val & 0xFF);        

  digitalWrite(LCD_CS_PIN, HIGH);   
  delayMicroseconds(2); // Recovery time
}

// Synchronization
void hd66766_synchronize_transfer() {
  for (int i = 0; i < 4; i++) {
    hd66766_write_to_ir(0x0000); // This will use the corrected /CS logic
  }
}

// LCD Initialization
void lcd_reset() {
  digitalWrite(LCD_RESET_PIN, HIGH); 
  delayMicroseconds(100); 
  digitalWrite(LCD_RESET_PIN, LOW);
  delayMicroseconds(200); // Recommendation from Tech. Document HD66766
  digitalWrite(LCD_RESET_PIN, HIGH);
  delay(15); // Oscillator stabilization (>10ms)
}

void lcd_init() {
  Serial.println("Starting LCD Init...");
  lcd_reset();

  Serial.println("Init: Power On Initialized Bits (Step 5 - R03h, R04h, R0Ch)");
  hd66766_write_to_ir(REG_POWER_CTRL_1); // R03h
  hd66766_write_to_wdr(0x9000); // BS=1/12 (0x9), BT3=0, AP=off

  hd66766_write_to_ir(REG_CONTRAST_CTRL); // R04h
  hd66766_write_to_wdr(0x0040); // CT=mid_range (64) - STARTING POINT FOR EXPERIMENTATION
                                // Have to Try 0x0000, 0x0020, 0x0040, 0x0060, 0x007F

  hd66766_write_to_ir(REG_POWER_CTRL_2); // R0Ch
  hd66766_write_to_wdr(0x0000); // VC=0.92xVcc

  delay(60); // Step 6: Wait >50ms

  Serial.println("Init: Start Voltage Inverter (Step 7 - R03h)");
  hd66766_write_to_ir(REG_POWER_CTRL_1); // R03h
  hd66766_write_to_wdr(0x9800); // BT3=1 (inverter on), BS=0x9

  delay(220); // Step 8: Wait >200ms

  Serial.println("Init: Other Mode Settings (Step 9)");
  hd66766_write_to_ir(REG_POWER_CTRL_1); // R03h
  hd66766_write_to_wdr(0x9804); // AP=Small current, BT3=1, BS=0x9

  hd66766_write_to_ir(REG_DRIVER_OUTPUT_CTRL); // R01h
  hd66766_write_to_wdr(0x0013); // NL=160 lines (0x13), CMS=0, SGS=0

  hd66766_write_to_ir(REG_AC_DRIVE_CTRL); // R02h
  hd66766_write_to_wdr(0x0000); 

  hd66766_write_to_ir(REG_ENTRY_MODE); // R05h
  hd66766_write_to_wdr(0x0030); // SPR=0 (direct RGB565), HWM=0, I/D=inc (11), AM=horizontal (0), LG=replace (000)

  // Set GRAM Window to 120x160
  hd66766_write_to_ir(REG_H_RAM_ADDR_POS); // R16h
  hd66766_write_to_wdr( ((uint16_t)(120-1) << 8) | 0x00 ); // EndX=119 (0x77), StartX=0

  hd66766_write_to_ir(REG_V_RAM_ADDR_POS); // R17h
  hd66766_write_to_wdr( ((uint16_t)(160-1) << 8) | 0x00 ); // EndY=159 (0x9F), StartY=0

  hd66766_write_to_ir(REG_FRAME_CYCLE_CTRL); // R0Bh
  hd66766_write_to_wdr(0x0000); 

  hd66766_write_to_ir(REG_RAM_WRITE_MASK); // R20h
  hd66766_write_to_wdr(0x0000); // No mask

  hd66766_write_to_ir(REG_RAM_ADDR_SET); // R21h
  hd66766_write_to_wdr(0x0000); // Set AC to (0,0) within window

  Serial.println("Init: Display ON (Step 10 - R07h)");
  hd66766_write_to_ir(REG_DISPLAY_CTRL); // R07h
  hd66766_write_to_wdr(0x0003);

  hd66766_synchronize_transfer(); 
  Serial.println("LCD Init sequence completed.");
}

// Basic Drawing Functions
void lcd_set_window(uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1) {
  hd66766_write_to_ir(REG_H_RAM_ADDR_POS); 
  hd66766_write_to_wdr( ((uint16_t)x1 << 8) | x0 );
  hd66766_write_to_ir(REG_V_RAM_ADDR_POS); 
  hd66766_write_to_wdr( ((uint16_t)y1 << 8) | y0 );
  hd66766_write_to_ir(REG_RAM_ADDR_SET); 
  hd66766_write_to_wdr(0x0000); 
}

void lcd_clear_screen(uint16_t color) {
  lcd_set_window(0, 0, 120 - 1, 160 - 1);
  hd66766_write_to_ir(REG_GRAM_DATA_RW); 
  for (long i = 0; i < 120L * 160L; i++) {
    hd66766_write_to_wdr(color);
    if ((i & 0x7F) == 0) { 
      yield();
    }
  }
  yield(); 
}

void lcd_draw_pixel(uint8_t x, uint8_t y, uint16_t color) {
  if (x >= 120 || y >= 160) return;
  lcd_set_window(x, y, x, y); 
  hd66766_write_to_ir(REG_GRAM_DATA_RW); 
  hd66766_write_to_wdr(color);
}

void setup() {
  Serial.begin(115200);
  while (!Serial); 
  Serial.println("\nESP8266 HD66766 LCD Test Program - Corrected CS/Timings");

  pinMode(SER_PIN, OUTPUT);
  pinMode(SRCLK_PIN, OUTPUT);
  pinMode(RCLK_PIN, OUTPUT);

  pinMode(LCD_CS_PIN, OUTPUT);
  pinMode(LCD_RS_PIN, OUTPUT);
  pinMode(LCD_WR_PIN, OUTPUT);
  pinMode(LCD_RD_PIN, OUTPUT); 
  pinMode(LCD_RESET_PIN, OUTPUT);

  digitalWrite(LCD_CS_PIN, HIGH);    
  digitalWrite(LCD_WR_PIN, HIGH);   
  digitalWrite(LCD_RD_PIN, HIGH);   
  digitalWrite(LCD_RESET_PIN, HIGH); 

  lcd_init();

  Serial.println("Drawing test patterns...");
  lcd_clear_screen(COLOR_BLACK);
  delay(200); 

  lcd_clear_screen(COLOR_RED);
  delay(200);

  lcd_clear_screen(COLOR_GREEN);
  delay(200);

  lcd_clear_screen(COLOR_BLUE);
  delay(200);

  lcd_clear_screen(COLOR_BLACK);
  for(int i=0; i<120; i+=4){
    for(int j=0; j<160; j+=4){
        if ((i/4 + j/4) % 2 == 0) {
            lcd_draw_pixel(i, j, COLOR_WHITE);
        }
        if ((j & 0x0F) == 0) yield(); 
    }
    yield(); 
  }
  delay(500); 

  for(int y=0; y<160; y++){
    lcd_draw_pixel(0, y, COLOR_YELLOW);
    lcd_draw_pixel(119, y, COLOR_YELLOW);
    if ((y & 0x0F) == 0) yield(); 
  }
  for(int x=0; x<120; x++){
    lcd_draw_pixel(x, 0, COLOR_CYAN);
    lcd_draw_pixel(x, 159, COLOR_CYAN);
    if ((x & 0x0F) == 0) yield(); 
  }
  Serial.println("Test patterns complete.");
}

void loop() {
  delay(1000); 
}