#include <mission/tmtc/PusPacketFilter.h>

#include <algorithm>

PusPacketFilter::PusPacketFilter() {}

void PusPacketFilter::addApid(uint16_t apid) {
  if (not this->apid) {
    this->apid = std::vector<uint16_t>({apid});
    return;
  }
  this->apid.value().push_back(apid);
}

void PusPacketFilter::addService(uint8_t service) {
  if (not this->services) {
    this->services = std::vector<uint8_t>({service});
    return;
  }
  this->services.value().push_back(service);
}

void PusPacketFilter::addServiceSubservice(uint8_t service, uint8_t subservice) {
  if (not serviceSubservices) {
    serviceSubservices = std::vector<std::pair<uint8_t, uint8_t>>({std::pair(service, subservice)});
    return;
  }
  serviceSubservices.value().emplace_back(service, subservice);
}

bool PusPacketFilter::packetMatches(PusTmReader& reader) const {
  bool inApidList = false;
  if (apid) {
    auto& apidFilter = apid.value();
    if (std::find(apidFilter.begin(), apidFilter.end(), reader.getApid()) != apidFilter.end()) {
      if (not serviceSubservices and not services) {
        return true;
      }
      inApidList = true;
    }
  }
  std::pair<uint8_t, uint8_t> serviceSubservice;
  serviceSubservice.first = reader.getService();
  serviceSubservice.second = reader.getSubService();
  if (services) {
    auto& serviceFilter = services.value();
    if (std::find(serviceFilter.begin(), serviceFilter.end(), serviceSubservice.first) !=
        serviceFilter.end()) {
      if (apid and inApidList) {
        return true;
      }
    }
  }
  if (serviceSubservices) {
    auto& serviceSubserviceFilter = serviceSubservices.value();
    if (std::find(serviceSubserviceFilter.begin(), serviceSubserviceFilter.end(),
                  serviceSubservice) != serviceSubserviceFilter.end()) {
      if (apid and inApidList) {
        return true;
      }
    }
  }
  return false;
}