#include "SolarArrayDeploymentHandler.h"

#include "devices/gpioIds.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw_hal/common/gpio/GpioCookie.h"

#include <filesystem>
#include <iostream>
#include <fstream>

SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_,
                                                         GpioIF& gpioInterface,
                                                         PowerSwitchIF& mainLineSwitcher_,
                                                         pcdu::Switches mainLineSwitch_,
                                                         gpioId_t deplSA1, gpioId_t deplSA2,
                                                         SdCardMountedIF& sdcMountedIF)
    : SystemObject(setObjectId_),
      gpioInterface(gpioInterface),
      deplSA1(deplSA1),
      deplSA2(deplSA2),
      mainLineSwitcher(mainLineSwitcher_),
      mainLineSwitch(mainLineSwitch_),
      sdcMan(sdcMountedIF),
      actionHelper(this, nullptr) {
  auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
  commandQueue = QueueFactory::instance()->createMessageQueue(
      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}

SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() = default;

ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {
  using namespace std::filesystem;
  handleStateMachine();
  auto activeSdc = sdcMan.getActiveSdCard();
  if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_0 and
      sdcMan.isSdCardUsable(activeSdc.value())) {
    if (exists(SD_0_DEPL_FILE)) {
      // perform autonomous deployment handling
      performAutonomousDepl(sd::SdCard::SLOT_0);
    }
  } else if (activeSdc and activeSdc.value() == sd::SdCard::SLOT_1 and
             sdcMan.isSdCardUsable(activeSdc.value())) {
    if (exists(SD_1_DEPL_FILE)) {
      // perform autonomous deployment handling
      performAutonomousDepl(sd::SdCard::SLOT_1);
    }
  }
  return returnvalue::OK;
}

ReturnValue_t SolarArrayDeploymentHandler::performAutonomousDepl(sd::SdCard sdCard) {
  using namespace std::filesystem;
  using namespace std;
  auto initFile = [](const char* filename) {
    ofstream of(filename);
    of << "phase: init\n";
    of << "secs_since_start: 0\n";
  };
  auto handler = [](const char* filename) {

    ifstream file(filename);
    string line;
    string word;
    unsigned int lineNum = 0;
    AutonomousDeplState deplState;
    while(std::getline(file, line)) {
      if(lineNum == 0) {
        std::istringstream iss(line);
        if (lineNum == 0) {
          iss >> word;
          if(word.find("phase:") == string::npos) {
             return false;
          }
          iss >> word;
          if(word.find(PHASE_INIT_STR) != string::npos) {
            deplState = AutonomousDeplState::INIT;
          } else if(word.find(PHASE_FIRST_BURN_STR) != string::npos) {
            deplState = AutonomousDeplState::FIRST_BURN;
          } else if(word.find(PHASE_WAIT_STR) != string::npos) {
            deplState = AutonomousDeplState::WAIT;
          } else if(word.find(PHASE_SECOND_BURN_STR) != string::npos) {
            deplState = AutonomousDeplState::SECOND_BURN;
          } else if(word.find(PHASE_DONE) != string::npos) {
            deplState = AutonomousDeplState::DONE;
          } else {
            return false;
          }
        } else if(lineNum == 1) {
          iss >> word;
          if(word.find("phase:") == string::npos) {
             return false;
          }
          uint32_t secsSinceBoot = 0;
          iss >> secsSinceBoot;
          if (iss.bad()) {
            return false;
          }
        }
      }
      lineNum++;
    }
    return true;
  };
  if (sdCard == sd::SdCard::SLOT_0) {
    if (not exists(SD_0_DEPLY_INFO)) {
      initFile(SD_0_DEPLY_INFO);
    }
    if (not handler(SD_0_DEPLY_INFO)) {
      initFile(SD_0_DEPLY_INFO);
    }
  } else if(sdCard == sd::SdCard::SLOT_1) {
    if (not exists(SD_1_DEPLY_INFO)) {
      initFile(SD_1_DEPLY_INFO);
    }
    if (not handler(SD_1_DEPLY_INFO)) {
      initFile(SD_1_DEPLY_INFO);
    }
  }
  return returnvalue::OK;
}

