#include <iostream>
#include <thread>
#include <gtest/gtest.h>

#include "yaml-cpp/yaml.h"

// Messages
#include "dds/SystemHealthMonitoring.hpp"
#include "dds/MsgNovAtel.hpp"

#include "test_nomap.h"


FMEADDS* TestBase::pC = NULL;
TestBase::TestBase(){}
TestBase::~TestBase(){}
void TestBase::TearDown(){}

void TestBase::SetUpTestCase()
{
    std::cout<<"\n\n---------- NO MAP TEST ----------"<<std::endl;
    std::cout <<"Set up test cases."<<std::endl;
    if (pC != NULL)
    {
        delete pC;
    }

    pC = new FMEADDS;
}

void TestBase::SetUp()
{
    pC->invalid_status = -1000;
    pC->first_status = pC->invalid_status;
    pC->last_status = pC->invalid_status;
    pC->counter = 0;
}

void TestBase::TearDownTestCase()
{
    std::cout<<"Tear down test cases."<<std::endl;
    delete pC; pC = NULL;
}

FMEADDS::FMEADDS()
{
    initialization();
    domainCreation();
}

void FMEADDS::initialization()
{
    package_path = getPackagePath();
    std::string config_yaml = package_path + "/test/dds_test/config/FMEA_dummy.yaml";

    YAML::Node config = YAML::LoadFile(config_yaml);
    auto nodeFMEA = config["FMEA"];

    try
    {
        error_id = nodeFMEA["statusID"]["error"].as<int>();
        init_id = nodeFMEA["statusID"]["init"].as<int>();
        normal_id = nodeFMEA["statusID"]["normal"].as<int>();
        warning_id = nodeFMEA["statusID"]["warning"].as<int>();

        failure_bad_init = nodeFMEA["failureID"]["bad_initialization"].as<int>();
        failure_bad_matching = nodeFMEA["failureID"]["bad_matching"].as<int>();
        failure_no_map = nodeFMEA["failureID"]["no_map"].as<int>();
        failure_low_freq = nodeFMEA["failureID"]["low_frequency"].as<int>();
        failure_large_jump = nodeFMEA["failureID"]["large_jump"].as<int>();
        failure_high_cov = nodeFMEA["failureID"]["high_cov"].as<int>();
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << '\n';
        return;
    }

    std::string config_domain = package_path + "/test/dds_test/config/domain_dummy.yaml";
    YAML::Node domain_config = YAML::LoadFile(config_domain);
    auto nodeDomain = domain_config["hdl_localization"];

    try
    {
        vi_domain_id = nodeDomain["viDomain"].as<int>();
        rtk_domain_id = nodeDomain["rtkDomain"].as<int>();
        lidar_domain_id = nodeDomain["lidarDomain"].as<int>();
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << '\n';
        return;
    }
}

void FMEADDS::domainCreation()
{
    package_path = getPackagePath();
    std::cout<<"-------INITIATE DOMAIN PARTICIPANT------"<<std::endl;
    std::string qosPath = package_path + "/config/qos_loc.xml";
    qosProvider = dds::core::QosProvider(qosPath, "SHM_LOC_Library::SHM_LOC_Profile");

    shmParticipant = dds::domain::DomainParticipant(0, qosProvider.participant_qos("::SHM_LOC_Profile::SHM_LOC_Participant"));
    rtkOutParticipant = dds::domain::DomainParticipant(rtk_domain_id, qosProvider.participant_qos("::SHM_LOC_Profile::SHM_LOC_Participant"));
}

void mainPublisher(dds::pub::DataWriter<msgNovAtelPVA> &rtkWriter, int &counter, bool &finished)
{
    auto start = chrono::steady_clock::now();

    while(counter != 10)
    {
        std::cout<<"Pub RTK: "<<chrono::duration<double, std::milli>(chrono::steady_clock::now() - start).count() <<std::endl;
        msgNovAtelPVA rtkMsg;
        rtkMsg.lat(2.5);
        rtkMsg.lon(-2.5);
        rtkWriter.write(rtkMsg);
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    finished = true;
}

int FMEADDS::mainSubscriber()
{
    // Subscriber
    dds::topic::Topic<msgSHM> shmTopic(shmParticipant, "msgSHM");
    dds::sub::DataReader<msgSHM> shmReader(dds::sub::Subscriber(shmParticipant), shmTopic);

    // Publisher
    const dds::pub::qos::DataWriterQos &rtkQOS = qosProvider.datawriter_qos("::SHM_LOC_Profile::SHM_LOC_DataWriter");
    dds::topic::Topic<msgNovAtelPVA> rtkOutTopic(rtkOutParticipant, "msgNovAtelPVA");
    dds::pub::DataWriter<msgNovAtelPVA> rtkWriter(dds::pub::Publisher(rtkOutParticipant), rtkOutTopic, rtkQOS, NULL, dds::core::status::StatusMask::all());
    
    counter = 0;
    dds::sub::cond::ReadCondition read_condition(
        shmReader,
        dds::sub::status::DataState::any(),
        [&shmReader, this]()
    {
        dds::sub::LoanedSamples<msgSHM> samples = shmReader.take();
        for (auto sample :samples){
            // std::cout<<"SHM Message received!!!"<<std::endl;
            if (sample.info().valid())
            {
                auto shm_data = sample.data();
                
                if (this->first_status == this->invalid_status)
                    this->first_status = shm_data.statusID();
                
                this->last_status = shm_data.statusID();
                this->curr_failure = shm_data.failureID()[0];
                // std::cout<<"last_status: "<<this->last_status<<"; curr_failure:"<<this->curr_failure<<std::endl;

            }
            counter++;
        }
    });

    dds::core::cond::WaitSet waitset;
    waitset += read_condition;
    bool finished = false;

    std::thread mainThread(mainPublisher, std::ref(rtkWriter), std::ref(counter), std::ref(finished));
    while( !finished )
    {   
        waitset.dispatch(dds::core::Duration(0.5));
    }

    mainThread.join();
    return 1;
}

TEST_F(TestBase, testNoMap)
{
    std::cout<<"---------TEST 1: PUBLISH RTK WHICH TRIGGER USE MAP THAT DOESNT EXIST"<<std::endl;
    pC->mainSubscriber();
    EXPECT_GE(pC->counter, 0);
    EXPECT_EQ(pC->last_status, pC->error_id);
    EXPECT_EQ(pC->curr_failure, pC->failure_no_map);
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}