#include "color_utils.h"
#include <math.h>
#include <string.h>

#define DATA_HIGH    0b00011111
#define DATA_LOW     0b00000011

Color hex_to_color(int hex) {
    Color color;

    color.r = (hex >> 16) % 256;
    color.g = (hex >> 8) % 256;
    color.b = hex % 256;

    return color;
}

Color hsv_to_color(double h, double s, double v) {
    h = fmod(h, 360);
    if (s > 1)
        s = fmod(s, 1);
    if (v > 1)
        v = fmod(v, 1);

    double c = v * s;
    double x = c * (1 - fabs(fmod(h / 60, 2) - 1));
    double m = v - c;

    double r0, g0, b0;

    if (h >= 0 && h < 60) {
        r0 = c;
        g0 = x;
        b0 = 0;
    } else if (h >= 60 && h < 120) {
        r0 = x;
        g0 = c;
        b0 = 0;
    } else if (h >= 120 && h < 180) {
        r0 = 0;
        g0 = c;
        b0 = x;
    } else if (h >= 180 && h < 240) {
        r0 = 0;
        g0 = x;
        b0 = c;
    } else if (h >= 240 && h < 300) {
        r0 = x;
        g0 = 0;
        b0 = c;
    } else {
        r0 = c;
        g0 = 0;
        b0 = x;
    }

    Color color;

    color.r = (r0 + m) * 255;
    color.g = (g0 + m) * 255;
    color.b = (b0 + m) * 255;

    return color;
}

Color fix_color(const Color color) {
    Color result;

    result.r = color.r * color.r / 0xff;
    result.g = color.g * color.g * 0x88 / 0xff / 0xff;
    result.b = color.b * color.b * 0x66 / 0xff / 0xff;

    return result;
}

void set_color(uint8_t* buffer, const Color color) {
    // *buffer++ = 0x00;

    for (int i = 7; i >= 0; i--)
        *buffer++ = (color.g & (1 << i)) ? DATA_HIGH : DATA_LOW;

    for (int i = 7; i >= 0; i--)
        *buffer++ = (color.r & (1 << i)) ? DATA_HIGH : DATA_LOW;

    for (int i = 7; i >= 0; i--)
        *buffer++ = (color.b & (1 << i)) ? DATA_HIGH : DATA_LOW;
}

int construct_buffer(uint8_t* buffer,
                     const Color* colors,
                     const int num_leds,
                     const int color_bytes,
                     const int res_bytes) {
    const int data_size = num_leds * color_bytes + res_bytes;
    memset(buffer, 0, data_size);
    for (int i = 0; i < num_leds; i++) {
        // set_color(buffer + i * color_bytes, colors[i]);
        set_color(buffer + i * color_bytes, fix_color(colors[i]));
    }
    return data_size;
}