#include "treppe.h"
// #define DEBUG_TIMING

/*
    - dimmer_tick: increment pwm jeden tick, bis anim beendet
    - return: fsm_pend.anim_beendet
*/
bool Treppe::dimmer_tick(dimmer_t* dimmer, bool dim_type)
{
  dimmer->pwm += dimmer->delta_pwm;
  Serial.printf("%.0f", dimmer->pwm);

  if(dim_type == DIM_STUFEN) {
    pwmController.setChannelPWM(dimmer->stufe, static_cast<uint16_t>(dimmer->pwm));
  } else { // DIM_LDR
    pwmController.setAllChannelsPWM(static_cast<uint16_t>(dimmer->pwm));
  }

  dimmer->tick++;
  if (dimmer->tick < dimmer->ticks)
  {
    Serial.print("-");
    return false;
  }
  Serial.println("");
  
  if(dim_type == DIM_LDR) {
    Serial.printf("DIM_LDR: start: %d, ziel: %d\n",
           dimmer->start_pwm, dimmer->ziel_pwm);
    return true;
  }
  else // DIM_STUFEN
  {
    Serial.printf("DIM_STUFEN:  stufe: %d, start: %d, ziel: %d\n",
            dimmer->stufe, dimmer->start_pwm, dimmer->ziel_pwm);

    if (fsm_outputs.laufrichtung == LR_HOCH)
    {
      if (dimmer->stufe >= stufen - 1)
        return true;
      dimmer->stufe++;
    }
    else // LR_RUNTER
    {
      if (dimmer->stufe <= 0)
        return true;
      dimmer->stufe--;
    }
    dimmer->tick = 0;
    dimmer->pwm = dimmer->start_pwm;
  }
  return false;
}

// startbedingunen für animation
void Treppe::start_animation( dimmer_t* dimmer, bool dim_type,
                              uint16_t on_pwm, uint16_t off_pwm)
{
  fsm_pend.anim_beendet = false;

  if(dim_type == DIM_STUFEN) {
    if (fsm_outputs.laufrichtung == LR_HOCH)
      dimmer->stufe = 0;
    else
      dimmer->stufe = stufen - 1;

    dimmer->ticks = time_per_stair / INT_TIME; // [ms]
  } 
  else { // DIM_LDR
    dimmer->ticks = time_ldr / INT_TIME; // [ms]
  }

  if (fsm_outputs.dimmrichtung == DR_AUFDIMMEN)
  {
    dimmer->start_pwm = off_pwm;
    dimmer->ziel_pwm = on_pwm;  
    dimmer->delta_pwm = (float)(on_pwm - off_pwm) 
                          / (float)dimmer->ticks;
  }
  else
  {
    dimmer->start_pwm = on_pwm;
    dimmer->ziel_pwm = off_pwm;    
    dimmer->delta_pwm = (float)(off_pwm - on_pwm) 
                          / (float)dimmer->ticks;
  }

  dimmer->tick = 0;
  dimmer->pwm = dimmer->start_pwm;

  Serial.printf("stufe %d, ticks %d, delta %f, start %d, ziel %d\n",
    dimmer->stufe, dimmer->ticks, dimmer->delta_pwm, dimmer->start_pwm, dimmer->ziel_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_inputs.ldr_schwelle != last_in.ldr_schwelle ||
      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_in.ldr_schwelle = fsm_inputs.ldr_schwelle;
    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, a_b: %d, l_s: %d => ",
                  fsm_inputs.sensor_oben, fsm_inputs.sensor_unten, 
                  fsm_inputs.anim_beendet, fsm_inputs.ldr_schwelle);
    Serial.printf("OUT: LR: %d DR: %d ST: %d\n",
                  fsm_outputs.laufrichtung, fsm_outputs.dimmrichtung, fsm_outputs.status);
  }
}

bool Treppe::read_sensor(int sensor)
{

  /*
  reads sensors with edge detection
  
  returns true if motion was detected
  returns false if no motion was detected
  returns false if motion was detected, but state did not change back to not detected
  */
  uint8_t pegel = digitalRead(sensor);
  static uint8_t pegel_alt[2] = {0, 0};

  uint8_t index = 0;
  if (sensor == SENSOR_OBEN)
    index = 0;
  else
    index = 1;

  if (pegel == 1 && pegel_alt[index] == 0)
  {
    pegel_alt[index] = pegel;
    return true;
  }
  else
  {
    pegel_alt[index] = pegel;
    return false;
  }
  //return static_cast<bool>(pegel);
}

