#include <zephyr/kernel.h>
#include <zephyr/smf.h>
#include <zephyr/logging/log.h>

#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/spi.h>


LOG_MODULE_REGISTER(nRF_Log, LOG_LEVEL_INF);

static const struct gpio_dt_spec OLED_rst = GPIO_DT_SPEC_GET(DT_ALIAS(oledrst), gpios);
static const struct gpio_dt_spec OLED_isData = GPIO_DT_SPEC_GET(DT_ALIAS(oledisdata), gpios);

// Forward declaration of SMF context
struct robot_context {
    struct smf_ctx ctx;
};
static struct robot_context robot;
static struct k_timer OLED_timer;
static void test_oled();

/* ==============================================================================
 *  OLED SPI
 * ============================================================================== */

/* Match the label in the overlay: oled */
#define SPI_NODE DT_NODELABEL(oled)
#define SPIOP (SPI_WORD_SET(8) | SPI_TRANSFER_MSB | SPI_HOLD_ON_CS)

/* Create a devicetree-based SPI spec */
struct spi_dt_spec spispec = SPI_DT_SPEC_GET(SPI_NODE, SPIOP, 0);


// SSD1351 Control
#define OLED_CMD    0
#define OLED_DAT    1
#define SSD1351_WIDTH   128u
#define SSD1351_HEIGHT  128u
#define SSD1351_BURST_BYTES 4096u

static int spi_write_bytes(const uint8_t *data, size_t len)
{
    if (!spi_is_ready_dt(&spispec)) {
        LOG_ERR("SPI device not ready");
        return -ENODEV;
    }

    struct spi_buf tx_buf = {
        .buf = (void *)data,   // Zephyr API uses void*
        .len = len,
    };
    struct spi_buf_set tx_set = {
        .buffers = &tx_buf,
        .count = 1,
    };

    int err = spi_write_dt(&spispec, &tx_set);
    // if (err) {
    //     LOG_ERR("spi_write_dt() failed, err: %d", err);
    // }
    return err;
}

static inline int SSD1351_WriteCmd(uint8_t cmd)
{
    gpio_pin_set_dt(&OLED_isData, OLED_CMD);
    return spi_write_bytes(&cmd, 1);
}

static inline int SSD1351_WriteDataByte(uint8_t data)
{
    gpio_pin_set_dt(&OLED_isData, OLED_DAT);
    return spi_write_bytes(&data, 1);
}

static inline int SSD1351_WriteDataBuf(const uint8_t *data, size_t len)
{
    gpio_pin_set_dt(&OLED_isData, OLED_DAT);
    return spi_write_bytes(data, len);
}

void SSD1351_FillScreen(uint16_t color)
{
    static uint8_t chunk[SSD1351_BURST_BYTES];
    const uint8_t high = (uint8_t)(color >> 8);
    const uint8_t low  = (uint8_t)(color & 0xFF);

    for (size_t i = 0; i < sizeof(chunk); i += 2) {
        chunk[i] = high;
        chunk[i + 1] = low;
    }

    (void)SSD1351_WriteCmd(0x15);
    (void)SSD1351_WriteDataByte(0x00);
    (void)SSD1351_WriteDataByte(SSD1351_WIDTH - 1);

    (void)SSD1351_WriteCmd(0x75);
    (void)SSD1351_WriteDataByte(0x00);
    (void)SSD1351_WriteDataByte(SSD1351_HEIGHT - 1);

    (void)SSD1351_WriteCmd(0x5C);

    size_t total = (size_t)SSD1351_WIDTH * (size_t)SSD1351_HEIGHT * 2u;

    int first_err = 0;

    while (total > 0) {
        size_t len = (total > sizeof(chunk)) ? sizeof(chunk) : total;
        len &= ~1u; // keep even

        int err = SSD1351_WriteDataBuf(chunk, len);
        if (err && !first_err) {
            first_err = err;   // store it; don’t spam logs
        }

        total -= len;
    }

    if (first_err) {
        LOG_ERR("FillScreen SPI error: %d", first_err);
    }
}

void SSD1351_TestPatternStep(void)
{
    static uint8_t step = 0;

    uint16_t color;

    // Pick a different color each call
    switch(step)
    {
        case 0: color = 0xF800; break; // Red
        case 1: color = 0x07E0; break; // Green
        case 2: color = 0x001F; break; // Blue
        case 3: color = 0xFFFF; break; // White
        case 4: color = 0xFFE0; break; // Yellow
        case 5: color = 0xF81F; break; // Magenta
        case 6: color = 0x07FF; break; // Cyan
        default: color = 0x0000; break; // Black
    }

    step++;
    if(step > 7) step = 0;

    // Fill screen with that color
    SSD1351_FillScreen(color);
}

