#include "ObjectFactory.h"
#include <bsp_rpi/gpio/GPIORPi.h>

#include <objects/systemObjectList.h>
#include <devices/addresses.h>
#include <devices/gpioIds.h>
#include <OBSWConfig.h>
#include <tmtc/apid.h>
#include <tmtc/pusIds.h>

#include <linux/boardtest/LibgpiodTest.h>
#include <linux/boardtest/SpiTestClass.h>
#include <linux/gpio/GpioCookie.h>
#include <linux/gpio/LinuxLibgpioIF.h>

#include <linux/spi/SpiCookie.h>

#include <mission/core/GenericFactory.h>
#include <mission/utility/TmFunnel.h>
#include <mission/devices/MGMHandlerLIS3MDL.h>

#include <fsfw/datapoollocal/LocalDataPoolManager.h>
#include <fsfw/tmtcservices/CommandingServiceBase.h>
#include <fsfw/tmtcservices/PusServiceBase.h>
#include <fsfw/osal/linux/TmTcUnixUdpBridge.h>
#include <fsfw/tmtcpacket/pus/TmPacketStored.h>
#include <fsfw/osal/linux/TcUnixUdpPollingTask.h>
#include <fsfw/tasks/TaskFactory.h>
#include <linux/spi/SpiComIF.h>
#include <mission/devices/MGMHandlerRM3100.h>

void Factory::setStaticFrameworkObjectIds() {
    PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR;
    PusServiceBase::packetDestination = objects::TM_FUNNEL;

    CommandingServiceBase::defaultPacketSource = objects::PUS_PACKET_DISTRIBUTOR;
    CommandingServiceBase::defaultPacketDestination = objects::TM_FUNNEL;

    TmFunnel::downlinkDestination = objects::UDP_BRIDGE;
    // No storage object for now.
    TmFunnel::storageDestination = objects::NO_OBJECT;

    LocalDataPoolManager::defaultHkDestination = objects::NO_OBJECT;

    VerificationReporter::messageReceiver = objects::PUS_SERVICE_1_VERIFICATION;
    TmPacketStored::timeStamperId = objects::TIME_STAMPER;
}



void ObjectFactory::produce(){
    Factory::setStaticFrameworkObjectIds();
    ObjectFactory::produceGenericObjects();

    new TmTcUnixUdpBridge(objects::UDP_BRIDGE,
            objects::CCSDS_PACKET_DISTRIBUTOR,
            objects::TM_STORE, objects::TC_STORE);
    new TcUnixUdpPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE);

    GpioIF* gpioIF = new LinuxLibgpioIF(objects::GPIO_IF);

#if RPI_ADD_SPI_TEST == 1
    new SpiTestClass(objects::SPI_TEST, gpioIF);
#endif

#if RPI_LOOPBACK_TEST_GPIO == 1
    GpioCookie* gpioCookieLoopback = new GpioCookie();
    /* Loopback pins. Adapt according to setup */
    gpioId_t gpioIdSender = gpioIds::TEST_ID_0;
    int bcmPinSender = 26;
    gpioId_t gpioIdReader = gpioIds::TEST_ID_1;
    int bcmPinReader = 16;
    gpio::createRpiGpioConfig(gpioCookieLoopback, gpioIdSender, bcmPinSender, "GPIO_LB_SENDER",
            gpio::Direction::OUT, 0);
    gpio::createRpiGpioConfig(gpioCookieLoopback, gpioIdReader, bcmPinReader, "GPIO_LB_READER",
            gpio::Direction::IN, 0);
    new LibgpiodTest(objects::LIBGPIOD_TEST, objects::GPIO_IF, gpioCookieLoopback);
#endif /* RPI_LOOPBACK_TEST_GPIO == 1 */

    new SpiComIF(objects::SPI_COM_IF, gpioIF);

#if RPI_TEST_ACS_BOARD == 1

    GpioCookie* gpioCookieAcsBoard = new GpioCookie();
    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_0_LIS3_CS, gpio::MGM_0_BCM_PIN,
            "MGM_0_LIS3", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_1_RM3100_CS, gpio::MGM_1_BCM_PIN,
            "MGM_1_RM3100", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_2_LIS3_CS, gpio::MGM_2_BCM_PIN,
            "MGM_2_LIS3", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_3_RM3100_CS, gpio::MGM_3_BCM_PIN,
            "MGM_3_RM3100", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN,
            "GYRO_0_ADIS", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_1_L3G_CS, gpio::GYRO_1_BCM_PIN,
            "GYRO_1_L3G", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_2_L3G_CS, gpio::GYRO_2_BCM_PIN,
            "GYRO_2_L3G", gpio::Direction::OUT, 1);
    gpioIF->addGpios(gpioCookieAcsBoard);

    std::string spiDev = "/dev/spidev0.0";
    SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3,
            gpioIds::MGM_0_LIS3_CS, spiDev, 24, spi::SpiMode::MODE_3, 3'900'000);
    auto mgmLis3Handler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER,
            objects::SPI_COM_IF, spiCookie);
    mgmLis3Handler->setStartUpImmediately();
    spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, 32,
    		spi::SpiMode::MODE_3, 976'000);
    auto mgmRm3100Handler = new MGMHandlerRM3100(objects::MGM_1_RM3100_HANDLER,
            objects::SPI_COM_IF, spiCookie);
    mgmRm3100Handler->setStartUpImmediately();

#endif /* RPI_TEST_ACS_BOARD == 1 */
}