#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "pico/stdlib.h"
#include "hardware/spi.h"
#include "stdbool.h"
#include "ssd1306.h"
#include "math.h"


// SPI Defines
#define SPI_PORT spi0
#define PIN_MISO 4 //? Q7
#define PIN_CL  2 //? CP
#define PIN_CS   5 //? PL

#define PIN_SDA 6
#define PIN_SCK 7

int main()
{
    stdio_init_all();

    //SPI initialisation. This example will use SPI at 1MHz.
    spi_init(SPI_PORT, 500000);
    gpio_set_function(PIN_MISO, GPIO_FUNC_SPI);
    gpio_set_function(PIN_CL, GPIO_FUNC_SPI);
    gpio_init(PIN_CS);
    gpio_set_dir(PIN_CS,true);

    //I2C Screen initialisation
    i2c_init(i2c1, 500000); //! Gave it higher baudrate, original was 400 000Hz
    gpio_set_function(PIN_SDA, GPIO_FUNC_I2C);
    gpio_set_function(PIN_SCK, GPIO_FUNC_I2C);
    ssd1306_t oled;
    int n = ssd1306_init(&oled, 128, 64, 0x3c, i2c1);
 
    //Variable Declaration
    uint8_t input_data;
    int arr_input_data[8];
    char str_input_data[10];
    str_input_data[0] = "{";
    str_input_data[9] = "}";

    while(true){
        
        //* Buttons ----------------
        //?Read D7 --> not included in second read_blocking
        spi_read_blocking(SPI_PORT, 0, &input_data, 1);
        arr_input_data[7] = input_data%2; //add value of 7th bit
        //?Read SPI input
        gpio_put(PIN_CS,1);
        spi_read_blocking(SPI_PORT, 0, &input_data, 1);
        gpio_put(PIN_CS,0);
        input_data = input_data>>1;//remove bit 7

        for (int i = 6; i>=0; i--) {
            arr_input_data[i] = input_data%2; //read if bit is 0 or 1
            input_data = input_data>>1; //remove bit from the var
        }
        //* -------------------------

        for (int i = 0; i <= 7; i++) {
            sprintf(&str_input_data[i], "%d", arr_input_data[i]);
        }
        //printf("%s", str_input_data);

        //* OLED rendering ---------
        ssd1306_clear(&oled);
        ssd1306_draw_string(&oled, 5, 5, 1, str_input_data);
        ssd1306_show(&oled);
        //* ------------------------


        sleep_ms(30);
    }
    return 0;
}