void SSD1351_Reset(void)
{
    gpio_pin_set_dt(&OLED_rst, 1);
    k_msleep(5);
    gpio_pin_set_dt(&OLED_rst, 0);
    k_msleep(5);
    gpio_pin_set_dt(&OLED_rst, 1);
    k_msleep(150);
}
void SSD1351_Init(void)
{    
    SSD1351_WriteCmd(0xFD);  // Lock state of the Driver
    SSD1351_WriteDataByte(0x12);  // By default unlocked, just to make sure.
    SSD1351_WriteCmd(0xFD);
    SSD1351_WriteDataByte(0xB1);   // Enable access to A2/B1/B3/BB/BE/C1 (common init practice)

	SSD1351_WriteCmd(0xae); 	// Command: Set Display OFF (AEh)

    // --- Set column/row range for your panel resolution ---
    SSD1351_WriteCmd(0x15);   // Set Column Address
    SSD1351_WriteDataByte(0x00);
    SSD1351_WriteDataByte((uint8_t)(SSD1351_WIDTH - 1));

    SSD1351_WriteCmd(0x75);   // Set Row Address
    SSD1351_WriteDataByte(0x00);
    SSD1351_WriteDataByte((uint8_t)(SSD1351_HEIGHT - 1));
    
	SSD1351_WriteCmd(0xa1); 	// Command: Set Display Start Line (A1h)
	SSD1351_WriteDataByte(0x00);  // Data: Display Start Line = 0 (Reset is 00h)
	SSD1351_WriteCmd(0xa2); 	// Command: Set Display Offset (A2h)
	SSD1351_WriteDataByte(0x00);  // Data: Display Offset = 0 (Reset is 00h)
	// Vorher A4
    SSD1351_WriteCmd(0xa6); 	// Command: Set Display Mode to Normal Display (A4h) (Reset is A4h)
	
	SSD1351_WriteCmd(0xa0); 	// Command: Set Re-map & Data Format (A0h)
    // Data: 0x74 = 0b01110100
    // A[7:6]=01: 65K color format
    // A[5]=1: Enable COM Split Odd Even
    // A[4]=1: COM Scan Direction: COM[N-1] to COM0 (scan up)
    // A[3]=0: Disable COM Left/Right Remap
    // A[2]=1: Color Sequence: C-B-A (BGR order)
    // A[1]=0: Column Address Remap: SEG0 maps to Column 0
    // A[0]=0: Address Increment: Horizontal
	SSD1351_WriteDataByte((0x64 + 0x10));

    // --- Master contrast / per-color contrast (SSD1351: C7 + C1) ---
    SSD1351_WriteCmd(0xC7);   // Master Contrast Current Control (scale factor 1..16) :contentReference[oaicite:10]{index=10}
    SSD1351_WriteDataByte(0x0F);   // 16/16 (full)

    SSD1351_WriteCmd(0xC1);
    SSD1351_WriteDataByte(0x75);
    SSD1351_WriteDataByte(0x70);
    SSD1351_WriteDataByte(0x6a);
	
    // --- Phase length (note: SSD1351 phase2 must be >= 3 DCLKs) ---
    SSD1351_WriteCmd(0xB1);   // Set Phase Length :contentReference[oaicite:12]{index=12}
    SSD1351_WriteDataByte(0x32);   // phase1=0x2, phase2=0x3 (min-valid on SSD1351)

	SSD1351_WriteCmd(0xb3); 	// Command: Set Display Clock Divide Ratio / Oscillator Frequency (B3h)
    // Data: 0x80 = 0b10000000
    // A[3:0]=0000b: Clock Divide Ratio D = 1 (DCLK = Fosc/1) (Reset is D=1)
    // A[7:4]=1000b: Oscillator Frequency setting 8 (Reset is setting 12/0xC)
	SSD1351_WriteDataByte(0x80);

	SSD1351_WriteCmd(0xbb); 	// Command: Set Pre-charge Voltage (BBh)
	SSD1351_WriteDataByte(0x08);  // Data: Pre-charge voltage level 0x08 (low level) (Reset is 3Eh)

    SSD1351_WriteCmd(0xBE);   // Set VCOMH Voltage (see Table 9-1) :contentReference[oaicite:15]{index=15}
    SSD1351_WriteDataByte(0x06);   // ~0.84*VCC on typical SSD1351 tables

    // --- Linear LUT ---
    SSD1351_WriteCmd(0xB9);   // Use Built-in Linear LUT :contentReference[oaicite:16]{index=16}

    // --- Display ON (Sleep mode OFF) ---
    SSD1351_WriteCmd(0xAF);   // Sleep mode OFF / display ON :contentReference[oaicite:17]{index=17}
}

/* ==============================================================================
 *  STATE MACHINE
 * ============================================================================== */

static const struct smf_state demo_states[];

enum state_ids {
    STATE_IDLE,
    STATE_ONE,
    STATE_CONNECTED,
};

// -- Transition Actions --



// -- STATE: IDLE --
static void state_idle_entry(void *o)
{
    k_timer_start(&OLED_timer, K_MSEC(2200), K_MSEC(2200));
}

static enum smf_state_result state_idle_run(void *o)
{
    return SMF_EVENT_HANDLED;
}

// -- State Table --
static const struct smf_state demo_states[] = {
    [STATE_IDLE] = SMF_CREATE_STATE(state_idle_entry, state_idle_run, NULL, NULL, NULL),
};

/* ==============================================================================
 *  MAIN
 * ============================================================================== */

int main(void)
{
    if (!gpio_is_ready_dt(&OLED_rst)) {
        return 0;
    }
    // Config GPIOs
    gpio_pin_configure_dt(&OLED_rst, GPIO_OUTPUT_ACTIVE);
    gpio_pin_configure_dt(&OLED_isData, GPIO_OUTPUT_ACTIVE);

    k_timer_init(&OLED_timer, test_oled, NULL);

    smf_set_initial(SMF_CTX(&robot), &demo_states[STATE_IDLE]);

    if (!device_is_ready(spispec.bus)) {
        LOG_ERR("SPI bus not ready");
        return 0;
    }

    SSD1351_Reset();
    SSD1351_Init();

    LOG_INF("Hello.");
    
    while (1) {
        smf_run_state(SMF_CTX(&robot));
        k_yield();
    }
    return 0;
}

static void test_oled()
{
    SSD1351_TestPatternStep();
}