#include "main.h"
#include <stdint.h>
#define OLED_ADDRESS 0x78 //0x3C << 1


void SystemClock_Config(void);
static void MX_GPIO_Init(void);
void Error_Handler(void);

// --- Print Functions ---
void consolePrint(char text[]) {
    int counter = 0;
    while (text[counter] != '\0') {
        while (!(USART2->SR & USART_SR_TXE));
        USART2->DR = text[counter];
        counter++;
    }
}

void intPrint(int counter) {
    char digit[11];
    if (counter == 0) {
        consolePrint("0");
        return;
    }
    int i = 0;
    char temp[11];
    while (counter > 0) {
        temp[i] = (char)((counter % 10) + '0');
        i++;
        counter /= 10;
    }
    int j = 0;
    while (i > 0) {
        digit[j++] = temp[--i];
    }
    digit[j] = '\0';
    consolePrint(digit);
}
// --- I2C Related Functions

void I2C1Init(){
        // Reset and Clear to clean
        I2C1 -> CR1 |= I2C_CR1_SWRST;
        I2C1 -> CR1 &= ~I2C_CR1_SWRST;

        // Configuring the input Clock frequency
        I2C1 -> CR2 = 42U;

        //Configuring the I2C Clock control register
        I2C1 -> CCR = 210U;

        //Configuring the Max Rise Time
        I2C1 -> TRISE = 43U;

        // Enabling the peripheral
        I2C1 -> CR1 |= I2C_CR1_PE;
}

void OLEDWriteCommand(uint8_t cmd){
        I2C1 -> CR1 |= I2C_CR1_START;
        while(!(I2C1 -> SR1 & I2C_SR1_SB));
        I2C1 -> DR = OLED_ADDRESS;
        while (!(I2C1 -> SR1 & I2C_SR1_ADDR));
        (void) I2C1 -> SR1;
        (void) I2C1 -> SR2;
        //Send Control Byte (0x00 means next byte is a command)
        while (!(I2C1->SR1 & I2C_SR1_TXE));
        I2C1->DR = 0x00;
        while (!(I2C1->SR1 & I2C_SR1_TXE));
        I2C1->DR = cmd;

        while (!(I2C1->SR1 & I2C_SR1_BTF));
        I2C1 -> CR1 |= I2C_CR1_STOP;
}
void OLEDWriteData(uint8_t data){
        I2C1 -> CR1 |= I2C_CR1_START;
        while(!(I2C1 -> SR1 & I2C_SR1_SB));
        I2C1 -> DR = OLED_ADDRESS;
        while (!(I2C1 -> SR1 & I2C_SR1_ADDR));
        (void) I2C1 -> SR1;
        (void) I2C1 -> SR2;
        //Send Control Byte (0x40 means put the following byte directly into GDDRAM to light up pixels)
        while (!(I2C1->SR1 & I2C_SR1_TXE));
        I2C1->DR = 0x40;
        while (!(I2C1->SR1 & I2C_SR1_TXE));
        I2C1->DR = data;
        while (!(I2C1->SR1 & I2C_SR1_BTF));
        I2C1 -> CR1 |= I2C_CR1_STOP;
}

void OLEDCommandList(){
        OLEDWriteCommand(0xAE); // display OFF
        OLEDWriteCommand(0x20); // set Memory Addressing Mode
        OLEDWriteCommand(0x02); // 0x02 = Page Addressing Mode
        OLEDWriteCommand(0x8D); // charge Pump Command
        OLEDWriteCommand(0x14); // enable charge Pump
        OLEDWriteCommand(0xA4);
        OLEDWriteCommand(0xC8); // flip the screen
        OLEDWriteCommand(0xAF); // Display ON

}

