#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;
  
#ifdef DIMMDEBUG
  Serial.printf("%.0f", dimmer->pwm);
#endif

  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) {
#ifdef DIMMDEBUG
    Serial.print("-");
#endif
    return false;
  }
#ifdef DIMMDEBUG
  Serial.println("");
#endif

  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 = param.time_per_stair / INT_TIME; // [ms]
  } else {                                           // DIM_LDR
    dimmer->ticks = param.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;

#ifndef LDR_REGELUNG
  idle_pwm_ist = param.idle_pwm_max;
#endif

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

void Treppe::overwrite_sensors(bool s_oben, bool s_unten) {
  fsm_pend.web_ctrl_s_oben = s_oben;
  fsm_pend.web_ctrl_s_unten = s_unten;
}

void Treppe::read_sensors() {
  const bool s_oben = digitalRead(SENSOR_OBEN);
  const bool s_unten = digitalRead(SENSOR_UNTEN);

  fsm_pend.sensor_oben = false;
  fsm_pend.sensor_unten = false;

  // rising trigger => 1 cycle true !
  if (s_oben && !fsm_pend.last_s_oben) {
    fsm_pend.sensor_oben = true;
  }
  if (s_unten && !fsm_pend.last_s_unten) {
    fsm_pend.sensor_unten = true;
  }

  fsm_pend.last_s_oben = s_oben;
  fsm_pend.last_s_unten = s_unten;

  // check for manipulation over webserver
  if (fsm_pend.web_ctrl_s_oben) {
    fsm_pend.sensor_oben = true;
    fsm_pend.web_ctrl_s_oben = false;
  }
  if (fsm_pend.web_ctrl_s_unten) {
    fsm_pend.sensor_unten = true;
    fsm_pend.web_ctrl_s_unten = false;
  }
}

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 vol_adc = analogRead(A0) * 0.0036;
  if(vol_adc > 3.29) // prevent negative values !
    vol_adc = 3.29;
  float ldr_ohm = 40.67 * (3.3 - vol_adc) / vol_adc;
  float ldr_value = 6526.6 / (ldr_ohm * ldr_ohm);
#ifdef LDRDEBUG
  Serial.printf("vol_adc: %f Ohm: %f lux: %f Comp: %d\n", vol_adc, ldr_ohm, ldr_value,
                param.ldr_schwelle);
#endif
  return ldr_value;
}

void Treppe::sample_ldr() {
  ldr_average += read_ldr();
  ldr_average_cnt++;
  if(ldr_average_cnt > LDR_AVERAGE_SAMPLES) {
    float ldr_avg = static_cast<float>(ldr_average / LDR_AVERAGE_SAMPLES);
    Serial.printf("ldr_avg: %f schwelle: %d\n", ldr_avg, param.ldr_schwelle);
    if(check_ldr(ldr_avg)) {
      update_soll_pwm_with_ldr(ldr_avg);
    }
    ldr_average = 0.0;
    ldr_average_cnt = 0;
  }
}

void Treppe::update_soll_pwm_with_ldr(float ldr_avg) {
  // LDR quasi linear :)
/*
  soll     ldr_average
  ----  =  ------------
  ist      ldr_schwelle
*/

#ifdef LDR_REGELUNG
  if(ldr_avg >= param.ldr_schwelle) {
    return;
  }
  idle_pwm_soll = (1 - (ldr_avg / param.ldr_schwelle)) * param.idle_pwm_max;
  Serial.printf("Update idle_pwm_soll_with_ldr: %d\n", idle_pwm_soll);

  if(idle_pwm_ist != idle_pwm_soll)
    fsm_inputs.ldr_changed = true;
#endif
}