ReturnValue_t SolarArrayDeploymentHandler::initialize() {
  ReturnValue_t result = actionHelper.initialize(commandQueue);
  if (result != returnvalue::OK) {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  return SystemObject::initialize();
}

void SolarArrayDeploymentHandler::handleStateMachine() {
  //  switch (stateMachine) {
  //    case WAIT_ON_DELOYMENT_COMMAND:
  //      readCommandQueue();
  //      break;
  //    case SWITCH_8V_ON:
  //      mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
  //      mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
  //      stateMachine = WAIT_ON_8V_SWITCH;
  //      break;
  //    case WAIT_ON_8V_SWITCH:
  //      performWaitOn8VActions();
  //      break;
  //    case SWITCH_DEPL_GPIOS:
  //      deploymentTransistorsOn();
  //      break;
  //    case WAIT_ON_DEPLOYMENT_FINISH:
  //      handleDeploymentFinish();
  //      break;
  //    case WAIT_FOR_MAIN_SWITCH_OFF:
  //      if (mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF) {
  //        stateMachine = WAIT_ON_DELOYMENT_COMMAND;
  //      } else if (mainSwitchCountdown.hasTimedOut()) {
  //        triggerEvent(MAIN_SWITCH_OFF_TIMEOUT);
  //        sif::error << "SolarArrayDeploymentHandler::handleStateMachine: Failed to switch main"
  //                   << " switch off" << std::endl;
  //        stateMachine = WAIT_ON_DELOYMENT_COMMAND;
  //      }
  //      break;
  //    default:
  //      sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Invalid state" <<
  //      std::endl; break;
  //  }
}

void SolarArrayDeploymentHandler::performWaitOn8VActions() {
  //  if (mainLineSwitcher.getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) {
  //    stateMachine = SWITCH_DEPL_GPIOS;
  //  } else {
  //    if (mainSwitchCountdown.hasTimedOut()) {
  //      triggerEvent(MAIN_SWITCH_ON_TIMEOUT);
  //      actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
  //                          MAIN_SWITCH_TIMEOUT_FAILURE);
  //      stateMachine = WAIT_ON_DELOYMENT_COMMAND;
  //    }
  //  }
}

void SolarArrayDeploymentHandler::deploymentTransistorsOn() {
  //  ReturnValue_t result = returnvalue::OK;
  //  result = gpioInterface->pullHigh(deplSA1);
  //  if (result != returnvalue::OK) {
  //    sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
  //                  " array deployment switch 1 high "
  //               << std::endl;
  //    /* If gpio switch high failed, state machine is reset to wait for a command reinitiating
  //     * the deployment sequence. */
  //    stateMachine = WAIT_ON_DELOYMENT_COMMAND;
  //    triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED);
  //    actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
  //    SWITCHING_DEPL_SA2_FAILED); mainLineSwitcher.sendSwitchCommand(mainLineSwitch,
  //    PowerSwitchIF::SWITCH_OFF);
  //  }
  //  result = gpioInterface->pullHigh(deplSA2);
  //  if (result != returnvalue::OK) {
  //    sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
  //                  " array deployment switch 2 high "
  //               << std::endl;
  //    stateMachine = WAIT_ON_DELOYMENT_COMMAND;
  //    triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED);
  //    actionHelper.finish(false, rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
  //    SWITCHING_DEPL_SA2_FAILED); mainLineSwitcher.sendSwitchCommand(mainLineSwitch,
  //    PowerSwitchIF::SWITCH_OFF);
  //  }
  // deploymentCountdown.setTimeout(burnTimeMs);
  // stateMachine = WAIT_ON_DEPLOYMENT_FINISH;
}

// void SolarArrayDeploymentHandler::handleDeploymentFinish() {
//   ReturnValue_t result = returnvalue::OK;
//  if (deploymentCountdown.hasTimedOut()) {
//    actionHelper.finish(true, rememberCommanderId, DEPLOY_SOLAR_ARRAYS, returnvalue::OK);
//    result = gpioInterface->pullLow(deplSA1);
//    if (result != returnvalue::OK) {
//      sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
//                    " array deployment switch 1 low "
//                 << std::endl;
//    }
//    result = gpioInterface->pullLow(deplSA2);
//    if (result != returnvalue::OK) {
//      sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
//                    " array deployment switch 2 low "
//                 << std::endl;
//    }
//    mainLineSwitcher.sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
//    mainSwitchCountdown.setTimeout(mainLineSwitcher.getSwitchDelayMs());
//    stateMachine = WAIT_FOR_MAIN_SWITCH_OFF;
//  }
//}

void SolarArrayDeploymentHandler::readCommandQueue() {
  CommandMessage command;
  ReturnValue_t result = commandQueue->receiveMessage(&command);
  if (result != returnvalue::OK) {
    return;
  }

  result = actionHelper.handleActionMessage(&command);
  if (result == returnvalue::OK) {
    return;
  }
}

ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
                                                         MessageQueueId_t commandedBy,
                                                         const uint8_t* data, size_t size) {
  ReturnValue_t result = returnvalue::OK;
  //  if (stateMachine != WAIT_ON_DELOYMENT_COMMAND) {
  //    sif::error << "SolarArrayDeploymentHandler::executeAction: Received command while not in"
  //               << "waiting-on-command-state" << std::endl;
  //    return DEPLOYMENT_ALREADY_EXECUTING;
  //  }
  //  if (actionId != DEPLOY_SOLAR_ARRAYS) {
  //    sif::error << "SolarArrayDeploymentHandler::executeAction: Received invalid command"
  //               << std::endl;
  //    result = COMMAND_NOT_SUPPORTED;
  //  } else {
  //    stateMachine = SWITCH_8V_ON;
  //    rememberCommanderId = commandedBy;
  //    result = returnvalue::OK;
  //  }
  return result;
}

MessageQueueId_t SolarArrayDeploymentHandler::getCommandQueue() const {
  return commandQueue->getId();
}