#include "treppe.h"


/*
    dimm_stufe
    - dimmt stufe (0 - 15, PCA9685 outputs) mit linearen ticks
    von idle bis active brightness
    - return false solange gedimmt wird
    - return true bei nächster stufe
*/
bool Treppe::dimm_stufe(uint8_t stufe)
{
    if (fsm_outputs.dimmrichtung == DR_AUFDIMMEN)
        current_pwm += differenz_pwm_pro_tick;
    else
        current_pwm -= differenz_pwm_pro_tick;
    pwmController.setChannelPWM(stufe, static_cast<uint16_t> (current_pwm));

    current_tick++;
    if (current_tick >= ticks_pro_stufe)
        return false;
    return true;
}

/*
    animation tick
    - nach dem dimmen einer stufe wird die stufe weitergezählt
    - abbruch am ende => anim_beendet = true;
*/
void Treppe::anim_tick()
{
    if (!dimm_stufe(stufe))
    {
        Serial.printf("anim_tick():  stufe: %d, start: %d, ziel: %d, current %f\n",
                        stufe, start_pwm, ziel_pwm, current_pwm);

        if (fsm_outputs.laufrichtung == LR_HOCH)
        {
            if (stufe >= stufen-1) {
                anim_beendet = true;
                return;
            }
            stufe++;
        }
        else
        {
            if (stufe <= 0) {
                anim_beendet = true;
                return;
            }
            stufe--;
        }
        current_tick = 0;
        current_pwm = start_pwm;
    }
}

// startbedingunen für animation
void Treppe::start_animation() {
    anim_beendet = false;

    if(fsm_outputs.laufrichtung == LR_HOCH)
        stufe = 0;
    else
        stufe = stufen-1;
    
    if(fsm_outputs.dimmrichtung == DR_AUFDIMMEN) {
        start_pwm = idle_brightness;
        ziel_pwm = active_brightness;
    } else {
        start_pwm = active_brightness;
        ziel_pwm = idle_brightness;
    }

    current_tick = 0;
    current_pwm = start_pwm;
}

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);
    fsm_inputs.ldr_schwelle = true; // <=== LDR implementierung !!

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

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

    if(fsm_outputs.status > ST_RUHEZUSTAND) {
        if( anim_beendet == true &&
            ( fsm_outputs.status == ST_AUFDIMMEN_HOCH || fsm_outputs.status == ST_ABDIMMEN_HOCH ||
            fsm_outputs.status == ST_AUFDIMMEN_RUNTER || fsm_outputs.status == ST_ABDIMMEN_RUNTER ))
        {
            start_animation();
        }
        if( !anim_beendet )
            anim_tick();
    }
}

void Treppe::berechne_dimmer() {
    ticks_pro_stufe = time_per_stair / INT_TIME; // [ms]
    differenz_pwm_pro_tick = (float) (active_brightness - idle_brightness) 
                                        / (float) ticks_pro_stufe;
}

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.printf("Treppe: initial parameters: stufen=%d\n", stufen);
}

// ###################################################################################################################
// GEBUFFERT => Erst im Ruhezustand übernehmen !!!!
void Treppe::set_idle_pwm(uint16_t _idle_brightness)
{
    idle_brightness = _idle_brightness;
    berechne_dimmer();
    Serial.printf("Treppe: idle_brightness=%d\n", idle_brightness);
}
void Treppe::set_active_pwm(uint16_t _active_brightness)
{
    active_brightness = _active_brightness;
    berechne_dimmer();
    Serial.printf("Treppe: active_brightness=%d\n", active_brightness);
}
void Treppe::set_time_per_stair(uint16_t _time_per_stair)
{
    time_per_stair = _time_per_stair;
    berechne_dimmer();
    Serial.printf("Treppe: time_per_stair=%d\n", time_per_stair);
}