#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"
#include "pico/binary_info.h"
#include "hardware/regs/rosc.h"

#include "hardware/spi.h"

#include "framebuffer.h"
#include "fb_text.h"
#include "oled.h"

#define SSD1309_HEIGHT 64

const uint LED_PIN = 25;

/* Picotool info */
bi_decl(bi_program_description("OLED demo"))
bi_decl(bi_1pin_with_name(LED_PIN, "Blink LED"))

static const uint8_t zir_data[] = {
        // levo
        0b10000000,
        0b01000000,
        0b00100000,
        0b00010000,
        0b00001000,
        0b00000100,
        0b00000010,
        0b00011001,
        0b00011111,
        0b11111000,
        0b00011111,
        0b00001000,
        //
        0b10000000,
        0b01000000,
        0b00100000,
        0b00010000,
        0b00001000,
        0b00000100,
        0b00000010,
        0b00000001,
        0b10000000,
        0b11111111,
        0b00100000,
        0b00010000,
        //
        0b00110000,
        0b10001000,
        0b11111000,
        0b00011000,
        0b11111000,
        0b10011000,
        0b00011000,
        0b10011000,
        0b11111000,
        0b00011111,
        0b11111000,
        0b10000000,
};

static const fb_bitmap_t zir = {
        .width = 12,
        .height = 24,
        .data = zir_data
};

void seed_random_from_rosc()
{
    uint32_t random = 0x811c9dc5;
    uint8_t next_byte = 0;
    volatile uint32_t *rnd_reg = (uint32_t *)(ROSC_BASE + ROSC_RANDOMBIT_OFFSET);

    for (int i = 0; i < 16; i++) {
        for (int k = 0; k < 8; k++) {
            next_byte = (next_byte << 1) | (*rnd_reg & 1);
        }

        random ^= next_byte;
        random *= 0x01000193;
    }

    srand(random);
}



void freeline(float x1, float y1, float x2, float y2, fbcolor_t color)
{
    float dx = ((float) x2 - (float) x1);
    float dy = (float) y2 - (float) y1;
    float dxa = dx > 0 ? dx : -dx;
    float dya = dy > 0 ? dy : -dy;
    float maxdist = MAX(dxa, dya);
    dx /= maxdist;
    dy /= maxdist;

    fb_px((fbpos_t) roundf(x1), (fbpos_t) roundf(y1), color);
    //fb_px((fbpos_t) x2, (fbpos_t) y2, color);
    for (uint d = 0; d <= (uint) maxdist; d++) {
        x1 += dx;
        y1 += dy;
        fb_px((fbpos_t) roundf(x1), (fbpos_t) roundf(y1), color);
    }
}


int main() {
	stdio_init_all();

    seed_random_from_rosc();

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);

    oled_init();

    fb_clear();

    fb_blit();

#define NUMBALLS 50
#define BALLW 2
    struct ball {
        float x;
        float y;
        float sx;
        float sy;
    };

    struct ball balls[NUMBALLS];

    for (uint i = 0; i < NUMBALLS; i++) {
        balls[i].x = (float) (random() % FBW);
        balls[i].y = (float) (random() % FBH);
        balls[i].sx = ((float) (random() % 100) - 50.0f) / 50.0f;
        balls[i].sy = ((float) (random() % 100) - 50.0f) / 50.0f;
    }

    float fi = 0;

	while (1) {
        //gpio_put(LED_PIN, 0);
        //oled_invert(true);

        fi += M_PI / 180.0f;

        for (uint i = 0; i < NUMBALLS; i++) {
            struct ball *ball = &balls[i];
            ball->x += ball->sx;
            ball->y += ball->sy;

            if (ball->sx < 0 && ball->x <= 0) {
                ball->sx *= -1;
                ball->x = 0;
            }
            if (ball->sx > 0 && ball->x >= FBW - BALLW) {
                ball->sx *= -1;
                ball->x = FBW - BALLW;
            }
            if (ball->sy < 0 && ball->y <= 0) {
                ball->sy *= -1;
                ball->y = 0;
            }
            if (ball->sy > 0 && ball->y >= FBH - BALLW) {
                ball->sy *= -1;
                ball->y = FBH - BALLW;
            }
        }

        // rotating hexagon
        const float CW = 15;
        const float AA = M_PI / 3.0f;
        const float HEXX = 30;
        const float HEXY = 32;

        float ar[] = {
                sinf(0 + fi) * CW,
                cosf(0 + fi) * CW,

                sinf(AA + fi) * CW,
                cosf(AA + fi) * CW,

                sinf(AA*2 + fi) * CW,
                cosf(AA*2 + fi) * CW,

                sinf(AA*3 + fi) * CW,
                cosf(AA*3 + fi) * CW,

                sinf(AA*4 + fi) * CW,
                cosf(AA*4 + fi) * CW,

                sinf(AA*5 + fi) * CW,
                cosf(AA*5 + fi) * CW,

                sinf(AA*6 + fi) * CW,
                cosf(AA*6 + fi) * CW,
        };

        fb_clear();
        fb_text(70, 30, "MEOW", FONT_DOUBLE, 1);
        fb_text(80, 15, "meow", FONT_DOUBLE, 1);

        for (uint i = 3; i <= 13; i += 2) {
            freeline(HEXX + ar[i-3], HEXY + ar[i-2], HEXX + ar[i-1], HEXY + ar[i], 1);
            if (i == 3 || i == 7 || i == 11) {
                fb_text(HEXX + ar[i-3] -5, HEXY + ar[i-2] - 3, "Miau!", FONT_5X7, 1);
            }
        }

        for (uint i = 0; i < NUMBALLS; i++) {
            struct ball *ball = &balls[i];
            fb_rect((fbpos_t) roundf(ball->x), (fbpos_t) roundf(ball->y), BALLW, BALLW, 1);
        }

        fb_blit();

        sleep_ms(10);

        //oled_invert(false);

		//gpio_put(LED_PIN, 1);

		//puts("Hello World\n");
		//sleep_ms(1000);
	}
}