#include "drv8710.hpp"
using namespace iflytop;

void DRV8710::initialize(config_t* cfg) {  //
  m_cfg                  = *cfg;
  cfg->pwm_cfg.calltrace = cfg->enableTrace;
  m_pwmCtrl.initialize(&cfg->pwm_cfg);

  ZASSERT(cfg->nsleep != PinNull);
  ZASSERT(cfg->nfault != PinNull);
  ZASSERT(cfg->sensePin != PinNull);

  m_nsleep.initAsOutput(cfg->nsleep, ZGPIO::kMode_nopull, false, false);
  m_nsleep.enableTrace(cfg->enableTrace);
  m_nfault.initAsInput(cfg->nfault, ZGPIO::kMode_pullup, ZGPIO::kIRQ_noIrq, false);
  m_nsensePin.initAsInput(cfg->sensePin, ZGPIO::kMode_pullup, ZGPIO::kIRQ_noIrq, false);

  m_in2.initAsOutput(cfg->in2, ZGPIO::kMode_nopull, false, false);
  m_in2.enableTrace(cfg->enableTrace);
  enable(false);
}

void DRV8710::moveForward(int32_t duty) {
  if (duty > 100) duty = 100;
  if (duty < 0) duty = 0;

  set_direcetion(true);
  m_pwmCtrl.startPWM(m_cfg.in1_chnannel_index, duty);
}
void DRV8710::moveBackward(int32_t duty) {
  if (duty > 100) duty = 100;
  if (duty < 0) duty = 0;

  set_direcetion(false);
  m_pwmCtrl.startPWM(m_cfg.in1_chnannel_index, duty);
}

bool DRV8710::isFault() { return !m_nfault.getState(); }

void DRV8710::move(int32_t duty) {
  if (duty >= 0) {
    moveForward(duty);
  } else if (duty < 0) {
    moveBackward(-duty);
  }
}

void DRV8710::set_direcetion(bool direcetion) {
  if (m_cfg.shaft) {
    direcetion = !direcetion;
  }
  m_in2.setState(direcetion);
}

void DRV8710::enable(bool varenable) { m_nsleep.setState(varenable); }