/**
 * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
 * distributed under the terms of the BSD-3-Clause license.
 */

#include <cstdlib>
#include <chrono>
#include <filesystem>
#include <random>
#include <thread>

// BLF
#include <BipedalLocomotion/TextLogging/Logger.h>

// Catch2
#include <catch2/catch_test_macros.hpp>

// YARP
#include <yarp/sig/Vector.h>
#include <yarp/eigen/Eigen.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/Network.h>
#include <yarp/robotinterface/XMLReader.h>
#include <yarp/dev/IEncoders.h>
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
#include <yarp/dev/PolyDriver.h>

// matioCpp
#include <matioCpp/matioCpp.h>

// This function populate the YARP_DATA_DIRS environment variable to
// ensure that YARP devices from YARP (from YARP install prefix) and BLF (from the build directory of BLF)
// can be correctly found and launched by libYARP_robotinterface
// Once this is used in multiple tests, we can move it to a common place to share it among tests
bool blf_setenv(const std::string &_name, const std::string &_value)
{
#ifdef _WIN32
    if (0 != _putenv_s(_name.c_str(), _value.c_str()))
    {
        return false;
    }
#else
    if (0 != ::setenv(_name.c_str(), _value.c_str(), true))
    {
        return false;
    }
#endif
    return true;
}


bool ensureYARPAndBLFYARPDevicesCanBeFound()
{
    // To make sure that YarpRobotLoggerDevice is found from the build directory, we add CMAKE_BINARY_DIR
    // to YARP_DATA_DIRS
#ifdef _WIN32
    std::string envVarListSeparator = ";";
#else
    std::string envVarListSeparator = ":";
#endif

    // Make sure that YARP devices can be found
    std::string new_yarp_data_dirs_value = YARP_DATA_INSTALL_DIR_FULL;

    // Make sure that BLF devices are available
    new_yarp_data_dirs_value = new_yarp_data_dirs_value + envVarListSeparator + CMAKE_BINARY_DIR + "/share/yarp";

    return blf_setenv("YARP_DATA_DIRS", new_yarp_data_dirs_value);
}

