#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

// Structures
typedef struct vector {int x;int y;}vector2D;
typedef struct coordinate {int x;int y;}coord;

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;
    uint8_t framebuffer[64][128]; //TODO testing
    //// oled.buffer = &framebuffer; //TODO testing
    ssd1306_init(&oled, 128, 64, 0x3c, i2c1);
    ssd1306_invert(&oled,1);
    printf("This is the buffer: %d", *oled.buffer); //TODO this is just for testing
    //Input Variables
    uint8_t input_data;
    int arr_input_data[8];
    char str_input_data[10];

    //Snake Variables
    int position[] = {0,0}; //coordinate (x, y) 
    int direction[] = {0,0}; //vector (x, y)

    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
        }
        //* -------------------------

        

        //* OLED rendering ---------
        //ssd1306_clear(&oled);

        ssd1306_show(&oled);
        //* ------------------------


        sleep_ms(30);
    }
    return 0;
}