#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 = parameters.time_per_stair / INT_TIME; // [ms]
  } else {                                                // DIM_LDR
    dimmer->ticks = parameters.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 < parameters.ldr_schwelle) {
    active = 1;
  }
  if (ldr > parameters.ldr_schwelle + LDR_HYS) {
    active = 0;
  }
  return active;
}

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

  // TODO wenn LDR geändert => idle_pwm_soll anpassen 
  // fsm_pend.ldr_changed = true;

  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, parameters.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);
  
  // WARNING: before getting Parameters of Flash, make sure plausible parameters are written in flash!
  EEPROM.get(EEP_START_ADDR, parameters);     // get Parameters of flash
  
  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::saveParam(){
  EEPROM.put(EEP_START_ADDR, parameters); // copy Parameters so "EEPROM"-section in RAM
  EEPROM.commit();                        // write "EEPROM"-section to flash
}

void Treppe::set_idle_pwm_max(const uint16_t value,
                              const vorgabe_typ_t vorgabe_typ) {
  if (vorgabe_typ == VORGABE_PROZENT) {
    parameters.idle_pwm_max = parameters.active_pwm * value / 100;
  } else if (vorgabe_typ == VORGABE_12BIT) {
    parameters.idle_pwm_max = value;
  }

  if (parameters.idle_pwm_max > parameters.active_pwm) {
    parameters.idle_pwm_max = parameters.active_pwm;
  }
  saveParam();
  Serial.printf("Treppe: parameters.idle_pwm_max=%d\n",
                parameters.idle_pwm_max);
}

void Treppe::set_active_pwm(const uint16_t value,
                            const vorgabe_typ_t vorgabe_typ) {

  if (vorgabe_typ == VORGABE_PROZENT) {
    parameters.active_pwm = 4095 * value / 100;
  } else if (vorgabe_typ == VORGABE_12BIT) {
    parameters.active_pwm = value;
  }

  if (parameters.active_pwm > 4095) {
    parameters.idle_pwm_max = 4095;
  }
  saveParam();
  Serial.printf("Treppe: parameters.active_pwm=%d\n", parameters.active_pwm);
}

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