float Treppe::read_ldr()
{
  /* 
  Reads Illuminance in Lux

  FUTURE USE : show current Illuminance on Webserver in order to calibrate

  Voltage Divider 1 (R13, R14):
  R13 = 220k, R14 = 82k
  V(ADC) = V(in1) * R14/(R13+R14)
  -> V(in1) = V(ADC) * (R13+R14)/R14
  V(ADC) = analogRead(A0)/1023.00
  -> V(in1) = analogRead(A0)/1023.00 * (R13+R14)/R14
            = analogRead(A0) * (R13+R14)/(R14*1023.00)
            = analogRead(A0) * (220k+82k)/(82k*1023.00)
            = analogRead(A0) * 0.0036
  
  Voltage Divider 2 (LDR, R1 || (R13+R14))
  R1 = 47k, R13+R14 = 302k -> R1||(R13+R14) = 40,67k
  Vcc/V(in1) = R(LDR) / (R1||(R13+R14))
  -> R(LDR) = Vcc/V(in1) * (R1||(R13+R14))
  R(LDR) = 3.3V * 40.67k / V(in1)

  Join formulas:

  R(LDR) = 3.3V * 40.67k / (0.0036 * analogRead(A0))
         = 37280.00/analogRead(A0)
  ldr_ohm = R(LDR)

  E(LDR) = 6526.5 * R(LDR)^-2   (see Excel Regression)
  E(LDR) = 6526.5 / (R(LDR)^2)
  ldr_value = E(LDR)
  */
  float ldr_ohm = 37280.00 / analogRead(A0);
  float ldr_value = 6526.6/(ldr_ohm*ldr_ohm);
  return ldr_value;
}

bool Treppe::check_ldr()
{
  static uint8_t active = 0;

#ifdef LDRDEBUG
  Serial.printf("R(LDR) = %f kOhm  %f lux\n", ldr_value, lux);
  return true;
#endif

  // follow up: averaging over many samples?
  float ldr = read_ldr();

  if (ldr < ldr_schwelle) {
    active = 1;
  }
  if (ldr > ldr_schwelle + LDR_HYS) {
    active = 0;
  }
  return active;
}

void Treppe::task()
{  
#ifdef DEBUG_TIMING
  uint32_t m=micros();
#endif

  fsm_inputs.ldr_schwelle = check_ldr();

#ifdef DEBUG_TIMING
  Serial.print("1:");
  Serial.println(micros()-m);
  m=micros();
#endif

  fsm_inputs.sensor_oben = read_sensor(SENSOR_OBEN);
  fsm_inputs.sensor_unten = read_sensor(SENSOR_UNTEN);
  fsm_inputs.anim_beendet = fsm_pend.anim_beendet;

#ifdef DEBUG_TIMING
  Serial.print("2:");
  Serial.println(micros()-m);
  m=micros();
#endif

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

#ifdef DEBUG_TIMING
  Serial.print("3:");
  Serial.println(micros()-m);
  m=micros();
#endif

  print_state_on_change();

#ifdef DEBUG_TIMING
  Serial.print("4:");
  Serial.println(micros()-m);
  m=micros();
#endif

  if( fsm_outputs.status == ST_AUFDIMMEN_HOCH || 
      fsm_outputs.status == ST_ABDIMMEN_HOCH  ||
      fsm_outputs.status == ST_AUFDIMMEN_RUNTER || 
      fsm_outputs.status == ST_ABDIMMEN_RUNTER )
  {
    if(fsm_pend.anim_beendet)
      start_animation(&dimmer_stufen, DIM_STUFEN, active_pwm, idle_pwm_ist);
    else
      fsm_pend.anim_beendet = dimmer_tick(&dimmer_stufen, DIM_STUFEN);
  }
  else if ( fsm_outputs.status == ST_AUFDIMMEN_LDR ||
            fsm_outputs.status == ST_ABDIMMEN_LDR )
  {
    if(fsm_pend.anim_beendet)
      start_animation(&dimmer_ldr, DIM_LDR, idle_pwm_ist, 0);
    else
      fsm_pend.anim_beendet = dimmer_tick(&dimmer_ldr, DIM_LDR);
  }
  else if ( fsm_outputs.status == ST_RUHEZUSTAND ) 
  {
    if ( fsm_pend.ldr_changed ) {
      fsm_pend.ldr_changed = false;
      fsm_outputs.dimmrichtung = DR_AUFDIMMEN;
      start_animation(&dimmer_ldr, DIM_LDR, idle_pwm_soll, idle_pwm_ist);
      idle_pwm_ist = idle_pwm_soll;
    }
    if(!fsm_pend.anim_beendet) {
      fsm_pend.anim_beendet = dimmer_tick(&dimmer_ldr, DIM_LDR);
    }
  }


#ifdef DEBUG_TIMING
  Serial.print("5:");
  Serial.println(micros()-m);
#endif
}

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_pwm);

  pinMode(13, OUTPUT);
  pinMode(0, OUTPUT);
  digitalWrite(13, HIGH);
  digitalWrite(0, HIGH);

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

  Serial.printf("Treppe: stufen=%d\n", stufen);
}

void Treppe::set_idle_prozent(const int prozent) 
{
  uint16_t new_pwm = active_pwm * prozent / 100;
  set_idle_pwm_max(new_pwm);
}

void Treppe::set_idle_pwm_max(const uint16_t new_pwm)
{
  if(new_pwm > active_pwm) {
    idle_pwm_soll = active_pwm;
  } else {
    idle_pwm_soll = new_pwm;
  }

  Serial.printf("Treppe: idle_pwm_soll=%d\n", idle_pwm_soll);
  fsm_pend.ldr_changed = true;
}

void Treppe::set_active_pwm(uint16_t _active_pwm)
{
  active_pwm = _active_pwm;
  Serial.printf("Treppe: active_pwm=%d\n", active_pwm);
}
void Treppe::set_time_per_stair(uint16_t _time_per_stair)
{
  time_per_stair = _time_per_stair;
  Serial.printf("Treppe: time_per_stair=%d\n", time_per_stair);
}