#include <Arduino.h>  // Standard Arduino header
#include <SPI.h>
#include <HardwareSerial.h>

// Define the serial port using PF0 (TX) and PF1 (RX)
HardwareSerial Serial2(PF0, PF1);


// Pin definitions for the LCD
#define LCD_BL_PWR  PA9
#define LCD_VDD_PWR PF_3  // PY32Core! PF_3 IS RIGHT!
#define LCD_PIN_RESET    PB4    
#define LCD_PIN_SCK    PB3    
#define LCD_PIN_DATA     PB5    
#define LCD_PIN_CS   PA8    
#define LCD_PIN_CMD_DATA PA12   
#define LCD_PIN_BACKLIGHT PA11  
#define LCD_WIDTH   128
#define LCD_HEIGHT  160
// Some RGB565 colors
#define LCD_RGB565_BLACK    0x0000
#define LCD_RGB565_WHITE    0xFFFF
#define LCD_RGB565_RED      0xF800
#define LCD_RGB565_GREEN    0x07E0
#define LCD_RGB565_BLUE     0x001F

void setup() {
  // Enable the LSI (Low-Speed Internal) oscillator used by the IWDG
  RCC->CSR |= RCC_CSR_LSION;  // Enable LSI oscillator
  // Wait for the LSI to become ready
  while (!(RCC->CSR & RCC_CSR_LSIRDY)) {
    // Wait here until LSI is stable
  }
  // Unlock the IWDG registers
  IWDG->KR = 0x5555;  // Writing 0x5555 unlocks the control registers of IWDG
  // Set the prescaler (divides the LSI clock to determine the watchdog timeout)
  IWDG->PR = 0x06;  // Set prescaler to divide by 64 (prescaler values range from 0-7)
  // Set the reload value (this sets the timeout period)
  IWDG->RLR = 0x0FFF;  // Maximum reload value for the longest timeout
  // Start the watchdog timer
  IWDG->KR = 0xCCCC;  // Writing 0xCCCC starts the IWDG
  
    Serial2.begin(115200);
    Serial2.println("Initializing LCD...");
    
    pinMode(LCD_VDD_PWR, OUTPUT);
    pinMode(LCD_BL_PWR, OUTPUT);
    pinMode(LCD_PIN_BACKLIGHT, OUTPUT);
    pinMode(LCD_PIN_RESET, OUTPUT);
    pinMode(LCD_PIN_CS, OUTPUT);
    pinMode(LCD_PIN_CMD_DATA, OUTPUT);
    
    digitalWrite(LCD_VDD_PWR, LOW);
    digitalWrite(LCD_BL_PWR, HIGH);
    digitalWrite(LCD_PIN_BACKLIGHT, HIGH);
    digitalWrite(LCD_PIN_CS, HIGH);
    digitalWrite(LCD_PIN_RESET, LOW);
    delay(100);
    digitalWrite(LCD_PIN_RESET, HIGH);
    delay(100);

    // Configure SPI for LCD
    SPI.setSCLK(LCD_PIN_SCK); 
    SPI.setMOSI(LCD_PIN_DATA);
    SPI.begin();
    SPI.beginTransaction(SPISettings(400000, MSBFIRST, SPI_MODE0));
    
    lcdInit(0); 
    digitalWrite(LCD_PIN_BACKLIGHT, HIGH);
}

void loop() {
    refreshIWDG();
    
    digitalWrite(LCD_PIN_BACKLIGHT, HIGH);
    digitalWrite(LCD_VDD_PWR, LOW);
    digitalWrite(LCD_BL_PWR, HIGH);
    // LCD_transfer_test();
     //
     lcdFillScreen(LCD_RGB565_RED);  
    delay(500);
}

void lcdFillScreen(uint16_t color) {
       digitalWrite(LCD_PIN_BACKLIGHT, HIGH);
     // Display ON command
    lcdCommand(0x29);

    // Memory Access Control
    lcdCommand(0x36);
    lcdData(0x00);

    // Memory Write
    lcdCommand(0x2C);
    for (uint32_t i = 0; i < LCD_WIDTH * LCD_HEIGHT; i++) {
        SPI.transfer(color << 8);
        SPI.transfer(color & 0xFF);
    }
}

