#include "../PluginStructs/P098_data_struct.h"

#ifdef USES_P098

# include "../ESPEasyCore/ESPEasyGPIO.h"

# include "../Commands/GPIO.h" // FIXME TD-er: Only needed till we can call GPIO commands from the ESPEasy core.


const __FlashStringHelper * P098_config_struct::toString(P098_config_struct::PWM_mode_type PWM_mode) {
  switch (PWM_mode) {
    case P098_config_struct::PWM_mode_type::NoPWM:  return F("No PWM");
    case P098_config_struct::PWM_mode_type::PWM:    return F("PWM");

    case P098_config_struct::PWM_mode_type::MAX_TYPE: break;
  }

  return F("");
}

P098_data_struct::P098_data_struct(const P098_config_struct& config) : _config(config) {}

P098_data_struct::~P098_data_struct() {
  if (initialized) {
    detachInterrupt(digitalPinToInterrupt(_config.limitA.gpio));
    detachInterrupt(digitalPinToInterrupt(_config.limitB.gpio));
    detachInterrupt(digitalPinToInterrupt(_config.encoder.gpio));
  }
}

bool P098_data_struct::begin(int pos, int limitApos, int limitBpos)
{
  if (!initialized) {
    initialized = true;

    const bool switchPosSet = pos != 0 && limitApos != 0;

    limitA.switchposSet = switchPosSet;
    limitA.switchpos    = switchPosSet ? limitApos : 0;
    limitB.switchpos    = limitBpos;
    position            = pos + limitA.switchpos;
    stop();
    state = P098_data_struct::State::Idle;

    if (setPinMode(_config.limitA)) {
      attachInterruptArg(
        digitalPinToInterrupt(_config.limitA.gpio),
        reinterpret_cast<void (*)(void *)>(ISRlimitA),
        this, CHANGE);
    }

    if (setPinMode(_config.limitB)) {
      attachInterruptArg(
        digitalPinToInterrupt(_config.limitB.gpio),
        reinterpret_cast<void (*)(void *)>(ISRlimitB),
        this, CHANGE);
    }

    if (setPinMode(_config.encoder)) {
      attachInterruptArg(
        digitalPinToInterrupt(_config.encoder.gpio),
        reinterpret_cast<void (*)(void *)>(ISRencoder),
        this, CHANGE); // Act on 'CHANGE', not on rising/falling
    }
  }

  return true;
}

bool P098_data_struct::loop()
{
  const State old_state(state);

  check_limit_switch(_config.limitA, limitA);
  check_limit_switch(_config.limitB, limitB);

  if (check_encoder_timeout(_config.encoder)) {
    stop();
    state = P098_data_struct::State::StopEncoderTimeout;  
    return old_state == state;
  }

  switch (state) {
    case P098_data_struct::State::Idle:
      return true;
    case P098_data_struct::State::RunFwd:
    {
      updatePosition();
      checkLimit(limitB);
      break;
    }
    case P098_data_struct::State::RunRev:
    {
      updatePosition();
      checkLimit(limitA);
      break;
    }
    case P098_data_struct::State::StopEncoderTimeout:
    case P098_data_struct::State::StopLimitSw:
    case P098_data_struct::State::StopPosReached:
      // Still in a state that needs inspection
      return false;
  }

  // Must check when state has changed from running to some other state
  return old_state == state;
}

bool P098_data_struct::homePosSet() const
{
  return limitA.switchposSet;
}

bool P098_data_struct::canRun()
{
  if (checkValidPortRange(PLUGIN_GPIO, _config.limitA.gpio) && !homePosSet()) { return false; }

  switch (state) {
    case P098_data_struct::State::Idle:
    case P098_data_struct::State::RunFwd:
    case P098_data_struct::State::RunRev:
      return true;
    case P098_data_struct::State::StopLimitSw:
    case P098_data_struct::State::StopPosReached:
    case P098_data_struct::State::StopEncoderTimeout:
      // Still in a state that needs inspection
      return false;
  }
  return false;
}

void P098_data_struct::findHome()
{
  pos_dest            = INT_MIN;
  limitA.switchposSet = false;
  startMoving();
}

void P098_data_struct::moveForward(int steps)
{
  if (steps <= 0) {
    pos_dest            = INT_MAX;
    limitB.switchposSet = false;
  } else {
    pos_dest = position + steps;
  }
  startMoving();
}

void P098_data_struct::moveReverse(int steps)
{
  if (steps > 0) {
    pos_dest = position - steps;
    startMoving();
  }
}

bool P098_data_struct::moveToPos(int pos)
{
  if (!canRun()) { return false; }
  const int offset = pos - getPosition();

  pos_dest = position + offset;
  startMoving();
  return true;
}

void P098_data_struct::stop()
{
  setPinState(_config.motorFwd, 0);
  setPinState(_config.motorRev, 0);

  lastVirtualSpeedApplied_us = 0;
}

int P098_data_struct::getPosition() const
{
  if (limitA.switchposSet) {
    return position - limitA.switchpos;
  }
  return position;
}

