#include <devConf.h>
#include "ObjectFactory.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/boardtest/UartTestClass.h"

#include "mission/core/GenericFactory.h"
#include "mission/utility/TmFunnel.h"
#include <mission/devices/GPSHyperionHandler.h>
#include "mission/devices/GyroADIS16507Handler.h"

#include "fsfw/datapoollocal/LocalDataPoolManager.h"
#include "fsfw/tmtcservices/CommandingServiceBase.h"
#include "fsfw/tmtcservices/PusServiceBase.h"
#include "fsfw/tmtcpacket/pus/tm.h"
#include "fsfw/tasks/TaskFactory.h"

/* UDP server includes */
#if OBSW_USE_TMTC_TCP_BRIDGE == 1
#include <fsfw/src/fsfw/osal/common/TcpTmTcBridge.h>
#include <fsfw/src/fsfw/osal/common/TcpTmTcServer.h>
#else
#include "fsfw/osal/common/UdpTmTcBridge.h"
#include "fsfw/osal/common/UdpTcPollingTask.h"
#endif

#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
#include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h"
#include "fsfw_hal/linux/rpi/GpioRPi.h"
#include "fsfw_hal/common/gpio/GpioCookie.h"
#include "fsfw_hal/linux/spi/SpiCookie.h"
#include "fsfw_hal/linux/spi/SpiComIF.h"
#include <fsfw_hal/linux/uart/UartComIF.h>
#include <fsfw_hal/linux/uart/UartCookie.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::TMTC_BRIDGE;
    // No storage object for now.
    TmFunnel::storageDestination = objects::NO_OBJECT;

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



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

#if OBSW_USE_TMTC_TCP_BRIDGE == 1
    auto tmtcBridge = new TcpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR);
    tmtcBridge->setMaxNumberOfPacketsStored(50);
    new TcpTmTcServer(objects::TMTC_POLLING_TASK, objects::TMTC_BRIDGE);
#else
    auto tmtcBridge = new UdpTmTcBridge(objects::TMTC_BRIDGE, objects::CCSDS_PACKET_DISTRIBUTOR);
    tmtcBridge->setMaxNumberOfPacketsStored(50);
    new UdpTcPollingTask(objects::TMTC_POLLING_TASK, objects::TMTC_BRIDGE);
#endif

    GpioIF* gpioIF = new LinuxLibgpioIF(objects::GPIO_IF);
    GpioCookie* gpioCookie = nullptr;
    static_cast<void>(gpioCookie);

    new SpiComIF(objects::SPI_COM_IF, gpioIF);

    std::string spiDev;
    SpiCookie* spiCookie = nullptr;
    static_cast<void>(spiCookie);

#if OBSW_ADD_ACS_BOARD == 1
    if(gpioCookie == nullptr) {
        gpioCookie = new GpioCookie();
    }
    // TODO: Missing pin for Gyro 2
    gpio::createRpiGpioConfig(gpioCookie, gpioIds::MGM_0_LIS3_CS, gpio::MGM_0_BCM_PIN,
            "MGM_0_LIS3", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookie, gpioIds::MGM_1_RM3100_CS, gpio::MGM_1_BCM_PIN,
            "MGM_1_RM3100", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookie, gpioIds::MGM_2_LIS3_CS, gpio::MGM_2_BCM_PIN,
            "MGM_2_LIS3", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookie, gpioIds::MGM_3_RM3100_CS, gpio::MGM_3_BCM_PIN,
            "MGM_3_RM3100", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookie, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN,
            "GYRO_0_ADIS", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookie, gpioIds::GYRO_1_L3G_CS, gpio::GYRO_1_BCM_PIN,
            "GYRO_1_L3G", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookie, gpioIds::GYRO_2_ADIS_CS, gpio::GYRO_2_BCM_PIN,
            "GYRO_2_ADIS", gpio::Direction::OUT, 1);
    gpio::createRpiGpioConfig(gpioCookie, gpioIds::GYRO_3_L3G_CS, gpio::GYRO_3_BCM_PIN,
            "GYRO_3_L3G", gpio::Direction::OUT, 1);
    gpioIF->addGpios(gpioCookie);

    spiDev = "/dev/spidev0.1";
    spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, spiDev,
            MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
    auto mgmLis3Handler = new MgmLIS3MDLHandler(objects::MGM_0_LIS3_HANDLER,
            objects::SPI_COM_IF, spiCookie, 0);
    mgmLis3Handler->setStartUpImmediately();
#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1
    mgmLis3Handler->setToGoToNormalMode(true);
