#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), state(States::ADC_OFF_PL_OFF), gpioIF(gpioIF) {}

void PayloadPcduHandler::doStartUp() {
  switch(state) {
  case(States::ADC_OFF_PL_OFF): {
    // Switch on relays here
    gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT0);
    gpioIF->pullHigh(gpioIds::PLPCDU_ENB_VBAT1);
    state = States::SWITCH_ON_RELAY_TRANSITION;
    break;
  }
  case(States::SWITCH_ON_RELAY_TRANSITION): {
    // If necessary, check whether a certain amount of time has elapsed
    state = States::ADC_OFF_PL_OFF;
    break;
  }
  default:
    // Config error
    sif::error << "PayloadPcduHandler::doStartUp: Invalid state" << std::endl;
  }
}

void PayloadPcduHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
  if(mode == _MODE_TO_NORMAL) {
      switch(state) {
      case(States::ADC_OFF_PL_OFF): {
        // Switch on HPA here
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_DRO);
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_MPA);
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_TX);
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_X8);
        gpioIF->pullHigh(gpioIds::PLPCDU_ENB_HPA);
        state = States::SWITCH_ON_HPA_TRANSITION;
        break;
      }
      case(States::SWITCH_ON_HPA_TRANSITION): {
        // If necessary, check whether a certain amount of time has elapsed
        state = States::ADC_ON_PL_ON;
        setMode(MODE_NORMAL);
        break;
      }
      default:
        // Config error
        sif::error << "PayloadPcduHandler::doTransition: Invalid state" << std::endl;
      }
  }
}

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) { return 10000; }



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