#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_finished != last_in.anim_finished || 
    fsm_inputs.sensor_oben != last_in.sensor_oben || 
    fsm_inputs.sensor_unten != last_in.sensor_unten || 
    fsm_outputs.anim_active != last_out.anim_active
  ) {
    last_in.anim_finished = fsm_inputs.anim_finished;
    last_in.sensor_oben  = fsm_inputs.sensor_oben;
    last_in.sensor_unten = fsm_inputs.sensor_unten;
    last_out.anim_active = fsm_outputs.anim_active;

    Serial.printf("FSM inputs: s_u: %d, s_o: %d, an_fin: %d =>", 
      fsm_inputs.sensor_oben, fsm_inputs.sensor_unten, fsm_inputs.anim_finished);
    Serial.print(" step => ");
    Serial.printf("FSM outputs: an_act: %d\n", fsm_outputs.anim_active);
  }
}

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

  if(finish){
    direction = switch_direction;
    state = switch_state;
  }
  
  fsm_inputs.sensor_oben =  read_sensor(SENSOR_OBEN);
  fsm_inputs.sensor_unten = read_sensor(SENSOR_UNTEN);
  fsm_inputs.anim_finished = static_cast<bool>(finish);

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

  // 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");
  }
}