bool Treppe::check_ldr(float ldr_avg) {
  static uint8_t active = 0;
  
  if (ldr_avg < param.ldr_schwelle) {
    active = 1;
  }
  if (ldr_avg > param.ldr_schwelle + LDR_HYS) {
    active = 0;
  }
  fsm_inputs.ldr_schwelle = active;
  return active;
}

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


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

  read_sensors();
  fsm_inputs.sensor_oben = fsm_pend.sensor_oben;
  fsm_inputs.sensor_unten = fsm_pend.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, param.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) {
#ifdef LDR_REGELUNG
      start_animation(&dimmer_ldr, DIM_LDR, idle_pwm_soll, 0);
      idle_pwm_ist = idle_pwm_soll;
#else
      start_animation(&dimmer_ldr, DIM_LDR, idle_pwm_ist, 0);
#endif
    }
    else
      fsm_pend.anim_beendet = dimmer_tick(&dimmer_ldr, DIM_LDR);
  }
  else if(fsm_outputs.status == ST_LDR_CHANGED) {
    fsm_inputs.ldr_changed = false;

#ifdef LDR_REGELUNG
    if (fsm_pend.anim_beendet) {
      if(idle_pwm_soll < idle_pwm_ist) {
        fsm_outputs.dimmrichtung = DR_ABDIMMEN;
        start_animation(&dimmer_ldr, DIM_LDR, idle_pwm_ist, idle_pwm_soll);
      }
      else {
        fsm_outputs.dimmrichtung = DR_AUFDIMMEN;
        start_animation(&dimmer_ldr, DIM_LDR, idle_pwm_soll, idle_pwm_ist);
      }
      idle_pwm_ist = idle_pwm_soll;
    }
    else { 
      fsm_pend.anim_beendet = dimmer_tick(&dimmer_ldr, DIM_LDR);
    }
#endif
  }

  if (fsm_outputs.status == ST_RUHEZUSTAND ||
      fsm_outputs.status == ST_INAKTIV_LDR) {
    if (param_changed) {
      param_changed = false;
      param = param_pend;
      save_param_to_eeprom();
      Serial.printf("Parameter Change applied!\n");
    }
  }

#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(200);
  // pwmController.setAllChannelsPWM(idle_pwm);

  // WARNING: before getting Parameters of Flash, make sure plausible parameters
  // are written in flash!
  EEPROM.get(EEP_START_ADDR, param); // get Parameters of flash
  param_pend = param;
#ifndef LDR_REGELUNG
  idle_pwm_ist = param.idle_pwm_max;
#endif

  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::save_param_to_eeprom() {
  EEPROM.put(EEP_START_ADDR,
             param); // 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) {
    param_pend.idle_pwm_max = param_pend.active_pwm * value / 100;
  } else if (vorgabe_typ == VORGABE_12BIT) {
    param_pend.idle_pwm_max = value;
  }

  if (param_pend.idle_pwm_max > param_pend.active_pwm) {
    param_pend.idle_pwm_max = param_pend.active_pwm;
  }
  param_changed = true;

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

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

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

  if (param_pend.active_pwm > 4095) {
    param_pend.idle_pwm_max = 4095;
  }
  param_changed = true;

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

void Treppe::set_time_ldr(const uint16_t value) {
  param_pend.time_ldr = value;
  if (param_pend.time_ldr > TIME_MS_MAX)
    param_pend.time_ldr = TIME_MS_MAX;
  param_changed = true;

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

void Treppe::set_time_per_stair(const uint16_t value) {
  param_pend.time_per_stair = value;
  if (param_pend.time_per_stair > TIME_MS_MAX)
    param_pend.time_per_stair = TIME_MS_MAX;
  param_changed = true;

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

void Treppe::set_ldr_schwelle(const uint16_t value,
                              const vorgabe_typ_t vorgabe_typ) {
  if (vorgabe_typ == VORGABE_PROZENT) {
    // ?!
    param_pend.ldr_schwelle = 10 * value / 100;
  } else if (vorgabe_typ == VORGABE_12BIT) {
    param_pend.ldr_schwelle = value;
  }
  param_changed = true;

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

void Treppe::param_to_json(char *json_str, size_t sz) {
  snprintf(json_str, sz, "{\n\
    \"time_ldr\": %d,\n\
    \"time_per_stair\": %d,\n\
    \"idle_pwm_max\": %d,\n\
    \"active_pwm\": %d,\n\
    \"ldr_schwelle\": %d\n}\n",
           param.time_ldr, param.time_per_stair, param.idle_pwm_max,
           param.active_pwm, param.ldr_schwelle);
}