#include <mission/controller/PowerController.h>

PowerController::PowerController(object_id_t objectId, bool enableHkSets)
    : ExtendedControllerBase(objectId),
      enableHkSets(enableHkSets),
      parameterHelper(this),
      pwrCtrlCoreHk(this),
      enablePl(this) {}

ReturnValue_t PowerController::initialize() {
  ReturnValue_t result = parameterHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  return ExtendedControllerBase::initialize();
}

ReturnValue_t PowerController::handleCommandMessage(CommandMessage *message) {
  ReturnValue_t result = actionHelper.handleActionMessage(message);
  if (result == returnvalue::OK) {
    return result;
  }
  result = parameterHelper.handleParameterMessage(message);
  if (result == returnvalue::OK) {
    return result;
  }
  return result;
}

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

ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterId,
                                            ParameterWrapper *parameterWrapper,
                                            const ParameterWrapper *newValues,
                                            uint16_t startAtIndex) {
  switch (domainId) {
    case 0x0:  // direct members
      switch (parameterId) {
        case 0x0:
          parameterWrapper->set(batteryInternalResistance);
          break;
        case 0x1:
          parameterWrapper->set(batteryMaximumCapacity);
          break;
        case 0x2: {
          float oldCoulombCounterVoltageUpperThreshold = coulombCounterVoltageUpperThreshold;
          parameterWrapper->set(coulombCounterVoltageUpperThreshold);
          ReturnValue_t result = calculateCoulombCounterChargeUpperThreshold();
          if (result != returnvalue::OK) {
            coulombCounterVoltageUpperThreshold = oldCoulombCounterVoltageUpperThreshold;
          }
          break;
        }
        case 0x3:
          parameterWrapper->set(maxAllowedTimeDiff);
          break;
        case 0x4:
          parameterWrapper->set(payloadOpLimitOn);
          break;
        case 0x5:
          parameterWrapper->set(payloadOpLimitLow);
          break;
        case 0x6:
          parameterWrapper->set(higherModesLimit);
          break;
        default:
          return INVALID_IDENTIFIER_ID;
      }
      break;
    default:
      return INVALID_DOMAIN_ID;
  };
  return returnvalue::OK;
}

void PowerController::performControlOperation() {
  switch (internalState) {
    case InternalState::STARTUP: {
      initialCountdown.resetTimer();
      internalState = InternalState::INITIAL_DELAY;
      return;
    }
    case InternalState::INITIAL_DELAY: {
      if (initialCountdown.hasTimedOut()) {
        internalState = InternalState::INIT;
      }
      return;
    }
    case InternalState::INIT: {
      ReturnValue_t result = calculateCoulombCounterChargeUpperThreshold();
      if (result == returnvalue::OK) {
        internalState = InternalState::READY;
      }
      return;
    }
    case InternalState::READY: {
      if (mode != MODE_OFF) {
        calculateStateOfCharge();
        if (mode == MODE_NORMAL) {
          watchStateOfCharge();
        } else {
          PoolReadGuard pg(&enablePl);
          if (pg.getReadResult() == returnvalue::OK) {
            enablePl.setValidity(false, true);
          }
        }
      }
      break;
    }
    default:
      break;
  }
}

ReturnValue_t PowerController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                       LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(pwrctrl::PoolIds::TOTAL_BATTERY_CURRENT, new PoolEntry<int16_t>({0}));
  localDataPoolMap.emplace(pwrctrl::PoolIds::OPEN_CIRCUIT_VOLTAGE_CHARGE,
                           new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(pwrctrl::PoolIds::COULOMB_COUNTER_CHARGE, new PoolEntry<float>({0.0}));
  poolManager.subscribeForRegularPeriodicPacket({pwrCtrlCoreHk.getSid(), enableHkSets, 60.0});
  localDataPoolMap.emplace(pwrctrl::PoolIds::PAYLOAD_FLAG, new PoolEntry<uint8_t>({false}));
  poolManager.subscribeForRegularPeriodicPacket({enablePl.getSid(), false, 60.0});
  return returnvalue::OK;
}

LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
    case pwrctrl::CORE_HK:
      return &pwrCtrlCoreHk;
    case pwrctrl::ENABLE_PL:
      return &enablePl;
    default:
      return nullptr;
  }
  return nullptr;
}

ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                uint32_t *msToReachTheMode) {
  if (mode == MODE_OFF or mode == MODE_ON or mode == MODE_NORMAL) {
    if (submode == SUBMODE_NONE) {
      return returnvalue::OK;
    } else {
      return INVALID_SUBMODE;
    }
  }
  return INVALID_MODE;
}

void PowerController::calculateStateOfCharge() {
  // get time
  Clock::getClock_timeval(&now);

  // update EPS HK values
  ReturnValue_t result = updateEpsData();
  if (result != returnvalue::OK) {
    triggerEvent(power::DATASET_READ_FAILED);
    sif::error << "Power Controller::Reading of Datasets has failed" << std::endl;
    {
      PoolReadGuard pg(&pwrCtrlCoreHk);
      if (pg.getReadResult() == returnvalue::OK) {
        pwrCtrlCoreHk.totalBatteryCurrent.value = INVALID_TOTAL_BATTERY_CURRENT;
        pwrCtrlCoreHk.openCircuitVoltageCharge.value = INVALID_SOC;
        pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
        pwrCtrlCoreHk.setValidity(false, true);
      }
    }
    // store time for next run
    oldTime = now;
    return;
  }

  // calculate total battery current
  iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.dischargeCurrent.value;

  result = calculateOpenCircuitVoltageCharge();
  if (result != returnvalue::OK) {
    // notifying events have already been triggered
    {
      PoolReadGuard pg(&pwrCtrlCoreHk);
      if (pg.getReadResult() == returnvalue::OK) {
        pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
        pwrCtrlCoreHk.totalBatteryCurrent.setValid(true);
        pwrCtrlCoreHk.openCircuitVoltageCharge.value = INVALID_SOC;
        pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(false);
        pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
        pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
      }
    }
    // store time for next run
    oldTime = now;
    return;
  }

  result = calculateCoulombCounterCharge();
  if (result != returnvalue::OK) {
    // notifying events have already been triggered
    {
      PoolReadGuard pg(&pwrCtrlCoreHk);
      if (pg.getReadResult() == returnvalue::OK) {
        pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
        pwrCtrlCoreHk.totalBatteryCurrent.setValid(true);
        pwrCtrlCoreHk.openCircuitVoltageCharge.value =
            charge2stateOfCharge(openCircuitVoltageCharge, false);
        pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(true);
        pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
        pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
      }
    }
    // store time for next run
    oldTime = now;
    return;
  }

  // commit to dataset
  {
    PoolReadGuard pg(&pwrCtrlCoreHk);
    if (pg.getReadResult() == returnvalue::OK) {
      pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
      pwrCtrlCoreHk.openCircuitVoltageCharge.value =
          charge2stateOfCharge(openCircuitVoltageCharge, false);
      pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge, true);
      pwrCtrlCoreHk.setValidity(true, true);
    }
  }
  // store time for next run
  oldTime = now;
}

