#include "treppe.h"

uint8_t Treppe::softstart_led(uint8_t led, uint16_t startval, uint16_t stopval)
{
    /*
  softstart task

  - get's called at regular intervals (1ms at the moment)
  - dimms single led (0 - 15, PCA9685 outputs) with linear intervals vom startval to stopval
  - calculates pwm steps depending on startval, stopval and timeinterval
  - -> results in constanst speed
  - returns 1 if led dimming is running
  - returns 0 if led dimming is finished

  */

    static uint8_t lastled = 255;
    static float current_pwm = 0;
    static float stepsize = 1.0;
    if (led != lastled)
    {
        pwmController.setChannelPWM(led, (uint16_t)startval);
        lastled = led;
        current_pwm = startval;
        stepsize = INT_TIME * abs(stopval - startval) / (float)time_per_stair; // only valid at 1ms function call interval
        return 1;
    }

    if (startval > stopval)
    {
        current_pwm -= stepsize;
    }
    else
    {
        current_pwm += stepsize;
    }
    // Serial.println((uint16_t)current_pwm);
    pwmController.setChannelPWM(led, (uint16_t)current_pwm);
    if (current_pwm > stopval - stepsize && current_pwm < stopval + stepsize)
    {
        if (stopval == 0)
            pwmController.setChannelPWM(led, 0);
        return 0;
    }
    return 1;
}

void Treppe::ledsequence()
{
    static int8_t led = 0;
    static uint16_t brightness = 0;
    static uint16_t lastbrightness = 0;
    static uint16_t status = 0;
    uint16_t status_build = 0;
    status_build |= direction << 8;
    status_build |= state;
    if (status_build != status)
    {               // check if any parameter changed
        finish = 0; // set state unfinished -> start action
        if (direction)
            led = 0; // reset led counter depending of direction
        else
            led = stairs - 1;
        if (state)
        {
            brightness = active_brightness; // set brightness value depending of on/off
            lastbrightness = idle_brightness;
        }
        else
        {
            brightness = idle_brightness;
            lastbrightness = active_brightness;
        }
        status = status_build; // set parameter memory

        Serial.printf("----Status Changed! onoff: %d, dir: %d\n", state, direction);
    }
    if (!finish)
    { // finish == 0 -> action pending
        if (!softstart_led(led, lastbrightness, brightness))
        {
            Serial.printf("one LED finished:  led: %d, last: %d, curr %d\n",
                          led, lastbrightness, brightness);

            if (direction)
            {
                led++;
                if (led >= stairs)
                    finish = 1;
            }
            else
            {
                led--;
                if (led < 0)
                    finish = 1;
            }
        }
    }
}

void Treppe::rampe()
{
    if (state)
    {
        finish = 0;
        state = 0; // set parameter memory
    }

    if (!finish)
    {
        if (direction)
        { // aufwärts
            if (tick >= ticks_treppe - 1)
            { // ziel erreicht
                Serial.println("[Treppe] oberster tick !");
                finish = 1;
                return;
            }
            tick++; // eins hoch
        }
        else
        { // abwärts
            if (tick <= 0)
            { // ziel erreicht
                Serial.println("[Treppe] unterster tick !");
                finish = 1;
                return;
            }
            tick--; // eins runter
        }

        stufe = tick / ticks_pro_stufe;

        float new_pwm = 0.0;
        if (an_aus)
        {
            new_pwm = differenz_pwm_pro_tick * (tick - ticks_pro_stufe * stufe);
            new_pwm += idle_brightness;
            if (direction)
                new_pwm += differenz_pwm_pro_tick;
        }
        else
        {
            new_pwm = active_brightness - differenz_pwm_pro_tick * (tick - ticks_pro_stufe * stufe);
            new_pwm += idle_brightness;
            if (direction)
                new_pwm -= differenz_pwm_pro_tick;
        }

        pwmController.setChannelPWM(stufe, (uint16_t)new_pwm);
        Serial.printf("tick %04u, led %02d:%02u, pwm %4.1f\n",
                      tick,
                      stufe,
                      (tick - ticks_pro_stufe * stufe),
                      new_pwm);
    }
}