void lcdInit(char v) {
  if(v==1){
    Serial2.println("Variante 1");
    lcdCommand(0xFF);
    lcdData(0xA5);   
    delay(150);
    lcdCommand(0xFF);
    lcdData(0xA5);
    delay(120);
    lcdCommand(0xFF);
    lcdData(0xA5);
    lcdCommand(0x3E);
    lcdData(0x08);
    lcdCommand(0x3A);
    lcdData(0x65);
    lcdCommand(0x82);
    lcdData(0x00);
    lcdCommand(0x98);
    lcdData(0x00);
    lcdCommand(0x63);
    lcdData(0x0F);
    lcdCommand(0x64);
    lcdData(0x0F);
    lcdCommand(0xB4);
    lcdData(0x34);
    lcdCommand(0xB5);
    lcdData(0x30);
    lcdCommand(0x83);
    lcdData(0x13);
    lcdCommand(0x86);
    lcdData(0x04);
    lcdCommand(0x87);
    lcdData(0x16);
    lcdCommand(0x88);
    lcdData(0x28);
    lcdCommand(0x89);
    lcdData(0x2F);
    lcdCommand(0x93);
    lcdData(0x63);
    lcdCommand(0x96);
    lcdData(0x81);
    lcdCommand(0xC3);
    lcdData(0x11);
    lcdCommand(0xE6);
    lcdData(0x00);
    lcdCommand(0x99);
    lcdData(0x01);
    lcdCommand(0x44);
    lcdData(0x00);
    lcdCommand(0x70);
    lcdData(0x07);
    lcdCommand(0x71);
    lcdData(0x19);
    lcdCommand(0x72);
    lcdData(0x1A);
    lcdCommand(0x73);
    lcdData(0x13);
    lcdCommand(0x74);
    lcdData(0x19);
    lcdCommand(0x75);
    lcdData(0x1D);
    lcdCommand(0x76);
    lcdData(0x47);
    lcdCommand(0x77);
    lcdData(0x0A);
    lcdCommand(0x78);
    lcdData(0x07);
    lcdCommand(0x79);
    lcdData(0x47);
    lcdCommand(0x7A);
    lcdData(0x05);
    lcdCommand(0x7B);
    lcdData(0x09);
    lcdCommand(0x7C);
    lcdData(0x0D);
    lcdCommand(0x7D);
    lcdData(0x0C);
    lcdCommand(0x7E);
    lcdData(0x0C);
    lcdCommand(0x7F);
    lcdData(0x08);
    lcdCommand(0xA0);
    lcdData(0x0B);
    lcdCommand(0xA1);
    lcdData(0x36);
    lcdCommand(0xA2);
    lcdData(0x09);
    lcdCommand(0xA3);
    lcdData(0x0D);
    lcdCommand(0xA4);
    lcdData(0x08);
    lcdCommand(0xA5);
    lcdData(0x23);
    lcdCommand(0xA6);
    lcdData(0x3B);
    lcdCommand(0xA7);
    lcdData(0x04);
    lcdCommand(0xA8);
    lcdData(0x07);
    lcdCommand(0xA9);
    lcdData(0x38);
    lcdCommand(0xAA);
    lcdData(0x0A);
    lcdCommand(0xAB);
    lcdData(0x12);
    lcdCommand(0xAC);
    lcdData(0x0C);
    lcdCommand(0xAD);
    lcdData(0x07);
    lcdCommand(0xAE);
    lcdData(0x2F);
    lcdCommand(0xAF);
    lcdData(0x07);
    lcdCommand(0xFF);
    lcdData(0x00);
    lcdCommand(0x36);
    lcdData(0x40);
    lcdCommand(0x11); 
  }else {
    Serial2.println("Variante 2");
    lcdCommand(0xFF);
    lcdData(0xA5);
    delay(150);
    lcdCommand(0x3E);
    lcdData(0x08);
    delay(120);
    lcdCommand(0x3A);
    lcdData(0x65);
    lcdCommand(0x82);
    lcdData(0x00);
    lcdCommand(0x98);
    lcdData(0x00);
    lcdCommand(0x63);
    lcdData(0x0F);
    lcdCommand(0x64);
    lcdData(0x0F);
    lcdCommand(0xB4);
    lcdData(0x34);
    lcdCommand(0xB5);
    lcdData(0x30);
    lcdCommand(0x83);
    lcdData(0x13);
    lcdCommand(0x86);
    lcdData(0x04);
    lcdCommand(0x87);
    lcdData(0x12);
    lcdCommand(0x88);
    lcdData(0x09);
    lcdCommand(0x89);
    lcdData(0x2F);
    lcdCommand(0x93);
    lcdData(0x63);
    lcdCommand(0x96);
    lcdData(0x81);
    lcdCommand(0xC3);
    lcdData(0x11);
    lcdCommand(0xE6);
    lcdData(0x00);
    lcdCommand(0x99);
    lcdData(0x01);
    lcdCommand(0x44);
    lcdData(0x00);
    lcdCommand(0x70);
    lcdData(0x07);
    lcdCommand(0x71);
    lcdData(0x1B);
    lcdCommand(0x72);
    lcdData(0x08);
    lcdCommand(0x73);
    lcdData(0x0F);
    lcdCommand(0x74);
    lcdData(0x16);
    lcdCommand(0x75);
    lcdData(0x1A);
    lcdCommand(0x76);
    lcdData(0x3C);
    lcdCommand(0x77);
    lcdData(0x0A);
    lcdCommand(0x78);
    lcdData(0x05);
    lcdCommand(0x79);
    lcdData(0x3A);
    lcdCommand(0x7A);
    lcdData(0x06);
    lcdCommand(0x7B);
    lcdData(0x0B);
    lcdCommand(0x7C);
    lcdData(0x12);
    lcdCommand(0x7D);
    lcdData(0x0B);
    lcdCommand(0x7E);
    lcdData(0x0A);
    lcdCommand(0x7F);
    lcdData(0x08);
    lcdCommand(0xA0);
    lcdData(0x0B);
    lcdCommand(0xA1);
    lcdData(0x36);
    lcdCommand(0xA2);
    lcdData(0x0B);
    lcdCommand(0xA3);
    lcdData(0x0C);
    lcdCommand(0xA4);
    lcdData(0x08);
    lcdCommand(0xA5);
    lcdData(0x22);
    lcdCommand(0xA6);
    lcdData(0x43);
    lcdCommand(0xA7);
    lcdData(0x04);
    lcdCommand(0xA8);
    lcdData(0x05);
    lcdCommand(0xA9);
    lcdData(0x3F);
    lcdCommand(0xAA);
    lcdData(0x0A);
    lcdCommand(0xAB);
    lcdData(0x11);
    lcdCommand(0xAC);
    lcdData(0x0D);
    lcdCommand(0xAD);
    lcdData(0x06);
    lcdCommand(0xAE);
    lcdData(0x3B);
    lcdCommand(0xAF);
    lcdData(0x07);
    lcdCommand(0xFF);
    lcdData(0x00);
    lcdCommand(0x36);
    lcdData(0x40);
    lcdCommand(0x11);
  }
}