TEST_CASE("Launch simple logger")
{
    // This test launched a dummy logger and verify that it effectively saves some data
    // Locate location of .exe file of ipit-hipac-codesys-embobj-bridge, done in getDirectoryOfExecutable()

    // As a preliminary step, we rename any leftover .md and .mat files contained in the current working directory
    // to .md.bak and .mat.bak, to ensure that they are not mistakenly files generated by this tests
    std::filesystem::path currentPath = std::filesystem::current_path();
    for (const auto& entry : std::filesystem::directory_iterator(currentPath)) {
        if (entry.is_regular_file()) {
            std::filesystem::path filePath = entry.path();
            std::string extension = filePath.extension().string();

            if (extension == ".mat" || extension == ".md") {
                std::filesystem::path newFilePath = filePath;
                newFilePath += ".bak";
                std::filesystem::rename(filePath, newFilePath);
            }
        }
    }

    // We will be opening yarp ports in the tests, but we do not want to run the yarpserver, so let's set YARP
    // in local mode (everything will work fine as long as all the ports are opened in the same process)
    yarp::os::Network network;
    yarp::os::NetworkBase::setLocalMode(true);

    // We ensure that YARP_DATA_DIRS contains the values necessary to find YARP and BLF devices
    REQUIRE(ensureYARPAndBLFYARPDevicesCanBeFound());

    // We hardcode the yarprobotinterface configuration file is in the source directory and passed via
    // target_compile_definitions
    std::filesystem::path pathToXmlConfigurationFile = std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) / std::filesystem::path("launch-yarp-robot-logger.xml");

    BipedalLocomotion::log()->info("Loading yarprobotinterface file from {}", pathToXmlConfigurationFile.string());

    // Boilerplate to add enable_tags or disable_tags as necessary
    yarp::os::Property yarprobotinterfaceConfig;
    yarp::os::Bottle enableTags;
    yarp::os::Bottle& enableTagsList = enableTags.addList();
    yarp::os::Bottle disableTags;
    yarp::os::Bottle& disableTagsList = disableTags.addList();

    if (enableTagsList.size() > 0)
    {
        yarprobotinterfaceConfig.put("enable_tags", enableTags.get(0));
    }

    if (disableTagsList.size() > 0)
    {
        yarprobotinterfaceConfig.put("disable_tags", disableTags.get(0));
    }

    yarp::robotinterface::XMLReader yarprobotinterfaceReader;
    yarp::robotinterface::XMLReaderResult yarprobotinterfaceInstance = yarprobotinterfaceReader.getRobotFromFile(pathToXmlConfigurationFile.string(), yarprobotinterfaceConfig);

    REQUIRE(yarprobotinterfaceInstance.parsingIsSuccessful);

    // Enter the startup phase, that will open all the devices and  call attach if necessary
    REQUIRE(yarprobotinterfaceInstance.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup));

    // At this point, the system is running and the thread that called the
    // enterPhase methods does not need to do anything else

    BipedalLocomotion::log()->info("yarprobotinterface successfully loaded and started.");

    // Sleep 0.2 second to collect some data
    std::this_thread::sleep_for(std::chrono::milliseconds(200));

    // Now that some time passed, let's read some values that will compare against the values that we will read from the logger, and then close the robotinterface
    double sim_joint_1_pos_read = 0.0;
    double accelerometer_z_read = 0.0;

    // Make sure that the requires devices are there
    // These strings are the name attributes of the device tags specified in launch-yarp-robot-logger.xml
    CHECK(yarprobotinterfaceInstance.robot.hasDevice("sim_controlboard"));
    CHECK(yarprobotinterfaceInstance.robot.hasDevice("sim_imu"));

    // Read encoders
    yarp::dev::IEncoders* iencs=nullptr;
    CHECK(yarprobotinterfaceInstance.robot.device("sim_controlboard").driver()->view(iencs));
    REQUIRE(iencs != nullptr);
    Eigen::VectorXd jointPosRead;
    size_t nrOfJoints = 4;
    jointPosRead.resize(nrOfJoints);
    CHECK(iencs->getEncoders(jointPosRead.data()));

    // Read accelerometer
    yarp::dev::IThreeAxisLinearAccelerometers* ilinacc=nullptr;
    CHECK(yarprobotinterfaceInstance.robot.device("sim_imu").driver()->view(ilinacc));
    REQUIRE(ilinacc != nullptr);
    yarp::sig::Vector accelerometerReadYARP;
    Eigen::VectorXd accelerometerRead;
    size_t nrOfDirectionsInAccelerometer = 3;
    accelerometerRead.resize(nrOfDirectionsInAccelerometer);
    // The device only has one sensor
    size_t sensIndex = 0;
    double timestamp;
    CHECK(ilinacc->getThreeAxisLinearAccelerometerMeasure(sensIndex, accelerometerReadYARP, timestamp));
    accelerometerRead = yarp::eigen::toEigen(accelerometerReadYARP);


    BipedalLocomotion::log()->info("Halting yarprobotinterface.");
    REQUIRE(yarprobotinterfaceInstance.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1));
    REQUIRE(yarprobotinterfaceInstance.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown));

    // This call is commented as otherwise it can result in segfault, see https://github.com/ami-iit/bipedal-locomotion-framework/issues/872
    // BipedalLocomotion::log()->info("yarprobotinterface successfully halted.");

    // Now that the yarprobotinterface has been halted, let's open our .mat files and check if the log is working
    currentPath = std::filesystem::current_path();
    std::string matLogFilename;
    for (const auto& entry : std::filesystem::directory_iterator(currentPath)) {
        if (entry.is_regular_file()) {
            std::filesystem::path filePath = entry.path();
            std::string extension = filePath.extension().string();

            if (extension == ".mat") {
                matLogFilename = filePath.string();
            }
        }
    }

    // This call is commented as otherwise it can result in segfault, see https://github.com/ami-iit/bipedal-locomotion-framework/issues/872
    // BipedalLocomotion::log()->info("Loading saved log from {}", matLogFilename);
    matioCpp::File savedLog(matLogFilename);

    REQUIRE(savedLog.isOpen());

    matioCpp::Struct robotLoggerDeviceStruct = savedLog.read("robot_logger_device").asStruct();

    matioCpp::MultiDimensionalArray<double> jointPosLoggedData = robotLoggerDeviceStruct["joints_state"].asStruct()["positions"].asStruct()["data"].asMultiDimensionalArray<double>();
    // The values are constant, so we can just check the first one
    // The size of the each vector is the number of joints (nrOfJoints)
    Eigen::VectorXd jointPosLogged;
    jointPosLogged.resize(nrOfJoints);
    for(size_t i=0; i < jointPosLogged.size(); i++)
    {
        jointPosLogged(i) = jointPosLoggedData({i,0,0});
    }

    REQUIRE(jointPosLogged.isApprox(jointPosRead,  1e-14));

    // The values are constant, so we can just check the first one
    matioCpp::MultiDimensionalArray<double> accelerometerLoggedData = robotLoggerDeviceStruct["accelerometers"].asStruct()["sim_imu_sensor"].asStruct()["data"].asMultiDimensionalArray<double>();
    // The size of the vector is the number of readings of accelerometers (3)
    Eigen::VectorXd accelerometerLogged;
    accelerometerLogged.resize(nrOfDirectionsInAccelerometer);
    for(size_t i=0; i < accelerometerLogged.size(); i++)
    {
        accelerometerLogged(i) = accelerometerLoggedData({i,0,0});
    }

    REQUIRE(accelerometerLogged.isApprox(accelerometerRead,  1e-14));

    // The z component should be near to -9.8
    CHECK(std::abs(accelerometerLogged(2) - (-9.8)) <= 1.0);
}