void Treppe::setup()
{

    pwmController.resetDevices();
    // Deactive PCA9685 Phase Balancer due to LED Flickering
    // https://github.com/NachtRaveVL/PCA9685-Arduino/issues/15
    // see also lib/PCA9685-Arduin/PCA9685.h:204
    pwmController.init(PCA9685_PhaseBalancer_None);
    //pwmController.init(PCA9685_PhaseBalancer_Linear);
    pwmController.setPWMFrequency(100);
    pwmController.setAllChannelsPWM(idle_brightness);

    pinMode(A0, INPUT);
    pinMode(SENSOR_OBEN, INPUT);
    pinMode(SENSOR_UNTEN, INPUT);
    pinMode(OE, OUTPUT);
    digitalWrite(OE, 0);

    Serial.printf("differenz_pwm_pro_tick %f\n", differenz_pwm_pro_tick);
    Serial.println("Hello from Treppe");
    Serial.print("Treppe: initial parameters: stairs=");
    Serial.println(stairs);
}

void Treppe::print_state_on_change()
{
    static FSMTreppeModelClass::ExtU_FSMTreppe_T last_in;
    static FSMTreppeModelClass::ExtY_FSMTreppe_T last_out;
    if (
        fsm_inputs.anim_beendet != last_in.anim_beendet ||
        fsm_inputs.sensor_oben != last_in.sensor_oben ||
        fsm_inputs.sensor_unten != last_in.sensor_unten ||
        fsm_outputs.dimmrichtung != last_out.dimmrichtung ||
        fsm_outputs.laufrichtung != last_out.laufrichtung ||
        fsm_outputs.status != last_out.status)
    {
        last_in.anim_beendet = fsm_inputs.anim_beendet;
        last_in.sensor_oben = fsm_inputs.sensor_oben;
        last_in.sensor_unten = fsm_inputs.sensor_unten;
        last_out.dimmrichtung = fsm_outputs.dimmrichtung;
        last_out.laufrichtung = fsm_outputs.laufrichtung; 
        last_out.status = fsm_outputs.status;

        Serial.printf("FSM IN: s_u: %d, s_o: %d, beendet: %d =>",
                      fsm_inputs.sensor_oben, fsm_inputs.sensor_unten, fsm_inputs.anim_beendet);
        Serial.print(" step => ");
        Serial.printf("OUT: LR: %d DR: %d ST: %d\n",
                      fsm_outputs.laufrichtung, fsm_outputs.dimmrichtung, fsm_outputs.status);
    }
}

void Treppe::task()
{
    //Serial.printf("LDR: %f\n", ((float)analogRead(A0))/1023.*3.68);

    if (finish)
    {
        direction = switch_direction;
        state = switch_state;
    }

    fsm_inputs.ldr_schwelle = true;
    fsm_inputs.sensor_oben = read_sensor(SENSOR_OBEN);
    fsm_inputs.sensor_unten = read_sensor(SENSOR_UNTEN);
    fsm_inputs.anim_beendet = static_cast<bool>(finish);

    FSMTreppe_Obj.setExternalInputs(&fsm_inputs);
    FSMTreppe_Obj.step();
    fsm_outputs = FSMTreppe_Obj.getExternalOutputs();
    print_state_on_change();

    direction = fsm_outputs.laufrichtung;
    state = fsm_outputs.dimmrichtung;

    // setTick(ticks_treppe);
    // setAnAus(1);
    // setDirection(0);
    // setState(0);

    ledsequence();
}

uint16_t Treppe::setIdle(uint16_t _idle_brightness)
{
    idle_brightness = _idle_brightness;
    Serial.println("Treppe: idle brightness changed!");
    return idle_brightness;
}
uint16_t Treppe::setActive(uint16_t _active_brightness)
{
    active_brightness = _active_brightness;
    Serial.println("Treppe: active brightness changed!");
    return active_brightness;
}
uint16_t Treppe::setTime(uint16_t _time_per_stair)
{
    time_per_stair = _time_per_stair;
    Serial.println("Treppe: time changed!");
    return time_per_stair;
}

void Treppe::setDirection(uint8_t _direction)
{
    switch_direction = _direction;
    Serial.printf("Treppe: switch_direction=%d!\n", switch_direction);
    if (finish)
        Serial.println("apply direction request immediately");
    else
        Serial.println("currently active, dir change afterwards");
    // to do: implement state command variable to determine dimm-state
}

void Treppe::setState(uint8_t _state)
{
    if (state == _state)
        return;
    else
    {
        switch_state = _state;
        Serial.printf("Treppe: switch_state=%d!\n", switch_state);
        if (finish)
            Serial.println("apply state request immediately");
        else
            Serial.println("currently active, state changes after activity");
    }
}