#include "LibgpiodTest.h"

#include <fsfw/objectmanager/ObjectManager.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <fsfw/tasks/TaskFactory.h>

#include "devices/gpioIds.h"

LibgpiodTest::LibgpiodTest(object_id_t objectId, object_id_t gpioIfobjectId, GpioCookie* gpioCookie)
    : TestTask(objectId) {
  gpioInterface = ObjectManager::instance()->get<GpioIF>(gpioIfobjectId);
  if (gpioInterface == nullptr) {
    sif::error << "LibgpiodTest::LibgpiodTest: Invalid Gpio interface." << std::endl;
  }
  gpioInterface->addGpios(gpioCookie);
  testCase = TestCases::BLINK;
}

LibgpiodTest::~LibgpiodTest() {}

ReturnValue_t LibgpiodTest::performPeriodicAction() {
  gpio::Levels gpioState;
  ReturnValue_t result;

  switch (testCase) {
    case (TestCases::READ): {
      result = gpioInterface->readGpio(gpioIds::TEST_ID_0, gpioState);
      if (result != returnvalue::OK) {
        sif::warning << "LibgpiodTest::performPeriodicAction: Failed to read gpio " << std::endl;
        return returnvalue::FAILED;
      } else {
        sif::debug << "LibgpiodTest::performPeriodicAction: MIO 0 state = "
                   << static_cast<int>(gpioState) << std::endl;
      }
      break;
    }
    case (TestCases::LOOPBACK): {
      break;
    }
    case (TestCases::BLINK): {
      result = gpioInterface->readGpio(gpioIds::TEST_ID_0, gpioState);
      if (result != returnvalue::OK) {
        sif::warning << "LibgpiodTest::performPeriodicAction: Failed to read gpio " << std::endl;
        return returnvalue::FAILED;
      }
      if (gpioState == gpio::Levels::HIGH) {
        result = gpioInterface->pullLow(gpioIds::TEST_ID_0);
        if (result != returnvalue::OK) {
          sif::warning << "LibgpiodTest::performPeriodicAction: Could not pull GPIO low!"
                       << std::endl;
          return returnvalue::FAILED;
        }
      } else if (gpioState == gpio::Levels::LOW) {
        result = gpioInterface->pullHigh(gpioIds::TEST_ID_0);
        if (result != returnvalue::OK) {
          sif::warning << "LibgpiodTest::performPeriodicAction: Could not pull GPIO high!"
                       << std::endl;
          return returnvalue::FAILED;
        }
      } else {
        sif::warning << "LibgpiodTest::performPeriodicAction: Invalid GPIO state" << std::endl;
      }

      break;
    }
    default:
      sif::debug << "LibgpiodTest::performPeriodicAction: Invalid test case" << std::endl;
      break;
  }

  return returnvalue::OK;
}

ReturnValue_t LibgpiodTest::performOneShotAction() {
  gpio::Levels gpioState;
  ReturnValue_t result;

  switch (testCase) {
    case (TestCases::READ): {
      break;
    }
    case (TestCases::BLINK): {
      break;
    }
    case (TestCases::LOOPBACK): {
      result = gpioInterface->pullHigh(gpioIds::TEST_ID_0);
      if (result == returnvalue::OK) {
        sif::info << "LibgpiodTest::performOneShotAction: "
                     "GPIO pulled high successfully for loopback test"
                  << std::endl;
      } else {
        sif::warning << "LibgpiodTest::performOneShotAction: Could not pull GPIO high!"
                     << std::endl;
        return returnvalue::OK;
      }
      result = gpioInterface->readGpio(gpioIds::TEST_ID_1, gpioState);
      if (result == returnvalue::OK and gpioState == gpio::Levels::HIGH) {
        sif::info << "LibgpiodTest::performOneShotAction: "
                     "GPIO state read successfully and is high"
                  << std::endl;
      } else {
        sif::warning << "LibgpiodTest::performOneShotAction: GPIO read and is not high!"
                     << std::endl;
        return returnvalue::OK;
      }

      result = gpioInterface->pullLow(gpioIds::TEST_ID_0);
      if (result == returnvalue::OK) {
        sif::info << "LibgpiodTest::performOneShotAction: "
                     "GPIO pulled low successfully for loopback test"
                  << std::endl;
      }
      result = gpioInterface->readGpio(gpioIds::TEST_ID_1, gpioState);
      if (result == returnvalue::OK and gpioState == gpio::Levels::LOW) {
        sif::info << "LibgpiodTest::performOneShotAction: "
                     "GPIO state read successfully and is low"
                  << std::endl;
      } else {
        sif::warning << "LibgpiodTest::performOneShotAction: GPIO read and is not low!"
                     << std::endl;
        return returnvalue::OK;
      }
      break;
    }
  }
  return returnvalue::OK;
}