void PowerController::watchStateOfCharge() {
  if (pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
    if (pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitOn) {
      PoolReadGuard pg(&enablePl);
      if (pg.getReadResult() == returnvalue::OK) {
        enablePl.plUseAllowed.value = false;
        enablePl.setValidity(true, true);
      }
    } else {
      PoolReadGuard pg(&enablePl);
      if (pg.getReadResult() == returnvalue::OK) {
        enablePl.plUseAllowed.value = true;
        enablePl.setValidity(true, true);
      }
    }
    if (not pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitLow) {
      triggerEvent(power::POWER_LEVEL_LOW);
      pwrLvlLowFlag = true;
    } else {
      pwrLvlLowFlag = false;
    }
    if (not pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value < higherModesLimit) {
      triggerEvent(power::POWER_LEVEL_CRITICAL);
      pwrLvlCriticalFlag = true;
    } else {
      pwrLvlCriticalFlag = false;
    }
  } else {
    PoolReadGuard pg(&enablePl);
    if (pg.getReadResult() == returnvalue::OK) {
      enablePl.plUseAllowed.value = false;
      enablePl.setValidity(true, true);
    }
  }
}

ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
  float vBatCorrected =
      (bpxBatteryHk.battVoltage.value - iBat * batteryInternalResistance) * CONVERT_FROM_MILLI;
  uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
  ReturnValue_t result = lookUpTableOcvIdxFinder(vBatCorrected, lookUpTableIdx);
  if (result != returnvalue::OK) {
    return result;
  }
  openCircuitVoltageCharge = linearInterpolation(
      vBatCorrected, lookUpTableOcv[1][lookUpTableIdx], lookUpTableOcv[1][lookUpTableIdx + 1],
      lookUpTableOcv[0][lookUpTableIdx], lookUpTableOcv[0][lookUpTableIdx + 1]);
  return returnvalue::OK;
}

ReturnValue_t PowerController::calculateCoulombCounterCharge() {
  double timeDiff = timevalOperations::toDouble(now - oldTime);
  if (timeDiff > maxAllowedTimeDiff) {
    triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDiff * 10));
    sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff
               << std::endl;
    return returnvalue::FAILED;
  }
  if (not pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
    coulombCounterCharge = openCircuitVoltageCharge;
  } else {
    coulombCounterCharge =
        coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * SECONDS_TO_HOURS;
    if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
      coulombCounterCharge = coulombCounterChargeUpperThreshold;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t PowerController::updateEpsData() {
  std::vector<ReturnValue_t> results;
  {
    PoolReadGuard pgBat(&bpxBatteryHk);
    results.push_back(pgBat.getReadResult());
  }
  {
    PoolReadGuard pgP60(&p60CoreHk);
    results.push_back(pgP60.getReadResult());
  }
  for (const auto &result : results) {
    if (result != returnvalue::OK) {
      return result;
    }
  }
  return returnvalue::OK;
}

float PowerController::charge2stateOfCharge(float capacity, bool coulombCounter) {
  if (coulombCounter) {
    return capacity / coulombCounterChargeUpperThreshold;
  }
  return capacity / batteryMaximumCapacity;
}

float PowerController::linearInterpolation(float x, float x0, float x1, float y0, float y1) {
  return y0 + (x - x0) * (y1 - y0) / (x1 - x0);
}

ReturnValue_t PowerController::lookUpTableOcvIdxFinder(float voltage, uint8_t &idx) {
  if (voltage >= lookUpTableOcv[1][99]) {
    triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 0, static_cast<uint32_t>(voltage * 10));
    sif::error << "Power Controller::Voltage is too high: " << voltage << std::endl;
    return returnvalue::FAILED;
  } else if (voltage <= lookUpTableOcv[1][0]) {
    triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 1, static_cast<uint32_t>(voltage * 10));
    sif::error << "Power Controller::Voltage is too low: " << voltage << std::endl;
    return returnvalue::FAILED;
  }
  while (lookUpTableOcv[1][idx] > voltage) {
    idx--;
  }
  return returnvalue::OK;
}

ReturnValue_t PowerController::calculateCoulombCounterChargeUpperThreshold() {
  uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
  ReturnValue_t result =
      lookUpTableOcvIdxFinder(coulombCounterVoltageUpperThreshold, lookUpTableIdx);
  if (result != returnvalue::OK) {
    return result;
  }
  coulombCounterChargeUpperThreshold =
      linearInterpolation(coulombCounterVoltageUpperThreshold, lookUpTableOcv[1][lookUpTableIdx],
                          lookUpTableOcv[1][lookUpTableIdx + 1], lookUpTableOcv[0][lookUpTableIdx],
                          lookUpTableOcv[0][lookUpTableIdx + 1]);
  return returnvalue::OK;
}