void P098_data_struct::getLimitSwitchStates(bool& limitA_triggered, bool& limitB_triggered) const
{
  limitA_triggered = limitA.state == P098_limit_switch_state::State::High ? 1 : 0;
  limitB_triggered = limitB.state == P098_limit_switch_state::State::High ? 1 : 0;

  /*
     limitA_triggered = _config.limitA.readState();
     limitB_triggered = _config.limitB.readState();
   */
}

void P098_data_struct::getLimitSwitchPositions(int& limitApos, int& limitBpos) const
{
  limitApos = limitA.switchposSet ? limitA.switchpos : 0;
  limitBpos = limitB.switchposSet ? limitB.switchpos : 0;
}

void P098_data_struct::startMoving()
{
  if(pos_dest == 0) {
    pos_overshoot = -_config.pos0supplement;
  }

  // Stop first, to make sure both outputs will not be set high
  stop();

  if (pos_dest > position) {
    state = P098_data_struct::State::RunFwd;
    setPinState(_config.motorFwd, 1);
  } else {
    state = P098_data_struct::State::RunRev;
    setPinState(_config.motorRev, 1);
  }
  // Touch the timer, so it will not immediately timeout.
  enc_lastChanged_us = getMicros64();

  if(_config.virtualSpeed > 0) {
    lastVirtualSpeedApplied_us = enc_lastChanged_us;
  }
}

void P098_data_struct::updatePosition()
{
  if(lastVirtualSpeedApplied_us == 0) {
    return; 
  }

  int direction = 0;
  switch (state) {
    case P098_data_struct::State::RunFwd:
    {
      direction = 1;
      break;
    }
    case P098_data_struct::State::RunRev:
    {
      direction = -1;
      break;
    }
    default:
      return;
  }

  uint64_t time = getMicros64();

  uint64_t steps = ((time-lastVirtualSpeedApplied_us)*((uint64_t)_config.virtualSpeed))/1000ul;
  if(_config.PWM_mode == P098_config_struct::PWM_mode_type::NoPWM) {
    position += direction*steps;
  } else {
    position += direction*steps*_config.pwm_duty_cycle/1023; // pwm range is 1023
  }
  
  lastVirtualSpeedApplied_us = time;
}

void P098_data_struct::checkLimit(volatile P098_limit_switch_state& switch_state)
{
  if (switch_state.state == P098_limit_switch_state::State::High) {
    stop();
    state = P098_data_struct::State::StopLimitSw;
    return;
  }
  checkPosition();
}

void P098_data_struct::checkPosition()
{
  bool mustStop = false;

  switch (state) {
    case P098_data_struct::State::RunFwd:

      mustStop = ((position + pos_overshoot) >= pos_dest);
      break;
    case P098_data_struct::State::RunRev:

      mustStop = ((position - pos_overshoot) <= pos_dest);
      break;
    default:
      return;
  }

  if (mustStop) {
    stop();
    position -= pos_overshoot;
    pos_overshoot = 0;
    state = P098_data_struct::State::StopPosReached;

    /*
       // Correct for position error
       if (std::abs(position - pos_dest) > 10) {
       startMoving();
       }
     */
  }
}

void P098_data_struct::setPinState(const P098_GPIO_config& gpio_config, int8_t state)
{
  // FIXME TD-er: Must move this code to the ESPEasy core code.
  uint8_t mode = PIN_MODE_OUTPUT;

  state = state == 0 ? gpio_config.low() : gpio_config.high();
  uint32_t key = createKey(PLUGIN_GPIO, gpio_config.gpio);

  if (globalMapPortStatus[key].mode != PIN_MODE_OFFLINE)
  {
    int8_t currentState;
    GPIO_Read(PLUGIN_GPIO, gpio_config.gpio, currentState);

    if (currentState == -1) {
      mode  = PIN_MODE_OFFLINE;
      state = -1;
    }

    switch (_config.PWM_mode) {
      case P098_config_struct::PWM_mode_type::NoPWM:
        if (mode == PIN_MODE_OUTPUT)  {
          createAndSetPortStatus_Mode_State(key, mode, state);
          GPIO_Write(
            PLUGIN_GPIO,
            gpio_config.gpio,
            state,
            mode);
        }
        break;
      case P098_config_struct::PWM_mode_type::PWM:
      {
        const uint32_t dutycycle = state == 0 ? 0 : _config.pwm_duty_cycle;
        const uint32_t fade_duration = _config.pwm_soft_startstop ? 
              100 /* (_config.encoder.timer_us / 1000) */
              : 0;
        uint32_t frequency = _config.pwm_freq;
        set_Gpio_PWM(
          gpio_config.gpio,
          dutycycle,
          fade_duration,
          frequency,
          key);
        if (state == 0) {
          // Turn off PWM mode too
          createAndSetPortStatus_Mode_State(key, PIN_MODE_OUTPUT, state);
          GPIO_Write(
            PLUGIN_GPIO,
            gpio_config.gpio,
            state,
            PIN_MODE_OUTPUT);
        }
      }
      break;
      case P098_config_struct::PWM_mode_type::MAX_TYPE:
        break;
    }
  }
}

