#include <mission/controller/PowerController.h>

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

ReturnValue_t PowerController::initialize() {
  ReturnValue_t result = parameterHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  sif::debug << "Rush B, no stop" << std::endl;
  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;
        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) {
      sif::debug << "oh shit, now i gotta do something" << std::endl;
      calculateStateOfCharge();
      // do something
      //}
      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});
  return returnvalue::OK;
}

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

ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                uint32_t *msToReachTheMode) {
  if (mode == MODE_OFF) {
    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);
      }
    }
    return;
  }

  // calculate total battery current
  iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.heaterCurrent.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);
      }
    }
    return;
  }

  result = calculateCoulombCounterCharge();
  // store time for next run
  oldTime = now;
  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);
        pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(true);
        pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
        pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
      }
    }
    return;
  }

  // commit to dataset
  {
    PoolReadGuard pg(&pwrCtrlCoreHk);
    if (pg.getReadResult() == returnvalue::OK) {
      pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
      pwrCtrlCoreHk.openCircuitVoltageCharge.value = charge2stateOfCharge(openCircuitVoltageCharge);
      pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge);
      pwrCtrlCoreHk.setValidity(true, true);
    }
  }
}

ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
  float vBatCorrected = p60CoreHk.batteryVoltage.value - iBat * batteryInternalResistance;
  uint8_t lookUpTableIdx = 99;
  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);
    sif::error << "Power Controller::Time delta too large for Coulomb Counter" << std::endl;
    return returnvalue::FAILED;
  }
  if ((not pwrCtrlCoreHk.coulombCounterCharge.isValid()) or
      (p60CoreHk.batteryVoltage.value > coulombCounterVoltageUpperThreshold and
       pwrCtrlCoreHk.coulombCounterCharge.value >= coulombCounterChargeUpperThreshold)) {
    coulombCounterCharge = openCircuitVoltageCharge;
  } else {
    coulombCounterCharge = pwrCtrlCoreHk.coulombCounterCharge.value + iBat * timeDiff;
  }
  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());
  }
  {
    PoolReadGuard pgPdu1(&pdu1CoreHk);
    results.push_back(pgPdu1.getReadResult());
  }
  {
    PoolReadGuard pgPdu2(&pdu2CoreHk);
    results.push_back(pgPdu2.getReadResult());
  }
  {
    PoolReadGuard pgAcu(&acuCoreHk);
    results.push_back(pgAcu.getReadResult());
  }
  for (const auto &result : results) {
    if (result != returnvalue::OK) {
      return result;
    }
  }
  return returnvalue::OK;
}

float PowerController::charge2stateOfCharge(float capacity) {
  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][100]) {
    triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 0);
    sif::error << "Power Controller::Voltage is too high" << std::endl;
    return returnvalue::FAILED;
  } else if (voltage <= lookUpTableOcv[1][0]) {
    triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 1);
    sif::error << "Power Controller::Voltage is too low" << std::endl;
    return returnvalue::FAILED;
  }
  while (lookUpTableOcv[1][idx] > voltage) {
    idx--;
  }
  return returnvalue::OK;
}

ReturnValue_t PowerController::calculateCoulombCounterChargeUpperThreshold() {
  uint8_t lookUpTableIdx = 99;
  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;
}