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

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

  dimmer_stufe.tick++;
  if (dimmer_stufe.tick >= dimmer_stufe.ticks)
  {
    Serial.println("");
    return false;
  }
  Serial.print(" - ");
  return true;
}


/*
    - dimmt treppe (all PCA9685 outputs) mit linearen ticks
    von idle bis active brightness
    - return false solange gedimmt wird
    - return true bei ende
*/
bool Treppe::dimm_treppe()
{
  // needs to be in state machine
  return true;
}


/*
    - nach dem dimmen einer stufe wird die stufe weitergezählt
    - abbruch am ende => anim_beendet = true;
*/
void Treppe::anim_tick()
{
  if (!dimm_stufe(dimmer_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 (dimmer_stufe.stufe >= stufen - 1)
      {
        anim_beendet = true;
        return;
      }
      dimmer_stufe.stufe++;
    }
    else
    {
      if (dimmer_stufe.stufe <= 0)
      {
        anim_beendet = true;
        return;
      }
      dimmer_stufe.stufe--;
    }
    dimmer_stufe.tick = 0;
    dimmer_stufe.pwm = dimmer_stufe.start_pwm;
  }
}

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

  if (fsm_outputs.laufrichtung == LR_HOCH)
    dimmer_stufe.stufe = 0;
  else
    dimmer_stufe.stufe = stufen - 1;

  if (fsm_outputs.dimmrichtung == DR_AUFDIMMEN)
  {
    dimmer_stufe.start_pwm = idle_pwm_ist;
    dimmer_stufe.ziel_pwm = active_pwm;
  }
  else
  {
    dimmer_stufe.start_pwm = active_pwm;
    dimmer_stufe.ziel_pwm = idle_pwm_ist;
  }

  dimmer_stufe.tick = 0;
  dimmer_stufe.pwm = dimmer_stufe.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_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) {
    idle_pwm_soll = idle_pwm_max;
    active = true;
  }
  if (ldr > ldr_schwelle + LDR_HYS) {
    idle_pwm_soll = 0;
    active = false;
  }

  if (idle_pwm_soll != idle_pwm_ist) {
  }
  activate_idle_pwm(active);
  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 = 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(anim_beendet)
      start_animation();
    else
      anim_tick();
  }

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


  // else if (fsm_outputs.status == ST_DIMMEN_LDR) {
  //   if(anim_beendet) {
  //     berechne_dimmer();
  //     anim_beendet = false;
  //   }
  //   anim_beendet = dimm_treppe();
  // }

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

void Treppe::berechne_dimmer()
{
  dimmer_stufe.ticks = time_per_stair / INT_TIME; // [ms]
  dimmer_stufe.delta_pwm = (float)(active_pwm - idle_pwm_ist) 
                                / (float)dimmer_stufe.ticks;

  dimmer_ldr.ticks = time_ldr / INT_TIME; // [ms]
  dimmer_ldr.delta_pwm = (float)(idle_pwm_soll - idle_pwm_ist) 
                                / (float)dimmer_ldr.ticks;
}

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(A0, INPUT);
  pinMode(SENSOR_OBEN, INPUT);
  pinMode(SENSOR_UNTEN, INPUT);
  pinMode(OE, OUTPUT);
  digitalWrite(OE, 0);

  Serial.printf("dimmer_stufe.delta_pwm %f\n", dimmer_stufe.delta_pwm);
  Serial.printf("Treppe: initial parameters: 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_max = active_pwm;
  } else {
    idle_pwm_max = new_pwm;
  }

  Serial.printf("Treppe: idle_pwm_max=%d\n", idle_pwm_max);
  berechne_dimmer();
  activate_idle_pwm(true);
}

void Treppe::activate_idle_pwm(bool active)
{
  static uint16_t last_pwm = 0;

  if (fsm_outputs.status == ST_RUHEZUSTAND || fsm_outputs.status == ST_INAKTIV_LDR)
  {
    idle_pwm_ist = idle_pwm_max * active;
    if (idle_pwm_ist != last_pwm)
    {
      // Dimming Function for all LEDS ?
      berechne_dimmer();
      pwmController.setAllChannelsPWM(idle_pwm_ist);
      last_pwm = idle_pwm_ist;
    }
  }
}

void Treppe::set_active_pwm(uint16_t _active_pwm)
{
  active_pwm = _active_pwm;
  berechne_dimmer();
  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;
  berechne_dimmer();
  Serial.printf("Treppe: time_per_stair=%d\n", time_per_stair);
}