void lcdCommand(uint8_t command) {
    digitalWrite(LCD_PIN_CS, LOW);
    digitalWrite(LCD_PIN_CMD_DATA, LOW);  // Command mode
    SPI.transfer(command);
    digitalWrite(LCD_PIN_CS, HIGH);
}

void lcdData(uint8_t data) {
    digitalWrite(LCD_PIN_CS, LOW);
    digitalWrite(LCD_PIN_CMD_DATA, HIGH);  // Data mode
    SPI.transfer(data);
    digitalWrite(LCD_PIN_CS, HIGH);
}

void LCD_transfer_test() {
    uint8_t col_start = 0, col_end = LCD_WIDTH - 1;
    uint8_t row_start = 0, row_end = LCD_HEIGHT - 1;

    // Turn on the backlight
    digitalWrite(LCD_PIN_BACKLIGHT, HIGH);

    // Display ON command
    lcdCommand(0x29);

    // Memory Access Control
    lcdCommand(0x36);
    lcdData(0x00);

    // Column Address Set
    lcdCommand(0x2A);
    lcdData(0x00);  // Start high byte
    lcdData(col_start);  // Start low byte
    lcdData(0x00);  // End high byte
    lcdData(col_end);  // End low byte

    // Row Address Set
    lcdCommand(0x2B);
    lcdData(0x00);  // Start high byte
    lcdData(row_start);  // Start low byte
    lcdData(0x00);  // End high byte
    lcdData(row_end);  // End low byte

    // Memory Write
    lcdCommand(0x2C);

    // Send the pixels as 8 color bars
    for (uint8_t row = row_start; row <= row_end; row++) {
        uint16_t color;
        uint8_t which_color = (row - row_start) / ((row_end + 1 - row_start) / 8);

        switch (which_color) {
            case 0: color = LCD_RGB565_WHITE; break;
            case 1: color = LCD_RGB565_RED; break;
            case 2: color = LCD_RGB565_GREEN; break;
            case 3: color = LCD_RGB565_BLUE; break;
            case 4: color = LCD_RGB565_BLACK; break;
            case 5: color = LCD_RGB565_RED | LCD_RGB565_GREEN; break;
            case 6: color = LCD_RGB565_RED | LCD_RGB565_BLUE; break;
            default: color = LCD_RGB565_BLUE | LCD_RGB565_GREEN; break;
        }

        for (uint8_t col = col_start; col <= col_end; col++) {
            SPI.transfer(color >> 8);
            SPI.transfer(color & 0xFF);
        }
    }
}

void refreshIWDG() {
    IWDG->KR = 0xAAAA;  // Refresh the watchdog timer
    Serial2.println("Watchdog Refreshed");
}