#include "config.h"
#ifdef ILLUMINATION
#include "illumination.h"
#include <ComPort.h>
#ifdef SUPPORT_EEPROM
    #include <EEPROM.h>
#else
    #include "dummyEEPROM.h"
#endif

const uint16_t eeprom_offset = 100;

const uint8_t pwm_led_pins[] = WIRING_PWM_LEDS;
const uint16_t cc_value_eeprom = eeprom_offset;
const uint16_t pwm_frequency_eeprom = cc_value_eeprom + sizeof(uint8_t);
const uint16_t pwm_values_eeprom = pwm_frequency_eeprom + sizeof(uint16_t);

uint32_t pwm_frequency;
uint8_t cc_value;

uint16_t pwm_values[PWM_NUM];


void illumination_pwm_freq(String command)
{
    char * args[1];
    uint8_t parsed = parse_arguments(args, command, 1);
    #ifdef MCU_PICO
        if (parsed > 0 && !(args[0] == "" || args[0][0] == '?'))
        {
            pwm_frequency = atol(args[0]);
            EEPROM.put(pwm_frequency_eeprom, pwm_frequency);
            analogWriteFreq(pwm_frequency);
            analogWriteRange(65535);
        }

        comPort->print("PWM Frequency:");
        comPort->print(pwm_frequency);
        comPort->println("Hz");
    #else
        comPort->println("Feature not supported");
    #endif

    if (parsed > 0)
        free(args[0]);
}

void illumination_channels(String command)
{
    comPort->print("CC:");
    #ifdef WIRING_CC_LED
        comPort->print(1);
    #else
        comPort->print(0);
    #endif
    comPort->print(" PWM:");
    comPort->println(PWM_NUM);
}

void illumination_pwm(String command)
{
    #ifdef MCU_PICO
    //PWM frequency is shared between both PWM outputs
    char * args[2];
    uint8_t parsed = parse_arguments(args, command, 2);
    if (parsed > 0 && args[0][0] == '?')
    {
        comPort->print("PWM:");
        for(int i=0; i<PWM_NUM; i++)
        {
            #ifdef MCU_PICO
                comPort->print(pwm_values[i]/65535.0);
            #else
                comPort->print(pwm_values[i]/255.0);
            #endif
            comPort->print(";");
        }
        comPort->println("");
        free(args[0]);
        if (parsed == 2)
            free(args[1]);

        return;
    }
    uint8_t index = atoi(args[0]);
    if (index >= PWM_NUM)
    {
        comPort->println("Invalid index");
    }
    float val = atof(args[1]);

    if (val < 0.00001)
    {
        digitalWrite(pwm_led_pins[index], LOW);
    }
    #ifdef MCU_PICO
        uint16_t pwm_val = val * 65535;
    #else
        uint8_t pwm_val = val * 255;
    #endif

    analogWrite(pwm_led_pins[index], pwm_val);
    pwm_values[index] = pwm_val;
    EEPROM.put(pwm_values_eeprom+index*sizeof(uint16_t), pwm_val);
    comPort->print("PWM:");
    comPort->print(index);
    comPort->print(":");
    comPort->println(pwm_val);

    if (parsed > 0)
        free(args[0]);
    if (parsed > 1)
        free(args[1]);
    #endif
}

void illumination_setup()
{
    for (uint8_t i = 0; i < PWM_NUM; i++)
    {
        pinMode(pwm_led_pins[i], OUTPUT);
        digitalWrite(pwm_led_pins[i], LOW);
    }

    #ifdef WIRING_CC_LED
        EEPROM.get(cc_value_eeprom, cc_value);
        pinMode(WIRING_CC_LED, OUTPUT);
        if (cc_value > 0)
            cc_set_value(cc_value/32.0);
    #endif

    #ifdef MCU_PICO
        EEPROM.get(pwm_frequency_eeprom, pwm_frequency);
        if (pwm_frequency < 1)
            pwm_frequency = 64000;
        analogWriteRange(65535);
        analogWriteFreq(pwm_frequency);//32khz seems sensible
    #endif

    for(int i = 0; i < PWM_NUM; i++)
    {
        EEPROM.get(pwm_values_eeprom+i*sizeof(uint16_t), pwm_values[i]);
        if (pwm_values[i] > 0)
            analogWrite(pwm_led_pins[i], pwm_values[i]);
    }

    register_module(illumination_commands, NULL);
}

#ifdef WIRING_CC_LED
//these functions are useful for testing
float multi_read(uint8_t pin, uint16_t reads)
{
    float total = 0;
    for(uint16_t i = 0; i<reads; i++)
        total += analogRead(pin);

    return total/reads;
}

void cc_test()
{
    analogReadResolution(12);
    cc_set_value(0);

    for (float v = 0; v <= 1; v+=0.02)
    {

        comPort->print(v);
        comPort->print(" ");
        cc_set_value(v);
        comPort->print(" ");
        delay(2000);
        float current = multi_read(A0, 1000)/4095.0*3.3;
        comPort->println(current*1000);
    }

    cc_set_value(0);
}
#endif

void cc_set(String command)
{
    char * args[1];
    uint8_t parsed = parse_arguments(args, command, 1);
    if (parsed > 0 && !(args[0] == "" || args[0][0] == '?'))
    {
        float val = atof(args[0]);
        cc_set_value(val);
    }

    comPort->print("CC LED:");
    comPort->println(cc_value/32.0);
    if (parsed > 0)
        free(args[0]);
}

void cc_set_value(float val)
{
    #ifdef WIRING_CC_LED
        if (val<0)
        {
            cc_test();
            return;
        }

        cc_value = 32 * val;
        EEPROM.put(cc_value_eeprom, cc_value);

        const uint8_t td = 3; // >= 1.5 us
        const uint8_t tup = 2; // 1-75 us
        const uint8_t tdown = 180; //180-300us
        const uint8_t tstart = 110; //100-150us

        //reset the device
        digitalWrite(WIRING_CC_LED, LOW);
        if (val < 0.02)
            return;

        uint8_t setting = val*32;
        delayMicroseconds(650); //reset device

        //start sequence
        digitalWrite(WIRING_CC_LED, HIGH);
        delayMicroseconds(tstart);

        uint8_t stepd = tup;
        uint8_t dir = 1;

        if (setting < 16)
        {
            dir = -1;
            stepd = tdown;
        }

        //step current
        uint8_t steps_done =0;
        for (uint8_t i=16; i!=setting; i+=dir)
        {
            digitalWrite(WIRING_CC_LED, LOW);
            delayMicroseconds(stepd);
            digitalWrite(WIRING_CC_LED, HIGH);
            delayMicroseconds(td);
            steps_done++;
        }
    #else
        comPort->println("CC LED not supported");
    #endif
}

extern const Command illumination_commands[] = {
    {"led_pwm", illumination_pwm},
    {"led_channels", illumination_channels},
    {"led_frequency", illumination_pwm_freq},
    {"led_cc", cc_set},
    END_COMMAND};
#endif