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

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"
#include "ws2812.pio.h"

#define IS_RGBW true
#define NUM_PIXELS 1

#ifdef PICO_DEFAULT_WS2812_PIN
#define WS2812_PIN PICO_DEFAULT_WS2812_PIN
#else
// default to pin 2 if the board doesn't have a default WS2812 pin defined
#define WS2812_PIN 6
#endif

typedef struct {
    double r;       // a fraction between 0 and 1
    double g;       // a fraction between 0 and 1
    double b;       // a fraction between 0 and 1
} rgb;

static inline void put_pixel(uint32_t pixel_wgrb) {
    pio_sm_put_blocking(pio0, 0, pixel_wgrb); //<< 8u);
}

static inline uint32_t urgb_u32(uint8_t r, uint8_t g, uint8_t b) {
    return
            ((uint32_t) (r) << 8) |
            ((uint32_t) (g) << 16) |
            (uint32_t) (b);
}

void pulse(uint8_t r, uint8_t g, uint8_t b, uint8_t w) {
    for (int i = 0; i < 255; i++) {
        uint32_t newR = (uint32_t) fmax(fmin(r * i / 255, 255), 0);
        uint32_t newG = (uint32_t) fmax(fmin(g * i / 255, 255), 0);
        uint32_t newB = (uint32_t) fmax(fmin(b * i / 255, 255), 0);
        uint32_t newW = (uint32_t) fmax(fmin(w * i / 255, 255), 0);
        uint32_t color = (newW << 24) | (newR << 16) | (newG << 8) | newB;
        put_pixel(color);
        sleep_ms(20);
    };
}

rgb hsv2rgb(double h, double s, double v) {
    double      hh, p, q, t, ff;
    long        i;
    rgb        out;

    if(s <= 0.0) {       // < is bogus, just shuts up warnings
        out.r = v;
        out.g = v;
        out.b = v;
        return out;
    }
    hh = h;
    if(hh >= 360.0) hh = 0.0;
    hh /= 60.0;
    i = (long)hh;
    ff = hh - i;
    p = v * (1.0 - s);
    q = v * (1.0 - (s * ff));
    t = v * (1.0 - (s * (1.0 - ff)));

    switch(i) {
        case 0:
            out.r = v;
            out.g = t;
            out.b = p;
            break;
        case 1:
            out.r = q;
            out.g = v;
            out.b = p;
            break;
        case 2:
            out.r = p;
            out.g = v;
            out.b = t;
            break;

        case 3:
            out.r = p;
            out.g = q;
            out.b = v;
            break;
        case 4:
            out.r = t;
            out.g = p;
            out.b = v;
            break;
        case 5:
        default:
            out.r = v;
            out.g = p;
            out.b = q;
            break;
    }
    return out;
}

int main() { //BGRW B: Lowest bits, W: Highest bits
    stdio_init_all();

    // todo get free sm
    PIO pio = pio0;
    int sm = 0;
    uint offset = pio_add_program(pio, &ws2812_program);

    ws2812_program_init(pio, sm, offset, WS2812_PIN, 800000, IS_RGBW);

    put_pixel(0b11111111111111111111111111111111);
    sleep_ms(1000);
    put_pixel(0b00000000000000000000000000000000);
    sleep_ms(1000);

    while (1) {
        /*uint32_t color = (255 << 24) | (255 << 16) | (255 << 8) | 255;
        put_pixel(color);
        sleep_ms(1000);
        color = (0 << 24) | (0 << 16) | (0 << 8) | 255;
        put_pixel(color);
        sleep_ms(1000);
        color = (0 << 24) | (0 << 16) | (255 << 8) | 255;
        put_pixel(color);
        sleep_ms(1000);
        color = (0 << 24) | (255 << 16) | (255 << 8) | 255;
        put_pixel(color);
        sleep_ms(1000);*/
        /*pulse(255, 0, 0, 0);
        sleep_ms(10);
        pulse(0, 255, 0, 0);
        sleep_ms(10);
        pulse(0, 0, 255, 0);
        sleep_ms(10);
        pulse(0, 0, 0, 255);
        sleep_ms(10);
        pulse(0, 0, 255, 255);
        sleep_ms(10);
        pulse(0, 255, 255, 255);
        sleep_ms(10);
        pulse(255, 255, 255, 255);
        sleep_ms(10);*/
        for (int i = 0; i < 360; i++) {
            rgb newColor = hsv2rgb(i, 1, 1);
            uint32_t color =
                    ((uint32_t)(fmin(newColor.r * 255, 255)) << 16) | ((uint32_t)(fmin(newColor.g * 255, 255)) << 8) |
                    (uint32_t)(fmin(newColor.b * 255, 255));
            put_pixel(color);
            sleep_ms(20);
        }
        /*put_pixel(0b11111111111111111111111111111111);
        sleep_ms(1000);*/
    }
}