#include "SusDummy.h"

#include <objects/systemObjectList.h>

#include <cmath>
#include <cstdlib>

SusDummy::SusDummy()
    : ExtendedControllerBase(objects::SUS_0_N_LOC_XFYFZM_PT_XF, objects::NO_OBJECT), susSet(this) {
  ObjectManager::instance()->insert(objects::SUS_6_R_LOC_XFYBZM_PT_XF, this);
  ObjectManager::instance()->insert(objects::SUS_1_N_LOC_XBYFZM_PT_XB, this);
  ObjectManager::instance()->insert(objects::SUS_7_R_LOC_XBYBZM_PT_XB, this);
  ObjectManager::instance()->insert(objects::SUS_2_N_LOC_XFYBZB_PT_YB, this);
  ObjectManager::instance()->insert(objects::SUS_8_R_LOC_XBYBZB_PT_YB, this);
  ObjectManager::instance()->insert(objects::SUS_3_N_LOC_XFYBZF_PT_YF, this);
  ObjectManager::instance()->insert(objects::SUS_9_R_LOC_XBYBZB_PT_YF, this);
  ObjectManager::instance()->insert(objects::SUS_4_N_LOC_XMYFZF_PT_ZF, this);
  ObjectManager::instance()->insert(objects::SUS_10_N_LOC_XMYBZF_PT_ZF, this);
  ObjectManager::instance()->insert(objects::SUS_5_N_LOC_XFYMZB_PT_ZB, this);
  ObjectManager::instance()->insert(objects::SUS_11_R_LOC_XBYMZB_PT_ZB, this);
}

ReturnValue_t SusDummy::initialize() {
  static bool done = false;
  if (not done) {
    done = true;
    ReturnValue_t result = ExtendedControllerBase::initialize();
    if (result != returnvalue::OK) {
      return result;
    }
  }

  return returnvalue::OK;
}

ReturnValue_t SusDummy::handleCommandMessage(CommandMessage* message) {
  return returnvalue::FAILED;
}

void SusDummy::performControlOperation() {
  iteration++;
  value = sin(iteration / 80. * M_PI + 10) * 10 - 10;

  susSet.read();
  susSet.temperatureCelcius = value;
  if ((iteration % 100) < 20) {
    susSet.setValidity(false, true);
  } else {
    susSet.setValidity(true, true);
  }
  susSet.channels[0] = 3913;
  susSet.channels[1] = 3912;
  susSet.channels[2] = 3799;
  susSet.channels[3] = 4056;
  susSet.commit();
}

ReturnValue_t SusDummy::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(SUS::SusPoolIds::TEMPERATURE_C, new PoolEntry<float>({0}, 1, true));
  localDataPoolMap.emplace(SUS::SusPoolIds::CHANNEL_VEC,
                           new PoolEntry<uint16_t>({0, 0, 0, 0, 0, 0}));

  return returnvalue::OK;
}

LocalPoolDataSetBase* SusDummy::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
    case SUS::SUS_DATA_SET_ID:
      return &susSet;
    default:
      return nullptr;
  }
}

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