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

#ifdef PICO
#include <pico/printf.h>
#include <pico/stdlib.h>
#include <hardware/pwm.h>
#else
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#endif

void _sleep(size_t ms) {
#ifdef PICO
    sleep_ms(ms);
#else
    usleep(ms * 1000);
#endif
}

void _set_pwm_gpio_level(size_t gpio, size_t level) {
#ifdef PICO
    pwm_set_gpio_level(gpio, level);
#endif
}

size_t _compute_wave_level(float level) {
    float var = (sinf(level) + 1.0) / 2.0;
    return MAX_DUTY_STEPS * var;
}


void init_leds(leds *l) {
    for (size_t i = 0; i < NB_LED_BAR_GPIO; i++) {
        l->data[i].gpio = LED_BAR_GPIO[i];
        l->data[i].pos = i;
        l->data[i].level = 0;

        led *next, *previous;
        if (i != NB_LED_BAR_GPIO - 1) {
            next = &l->data[i + 1];
        }
        if (i > 0) {
            previous = &l->data[i - 1];
        }

        l->data[i].next_led = next;
        l->data[i].previous_led = previous;

        next = NULL;
        previous = NULL;
    }
    l->cur = &l->data[0];
    l->direction = FORWARD;
}

void switch_direction(leds *l) {
    for (size_t i = 0; i < NB_LED_BAR_GPIO; i++) {
        l->data[i].level = 0;

        led *tmp = l->data[i].next_led;

        l->data[i].next_led = l->data[i].previous_led;
        l->data[i].previous_led = tmp;

        tmp = NULL;
    }

    l->direction = !l->direction;
    l->cur = !l->direction ? &l->data[0] : &l->data[NB_LED_BAR_GPIO - 1];
    return;
}

void run_meteor(leds *l, size_t delay) {
    led *cur = l->cur;
    size_t max_level = MAX_DUTY_STEPS;

    while (cur != NULL) {
        set_led_level(cur, max_level);
        size_t level = max_level - DUTY_STEP;

        led *previous = cur->previous_led;
        while (previous != NULL && previous->level) {
            set_led_level(previous, level);
            previous = previous->previous_led;
            level -= DUTY_STEP;
        }

        _sleep(delay);
        print_led_bar_state(l);

        if (cur->next_led == NULL) {
            if (!cur->level) {
                break;
            }

            max_level -= DUTY_STEP;
            continue;
        }

        cur = cur->next_led;
    }
}

void print_led_bar_state(leds *l) {
    printf("* led bar state >> ");
    for (int i = 0; i < NB_LED_BAR_GPIO; i++) {
        char led_state[12];
        sprintf(led_state, "%ld (%.2ld)", l->data[i].gpio, l->data[i].level);
        if (i != NB_LED_BAR_GPIO - 1) {
            strcat(led_state, " | ");
        }
        printf("%s", led_state);
    }
    printf("\n");
}


void set_led_level(led *l, size_t level) {
    l->level = level;
    _set_pwm_gpio_level(l->gpio, level);
}

void breath_led(leds* l, size_t pos, size_t max_level, size_t step) {
#ifdef PICO
    pwm_set_wrap(l->data[pos].gpio, MAX_DUTY_STEPS);
#endif
    led* cur = &l->data[pos];

    size_t level = 0;
    short direction = 0;
    for(;;) {
        print_led_bar_state(l);
        set_led_level(cur, level);
        _sleep(50);

        if (!direction) {
            level += step;
            if (level > MAX_DUTY_STEPS) {
                level = MAX_DUTY_STEPS;
                direction = !direction;
            }
            continue;
        }

        level -= step;
        if (level <= 0) {
            level = 0;
            direction = !direction;
        }
    }
}

void run_wave(leds *l, size_t delay_ms) {
    const float twopi = 6.14;
    const float step = twopi / NB_LED_BAR_GPIO;

    float variation = 0.0;
    for (;;) {
        for (size_t led_idx = 0; led_idx < NB_LED_BAR_GPIO; led_idx++) {
            size_t level = _compute_wave_level(variation);
            set_led_level(&l->data[led_idx], level);
            variation += step;

            if (variation> twopi)
                variation = 0.0;
        }

        _sleep(delay_ms);
        print_led_bar_state(l);
    }
}