bool P098_data_struct::setPinMode(const P098_GPIO_config& gpio_config)
{
  if (checkValidPortRange(PLUGIN_GPIO, gpio_config.gpio)) {
    pinMode(gpio_config.gpio, gpio_config.pullUp ? INPUT_PULLUP : INPUT);
    return true;
  }
  return false;
}

void P098_data_struct::check_limit_switch(
  const P098_GPIO_config          & gpio_config,
  volatile P098_limit_switch_state& switch_state)
{
  if (gpio_config.gpio == -1) {
    return;
  }
  // State is changed first in ISR, but compared after values are copied.
  const int triggerpos          = switch_state.triggerpos;
  const uint64_t lastChanged_us = switch_state.lastChanged_us;

  if (switch_state.state == P098_limit_switch_state::State::TriggerWaitBounce) {
    if (lastChanged_us != 0) {
      const uint64_t timeSinceLastTrigger = getMicros64() - lastChanged_us;

      if (timeSinceLastTrigger > gpio_config.timer_us) {
        mark_limit_switch_state(triggerpos, switch_state);
      }
    }
  }
}

void P098_data_struct::mark_limit_switch_state(
    int triggerpos, 
    volatile P098_limit_switch_state& switch_state)
{
  if (!switch_state.switchposSet) {
    switch_state.switchpos    = triggerpos;
    switch_state.switchposSet = true;
  }

  // Perform an extra check here on the state as it may have changed in the ISR call
  if (switch_state.state == P098_limit_switch_state::State::TriggerWaitBounce) {
    switch_state.state = P098_limit_switch_state::State::High;
  }
}

bool P098_data_struct::check_encoder_timeout(const P098_GPIO_config & gpio_config)
{
  if (gpio_config.gpio == -1) {
    return false;
  }
  if (enc_lastChanged_us == 0) {
    return false;
  }
  const bool expired = usecPassedSince(enc_lastChanged_us) > static_cast<int64_t>(_config.encoder.timer_us);
  if (!expired) {
    return false;
  }
  switch (state) {
    case P098_data_struct::State::RunFwd:
    {
      mark_limit_switch_state(position, limitB);
      break;
    }
    case P098_data_struct::State::RunRev:
    {
      mark_limit_switch_state(position, limitA);
      break;
    }
    default:
      return false;
  }
  return true;
}

void ICACHE_RAM_ATTR P098_data_struct::process_limit_switch(
  const P098_GPIO_config          & gpio_config,
  volatile P098_limit_switch_state& switch_state,
  int                               position)
{
  ISR_noInterrupts();
  {
    // Don't call gpio_config.readState() here
    const bool pinState        = 
      (DIRECT_pinRead_ISR(gpio_config.gpio) != 0) ^ gpio_config.inverted;
    const uint64_t currentTime = getMicros64();


    switch (switch_state.state) {
      case P098_limit_switch_state::State::Low:

        if (pinState) {
          switch_state.state          = P098_limit_switch_state::State::TriggerWaitBounce;
          switch_state.lastChanged_us = currentTime;
          switch_state.triggerpos     = position;
        }
        break;
      case P098_limit_switch_state::State::TriggerWaitBounce:
      {
        // Do not evaluate the debounce time here, evaluate in the loop
        if (pinState) {
          // Only situation we can get here is when we missed a low state interrupt.
          switch_state.lastChanged_us = currentTime;
          switch_state.triggerpos     = position;
        } else {
          switch_state.state          = P098_limit_switch_state::State::Low;
          switch_state.lastChanged_us = 0;
          switch_state.triggerpos     = 0;
        }
        break;
      }
      case P098_limit_switch_state::State::High:

        if (!pinState) {
          switch_state.state          = P098_limit_switch_state::State::Low;
          switch_state.lastChanged_us = 0;
          switch_state.triggerpos     = 0;
        }
        break;
    }
  }
  ISR_interrupts(); // enable interrupts again.
}

void ICACHE_RAM_ATTR P098_data_struct::ISRlimitA(P098_data_struct *self)
{
  process_limit_switch(self->_config.limitA, self->limitA, self->position);
}

void ICACHE_RAM_ATTR P098_data_struct::ISRlimitB(P098_data_struct *self)
{
  process_limit_switch(self->_config.limitB, self->limitB, self->position);
}

void ICACHE_RAM_ATTR P098_data_struct::ISRencoder(P098_data_struct *self)
{
  ISR_noInterrupts();

  switch (self->state) {
    case P098_data_struct::State::RunFwd:
      ++(self->position);
      break;
    case P098_data_struct::State::RunRev:
      --(self->position);
      break;
    default:
      ISR_interrupts(); // enable interrupts again.
      return;
  }
  self->enc_lastChanged_us = getMicros64();
  ISR_interrupts(); // enable interrupts again.
}

#endif // ifdef USES_P098
