/*
 * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <stdlib.h>
#include <stdio.h>
#include "pico.h"
#include "pico/scanvideo.h"
#include "pico/sync.h"
#include "pico/scanvideo/composable_scanline.h"
#include "hardware/gpio.h"
#include "pico/stdlib.h"
#include "pico/multicore.h"

#if PICO_ON_DEVICE

#include "hardware/clocks.h"
#include "hardware/structs/vreg_and_chip_reset.h"

#endif

CU_REGISTER_DEBUG_PINS(generation)

#define vga_mode vga_mode_320x240_60

//#define USE_FLOAT 1

#if USE_FLOAT
//typedef float fixed;
typedef double fixed;
static inline fixed float_to_fixed(float x) {
    return x;
}
static inline fixed fixed_mult(fixed a, fixed b) {
    return a*b;
}
#else
#define FRAC_BITS 25u
typedef int32_t fixed;

static inline fixed float_to_fixed(float x) {
    return (fixed) (x * (float) (1u << FRAC_BITS));
}

static inline fixed fixed_mult(fixed a, fixed b) {
    int64_t r = ((int64_t) a) * b;
    return (int32_t) (r >> FRAC_BITS);
}

#endif

#define max_iters 127//255

struct mutex frame_logic_mutex;
static void frame_update_logic();

void fill_scanline_buffer(struct scanvideo_scanline_buffer *buffer);
static uint y;
static fixed x0, y0;
static fixed dx0_dx, dy0_dy;
static fixed max;
static bool params_ready;


static uint16_t framebuffer[320 * 240];
//static uint16_t *framebuffer;

static uint16_t colors[16] = {
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(66, 30, 15),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(25, 7, 26),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(9, 1, 47),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(4, 4, 73),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(0, 7, 100),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(12, 44, 138),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(24, 82, 177),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(57, 125, 209),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(134, 181, 229),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(211, 236, 248),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(241, 233, 191),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(248, 201, 95),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(255, 170, 0),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(204, 128, 0),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(153, 87, 0),
        PICO_SCANVIDEO_PIXEL_FROM_RGB8(106, 52, 3),
};

static void scanline(uint16_t *line_buffer, uint length, fixed mx, fixed my, fixed dmx_dx) {
    for (int x = 0; x < length; ++x) {
        int iters;
        fixed cr = mx;
        fixed ci = my;
        fixed zr = cr;
        fixed zi = ci;
        fixed xold = 0;
        fixed yold = 0;
        int period = 0;
        for (iters = 0; iters < max_iters; ++iters) {
            fixed zr2 = fixed_mult(zr, zr);
            fixed zi2 = fixed_mult(zi, zi);
            if (zr2 + zi2 > max) {
                break;
            }
            fixed zrtemp = zr2 - zi2 + cr;
            zi = 2 * fixed_mult(zr, zi) + ci;
            zr = zrtemp;
            if (zr == xold && zi == yold) {
                iters = max_iters + 1;
                break;
            }
            if (++period > 20) {
                period = 0;
                xold = zr;
                yold = zi;
            }
        }
        if (iters == max_iters + 1) {
            line_buffer[x] = 0;//x1f;
        } else {
            line_buffer[x] = iters == max_iters ? 0 : colors[iters & 15u];
        }
        mx += dmx_dx;
    }
}

// "Worker thread" for each core
void __time_critical_func(render_loop)() {
    static uint32_t last_frame_num = 0;
    int core_num = get_core_num();
    printf("Rendering on core %d\n", core_num);
    while (true) {
        mutex_enter_blocking(&frame_logic_mutex);
        if (y == vga_mode.height) {
            params_ready = false;
            frame_update_logic();
            y = 0;
        }
        uint _y = y++;
        fixed _x0 = x0, _y0 = y0;
        fixed _dx0_dx = dx0_dx, _dy0_dy = dy0_dy;
        mutex_exit(&frame_logic_mutex);

        scanline(framebuffer + _y * 320, 320, _x0, _y0 + _dy0_dy * _y, _dx0_dx);
#if !PICO_ON_DEVICE
        struct scanvideo_scanline_buffer *buffer = scanvideo_begin_scanline_generation(true);
        fill_scanline_buffer(buffer);
        scanvideo_end_scanline_generation(buffer);
#endif
    }
}

int64_t timer_callback(alarm_id_t alarm_id, void *user_data) {
    struct scanvideo_scanline_buffer *buffer = scanvideo_begin_scanline_generation(false);
    while (buffer) {
        fill_scanline_buffer(buffer);
        scanvideo_end_scanline_generation(buffer);
        buffer = scanvideo_begin_scanline_generation(false);
    }
    return 100;
}

void fill_scanline_buffer(struct scanvideo_scanline_buffer *buffer) {
    static uint32_t postamble[] = {
            0x0000u | (COMPOSABLE_EOL_ALIGN << 16)
    };

    buffer->data[0] = 4;
    buffer->data[1] = host_safe_hw_ptr(buffer->data + 8);
    buffer->data[2] = 158; // first four pixels are handled separately
    uint16_t *pixels = framebuffer + scanvideo_scanline_number(buffer->scanline_id) * 320;
    buffer->data[3] = host_safe_hw_ptr(pixels + 4);
    buffer->data[4] = count_of(postamble);
    buffer->data[5] = host_safe_hw_ptr(postamble);
    buffer->data[6] = 0;
    buffer->data[7] = 0;
    buffer->data_used = 8;

    // 3 pixel run followed by main run, consuming the first 4 pixels
    buffer->data[8] = (pixels[0] << 16u) | COMPOSABLE_RAW_RUN;
    buffer->data[9] = (pixels[1] << 16u) | 0;
    buffer->data[10] = (COMPOSABLE_RAW_RUN << 16u) | pixels[2];
    buffer->data[11] = ((317 + 1 - 3) << 16u) | pixels[3]; // note we add one for the black pixel at the end
}

struct semaphore video_setup_complete;

void core1_func() {
    sem_acquire_blocking(&video_setup_complete);
    render_loop();
}

int vga_main(void) {
//    framebuffer = calloc(320*240, sizeof(uint16_t));
    mutex_init(&frame_logic_mutex);
    sem_init(&video_setup_complete, 0, 1);

    // Core 1 will wait for us to finish video setup, and then start rendering
    multicore_launch_core1(core1_func);

    hard_assert(vga_mode.width + 4 <= PICO_SCANVIDEO_MAX_SCANLINE_BUFFER_WORDS * 2);
    scanvideo_setup(&vga_mode);
    scanvideo_timing_enable(true);

    frame_update_logic();
    sem_release(&video_setup_complete);

#if PICO_ON_DEVICE
    add_alarm_in_us(100, timer_callback, NULL, true);
#endif
    render_loop();
    return 0;
}

static inline void raw_scanline_finish(struct scanvideo_scanline_buffer *dest) {
    // Need to pivot the first pixel with the count so that PIO can keep up
    // with its 1 pixel per 2 clocks
    uint32_t first = dest->data[0];
    uint32_t second = dest->data[1];
    dest->data[0] = (first & 0x0000ffffu) | ((second & 0x0000ffffu) << 16);
    dest->data[1] = (second & 0xffff0000u) | ((first & 0xffff0000u) >> 16);
    dest->status = SCANLINE_OK;
}

void __time_critical_func(frame_update_logic)() {
    if (!params_ready) {
        float scale = vga_mode.height / 2;
        static int foo;
        float offx = (MIN(foo, 200)) / 500.0f;
        float offy = -(MIN(foo, 230)) / 250.0f;
        scale *= (10000 + (foo++) * (float) foo) / 10000.0f;
        x0 = float_to_fixed(offx + (-vga_mode.width / 2) / scale - 0.5f);
        y0 = float_to_fixed(offy + (-vga_mode.height / 2) / scale);
        dx0_dx = float_to_fixed(1.0f / scale);
        dy0_dy = float_to_fixed(1.0f / scale);
        max = float_to_fixed(4.f);
        params_ready = true;
    }
}

int main(void) {
#if PICO_ON_DEVICE
#ifdef TURBO_BOOST
    hw_set_bits(&mm_vreg_and_chip_reset->vreg, VREG_AND_CHIP_RESET_VREG_VSEL_BITS);
    sleep_ms(10);
    set_sys_clock_khz(48000*6, true);
#else
    //set_sys_clock(1536*MHZ, 4, 2);
    set_sys_clock_khz(48000 * 4, true);
#endif
#endif
    // Re init uart now that clk_peri has changed
    setup_default_uart();
    gpio_debug_pins_init();

    return vga_main();
}