#include "SensorHandlerOfAgent.h"
#include "printError.h"
#include <stdio.h>

#include <algorithm>

#define MAXNUMOF_MOUNTEDSENSORS 100

using namespace std;

SensorHandlerOfAgent::SensorHandlerOfAgent() { init_sensorHandler(); }

void SensorHandlerOfAgent::init_sensorHandler() {
  maxNumOf_mountedSensors = MAXNUMOF_MOUNTEDSENSORS;
}

// TODO: if(vMountedSensors.size() < maxNumOf_mountedSensors)  als aller erste
// Abfrage machen ... noch bevor "SensorSlotOfAgent* sensorSlotOfAgent = new
// SensorSlotOfAgent();"
// TODO: delete object if it cannot added to vector
bool SensorHandlerOfAgent::mount_sensorIntoSensorSlot(Channel *inputPort) {
  SensorSlotOfAgent *sensorSlotOfAgent = new SensorSlotOfAgent();
  if (sensorSlotOfAgent != NULL) {
    if (sensorSlotOfAgent->set_comPort(inputPort)) {
      try {
        if (vMountedSensors.size() < maxNumOf_mountedSensors) {
          vMountedSensors.push_back(sensorSlotOfAgent);
        } else {
          printError("Max number of mounted sensors is already reached!");

          return false;
        }
      } catch (bad_alloc &error) {
        printError("bad_alloc caught: ", error.what());
        return false;
      }
      return true;
    } else {
      printError("Input port is no set!");
      vMountedSensors.pop_back(); // TODO: check if it is right?!?!

      return false;
    }
  } else {
    printError("Couldn't create SensorSlot!");

    return false;
  }
}

// TODO: what to do when 2 sensorSlots have the same inputPort??!!
SensorSlotOfAgent *
SensorHandlerOfAgent::get_sensorSlotAddress(Channel *inputPort) {
  for (auto &sensorSlot : vMountedSensors) {
    if (sensorSlot->get_comPort() == inputPort) {
      return sensorSlot;
    }
  }
  return NULL;
}

// TODO: what to do when 2 slaveAgentSlots have the same inputPort??!!
// TODO: case if slot with comPort is not in this vector
unsigned int SensorHandlerOfAgent::get_sensorSlotNumber(Channel *inputPort) {
  unsigned int slotNumber = 0;
  for (auto &sensorSlot : vMountedSensors) {
    if (sensorSlot->get_comPort() == inputPort) {
      return slotNumber;
    }
    slotNumber++;
  }
  return 0;
}

// TODO: what to do when 2 sensorSlots have the same historyModule??!!
SensorSlotOfAgent *
SensorHandlerOfAgent::get_sensorSlotAddress(HistoryModule *historyModule) {
  for (auto &sensorSlot : vMountedSensors) {
    if (sensorSlot->get_historyModule() == historyModule) {
      return sensorSlot;
    }
  }
  return NULL;
}

// TODO: what to do when 2 sensorSlots have the same confidenceModule??!!
SensorSlotOfAgent *SensorHandlerOfAgent::get_sensorSlotAddress(
    ConfidenceModule *confidenceModule) {
  for (auto &sensorSlot : vMountedSensors) {
    if (sensorSlot->get_confidenceModule() == confidenceModule) {
      return sensorSlot;
    }
  }
  return NULL;
}

bool SensorHandlerOfAgent::demount_sensor(Channel *inputPort) {
  vMountedSensors.erase(vMountedSensors.begin() +
                        get_sensorSlotNumber(inputPort));
  return false;
}

// TODO: do it also for integer variables
bool SensorHandlerOfAgent::read_sensorValue(
    SensorSlotOfAgent *sensorSlotOfAgent) {
  if (sensorSlotOfAgent != NULL) {
    Channel *channel = sensorSlotOfAgent->get_comPort();
    if (channel != NULL) {
      float inputValue;
      if (channel->get_MsgUp(&inputValue)) {
        sensorSlotOfAgent->set_sensorValue(inputValue);
        return true;
      }
    }
  }
  return false;
}

bool SensorHandlerOfAgent::read_allSensorValues() {
  bool flag_readSensor = false;
  for (auto &sensorSlot : vMountedSensors) {
    if (read_sensorValue(sensorSlot)) {
      flag_readSensor = true;
    }
  }
  return flag_readSensor;
}

bool SensorHandlerOfAgent::attach_historyModule(Channel *inputPort,
                                                HistoryModule *historyModule) {
  SensorSlotOfAgent *sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
  if (sensorSlotOfAgent != NULL) {
    return sensorSlotOfAgent->set_historyModule(historyModule);
  }
  return false;
}

bool SensorHandlerOfAgent::detach_historyModule(
    SensorSlotOfAgent *sensorSlotOfAgent) {
  if (sensorSlotOfAgent != NULL) {
    return sensorSlotOfAgent->del_historyModule();
  }
  return false;
}

bool SensorHandlerOfAgent::detach_historyModule(Channel *inputPort) {
  SensorSlotOfAgent *sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
  return detach_historyModule(sensorSlotOfAgent);
}

bool SensorHandlerOfAgent::detach_historyModule(HistoryModule *historyModule) {
  SensorSlotOfAgent *sensorSlotOfAgent = get_sensorSlotAddress(historyModule);
  return detach_historyModule(sensorSlotOfAgent);
}

HistoryModule *
SensorHandlerOfAgent::get_historyModuleOfSensorSlot(Channel *inputPort) {
  SensorSlotOfAgent *sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
  return sensorSlotOfAgent->get_historyModule();
}

bool SensorHandlerOfAgent::attach_confidenceModule(
    Channel *inputPort, ConfidenceModule *confidenceModule) {
  SensorSlotOfAgent *sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
  if (sensorSlotOfAgent != NULL) {
    return sensorSlotOfAgent->set_confidenceModule(confidenceModule);
  }
  return false;
}

bool SensorHandlerOfAgent::detach_confidenceModule(
    SensorSlotOfAgent *sensorSlotOfAgent) {
  if (sensorSlotOfAgent != NULL) {
    return sensorSlotOfAgent->del_confidenceModule();
  }
  return false;
}

bool SensorHandlerOfAgent::detach_confidenceModule(Channel *inputPort) {
  SensorSlotOfAgent *sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
  return detach_confidenceModule(sensorSlotOfAgent);
}

bool SensorHandlerOfAgent::detach_confidenceModule(
    ConfidenceModule *confidenceModule) {
  SensorSlotOfAgent *sensorSlotOfAgent =
      get_sensorSlotAddress(confidenceModule);
  return detach_confidenceModule(sensorSlotOfAgent);
}

ConfidenceModule *
SensorHandlerOfAgent::get_confidenceModuleOfSensorSlot(Channel *inputPort) {
  SensorSlotOfAgent *sensorSlotOfAgent = get_sensorSlotAddress(inputPort);
  return sensorSlotOfAgent->get_confidenceModule();
}

vector<SensorSlotOfAgent *> *SensorHandlerOfAgent::get_vMountedSensors() {
  return &vMountedSensors;
}