void setCursor(uint8_t page, uint8_t column){
	OLEDWriteCommand(0xB0 | (page & 0x07)); // 0x07 is a safe guard to keep everything in range
	OLEDWriteCommand(0x00 | (column & 0x0F));
	OLEDWriteCommand(0x10 | ((column >> 4) & 0x0F));
}
void OLEDClear(){
	// Clearing the OLED screen
	for(int page =0; page<8;page++){
		setCursor(page,0);
		for(int column =0; column < 128 ; column++){
			// The display will auto increment for us
			OLEDWriteData(0x00);
		}
	}
}
void drawLogo(){
	static uint8_t logo[1024] = {
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0xF0, 0xF0, 0xF0,
	    0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0xF0, 0xF0, 0xF0, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0xF0, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF,
	    0xFF, 0xF0, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xFF, 0xFF, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xFF, 0xFF, 0xFF, 0xFF, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0xFF, 0xFF, 0xFF, 0xF0, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x0F, 0xFF, 0xFF, 0xFF, 0x0F, 0x0F, 0x0F, 0x0F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	    0x0F, 0x0F, 0x0F, 0x0F, 0xFF, 0xFF, 0xFF, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, 0xFF,
	    0xFF, 0xFF, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0xFF, 0x0F, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	};
	for(int page =0; page<8;page++){
		setCursor(page,0);
		for(int column =0; column < 128 ; column++){
			// The display will auto increment for us
			OLEDWriteData(logo[column + page*128]);
		}
	}
}

// --- Main Application ---
int main(void)
{
    HAL_Init();
    SystemClock_Config();

    // Initialize GPIO via HAL (handles PA2/PA3 for USART2)
    MX_GPIO_Init();

    // --- Enable Clocks ---
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
    RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
    RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;

    // --- USART2 Setup ---
    USART2->BRR = 0x1117; // 42Mhz / 9600
    USART2->CR1 |= USART_CR1_TE | USART_CR1_UE;

    // --- I2C1 GPIO Setup (PB8 = SCL, PB9 = SDA) ---
    // Mode Register -->  Alternate Function Mode for both Pins
    GPIOB -> MODER &= ~GPIO_MODER_MODE8_0;
    GPIOB -> MODER |= GPIO_MODER_MODE8_1;
    GPIOB -> MODER &= ~GPIO_MODER_MODE9_0;
    GPIOB -> MODER |= GPIO_MODER_MODE9_1;

    // Output Type Register --> Open Drain For both Pins
    GPIOB -> OTYPER |= GPIO_OTYPER_OT8;
    GPIOB -> OTYPER |= GPIO_OTYPER_OT9;

    // Output Speed Register --> High Speed for both
    GPIOB -> OSPEEDR |= GPIO_OSPEEDR_OSPEED8_0;
    GPIOB -> OSPEEDR |= GPIO_OSPEEDR_OSPEED8_1;
    GPIOB -> OSPEEDR |= GPIO_OSPEEDR_OSPEED9_0;
    GPIOB -> OSPEEDR |= GPIO_OSPEEDR_OSPEED9_1;

    // Pull up Pull Down Register --> Pull Up for both Pins
    GPIOB -> PUPDR |= GPIO_PUPDR_PUPD8_0;
    GPIOB -> PUPDR &= ~GPIO_PUPDR_PUPD8_1;
    GPIOB -> PUPDR |= GPIO_PUPDR_PUPD9_0;
    GPIOB -> PUPDR &= ~GPIO_PUPDR_PUPD9_1;

    // Setting up the Alternate Function to AF4
    GPIOB -> AFR[1] &= ~GPIO_AFRH_AFRH0_0;
    GPIOB -> AFR[1] &= ~GPIO_AFRH_AFRH0_1;
    GPIOB -> AFR[1] |= GPIO_AFRH_AFRH0_2;
    GPIOB -> AFR[1] &= ~GPIO_AFRH_AFRH0_3;

    GPIOB -> AFR[1] &= ~GPIO_AFRH_AFRH1_0;
    GPIOB -> AFR[1] &= ~GPIO_AFRH_AFRH1_1;
    GPIOB -> AFR[1] |= GPIO_AFRH_AFRH1_2;
    GPIOB -> AFR[1] &= ~GPIO_AFRH_AFRH1_3;

    I2C1Init();

    OLEDCommandList();
    drawLogo();
    consolePrint("OLED initialized.\r\n");
    while (1)
    {
        // Main loop
    }
}

// --- System Auto-Generated Configs ---

void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);

  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 16;
  RCC_OscInitStruct.PLL.PLLN = 336;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4;
  RCC_OscInitStruct.PLL.PLLQ = 2;
  RCC_OscInitStruct.PLL.PLLR = 2;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOH_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

  GPIO_InitStruct.Pin = USART_TX_Pin|USART_RX_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
  GPIO_InitStruct.Alternate = GPIO_AF7_USART2;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}

void Error_Handler(void)
{
  __disable_irq();
  while (1)
  {
  }
}

#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{
}
#endif
