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

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;
}

void set_color(uint8_t* buffer, const Color color) {
    for (int i = 7; i >= 0; i--)
        *buffer++ = (color.g & (1 << i)) ? 0b11111000 : 0b10000000;

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

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

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]);
    }
    return data_size;
}