#include "PayloadPcduHandler.h"

#include "devices/gpioIds.h"

PayloadPcduHandler::PayloadPcduHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie,
                                       GpioIF* gpioIF)
    : DeviceHandlerBase(objectId, comIF, cookie), gpioIF(gpioIF) {}

void PayloadPcduHandler::doStartUp() {
  if((state != States::PCDU_OFF) and (state != States::ON_TRANS_SSR)) {
    // Config error
    sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl;
  }
  if (state == States::PCDU_OFF) {
    // Switch on relays here
    gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT0);
    gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT1);
    state = States::ON_TRANS_SSR;
    transitionOk = true;
  }
  if (state == States::ON_TRANS_SSR) {
    // If necessary, check whether a certain amount of time has elapsed
    if(transitionOk) {
      transitionOk = false;
      // We are now in ON mode
      setMode(MODE_ON);
      // The ADC can now be read. If the values are not close to zero, we should not allow
      // transition
      monMode = MonitoringMode::CLOSE_TO_ZERO;
    }
  }
}

void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
  if (mode == _MODE_TO_NORMAL) {
    if (state == States::ON_TRANS_ADC_CLOSE_ZERO) {
      if(not commandExecuted) {
        countdown.resetTimer();
        commandExecuted = true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if(transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_DRO;
        // Now start monitoring for negative voltages instead
        monMode = MonitoringMode::NEGATIVE;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if (state == States::ON_TRANS_DRO) {
      if(not commandExecuted) {
        // Switch on DRO and start monitoring for negative voltagea
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_DRO);
        commandExecuted =  true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if(transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_X8;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if (state == States::ON_TRANS_X8) {
      if(not commandExecuted) {
        // Switch on X8
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_X8);
        commandExecuted =  true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if(transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_TX;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if(state == States::ON_TRANS_TX) {
      if(not commandExecuted) {
        // Switch on TX
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_TX);
        commandExecuted =  true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if(transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_MPA;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if(state == States::ON_TRANS_MPA) {
      if(not commandExecuted) {
        // Switch on MPA
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_MPA);
        commandExecuted =  true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if(transitionOk and countdown.hasTimedOut()) {
        state = States::ON_TRANS_HPA;
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
    if(state == States::ON_TRANS_HPA) {
      if(not commandExecuted) {
        // Switch on HPA
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_HPA);
        commandExecuted =  true;
      }
      // ADC values are ok, 5 seconds have elapsed
      if(transitionOk and countdown.hasTimedOut()) {
        state = States::PCDU_ON;
        setMode(MODE_NORMAL);
        countdown.resetTimer();
        commandExecuted = false;
        transitionOk = false;
      }
    }
  }
}

void PayloadPcduHandler::doShutDown() {}

ReturnValue_t PayloadPcduHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PayloadPcduHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  return HasReturnvaluesIF::RETURN_OK;
}

void PayloadPcduHandler::fillCommandAndReplyMap() {}

ReturnValue_t PayloadPcduHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                          const uint8_t* commandData,
                                                          size_t commandDataLen) {
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PayloadPcduHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                               DeviceCommandId_t* foundId, size_t* foundLen) {
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PayloadPcduHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                       const uint8_t* packet) {
  return HasReturnvaluesIF::RETURN_OK;
}

uint32_t PayloadPcduHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
  // 20 minutes transition delay is allowed
  return 20 * 60 * 60;
}

ReturnValue_t PayloadPcduHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                          LocalDataPoolManager& poolManager) {
  return HasReturnvaluesIF::RETURN_OK;
}