#endif

    spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev,
            RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
    auto mgmRm3100Handler = new MgmRM3100Handler(objects::MGM_1_RM3100_HANDLER,
            objects::SPI_COM_IF, spiCookie, 0);
    mgmRm3100Handler->setStartUpImmediately();
#if FSFW_HAL_RM3100_MGM_DEBUG == 1
    mgmRm3100Handler->setToGoToNormalMode(true);
#endif

    spiCookie = new SpiCookie(addresses::MGM_2_LIS3, gpioIds::MGM_2_LIS3_CS, spiDev,
            MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
    mgmLis3Handler = new MgmLIS3MDLHandler(objects::MGM_2_LIS3_HANDLER,
            objects::SPI_COM_IF, spiCookie, 0);
    mgmLis3Handler->setStartUpImmediately();
#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1
    mgmLis3Handler->setToGoToNormalMode(true);
#endif

    spiCookie = new SpiCookie(addresses::MGM_3_RM3100, gpioIds::MGM_3_RM3100_CS, spiDev,
            RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
    mgmRm3100Handler = new MgmRM3100Handler(objects::MGM_3_RM3100_HANDLER,
            objects::SPI_COM_IF, spiCookie, 0);
    mgmRm3100Handler->setStartUpImmediately();
#if FSFW_HAL_RM3100_MGM_DEBUG == 1
    mgmRm3100Handler->setToGoToNormalMode(true);
#endif

    spiCookie = new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, spiDev,
            L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
    auto adisHandler = new GyroADIS16507Handler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF,
            spiCookie);
    adisHandler->setStartUpImmediately();
    spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev,
            L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
    auto gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_1_L3G_HANDLER, objects::SPI_COM_IF,
            spiCookie, 0);
    gyroL3gHandler->setStartUpImmediately();
#if FSFW_HAL_L3GD20_GYRO_DEBUG== 1
    gyroL3gHandler->setToGoToNormalMode(true);
#endif

    spiCookie = new SpiCookie(addresses::GYRO_2_ADIS, gpioIds::GYRO_2_ADIS_CS, spiDev,
            L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
    adisHandler = new GyroADIS16507Handler(objects::GYRO_2_ADIS_HANDLER, objects::SPI_COM_IF,
            spiCookie);
    adisHandler->setStartUpImmediately();

    spiCookie = new SpiCookie(addresses::GYRO_3_L3G, gpioIds::GYRO_3_L3G_CS, spiDev,
            L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
    gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_3_L3G_HANDLER, objects::SPI_COM_IF,
            spiCookie, 0);
    gyroL3gHandler->setStartUpImmediately();
#if FSFW_HAL_L3GD20_GYRO_DEBUG== 1
    gyroL3gHandler->setToGoToNormalMode(true);
#endif

#endif /* RPI_TEST_ACS_BOARD == 1 */

#if OBSW_ADD_TEST_CODE == 1
    createTestTasks();
#endif /* OBSW_ADD_TEST_CODE == 1 */
}

void ObjectFactory::createTestTasks() {

    new TestTask(objects::TEST_TASK);

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

#if RPI_ADD_UART_TEST == 1
    new UartTestClass(objects::UART_TEST);
#else
    new UartComIF(objects::UART_COM_IF);
#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 */

#if RPI_TEST_ADIS16507 == 1
    if(gpioCookie == nullptr) {
        gpioCookie = new GpioCookie();
    }
    gpio::createRpiGpioConfig(gpioCookie, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN,
            "GYRO_0_ADIS", gpio::Direction::OUT, 1);
    gpioIF->addGpios(gpioCookie);

    spiDev = "/dev/spidev0.1";
    spiCookie = new SpiCookie(addresses::GYRO_0_ADIS, gpioIds::GYRO_0_ADIS_CS, spiDev,
            ADIS16507::MAXIMUM_REPLY_SIZE, spi::DEFAULT_ADIS16507_MODE, spi::DEFAULT_ADIS16507_SPEED,
            nullptr, nullptr);
    auto adisGyroHandler = new GyroADIS16507Handler(objects::GYRO_0_ADIS_HANDLER, objects::SPI_COM_IF, spiCookie);
    adisGyroHandler->setStartUpImmediately();
#endif /* RPI_TEST_ADIS16507 == 1 */

#if RPI_TEST_GPS_HANDLER == 1
    UartCookie* uartCookie = new UartCookie(objects::GPS0_HANDLER, "/dev/serial0",
            UartModes::CANONICAL, 9600, 1024);
    uartCookie->setToFlushInput(true);
    uartCookie->setReadCycles(6);
    GPSHyperionHandler* gpsHandler = new GPSHyperionHandler(objects::GPS0_HANDLER,
            objects::UART_COM_IF, uartCookie, false);
    gpsHandler->setStartUpImmediately();
#endif

}