PathTracer2D.hpp

 
#ifndef PATH_TRACER_2D_HPP
#define PATH_TRACER_2D_HPP
 
#include "../GeospatialOperations.hpp"
 
#include <chrono>
#include <matplot/matplot.h>
 
 
 
namespace logging
{
 
    namespace graphing
    {
 
        class PathTracer
        {
            public:
 
                PathTracer(const std::string& szPlotTitle = "Graph")
                {
                    // Initialize member variables.
                    m_mtRoverPathPlot = matplot::figure(true);
                    m_mtRoverPathAxes = m_mtRoverPathPlot->current_axes();
                    m_szPlotTitle     = szPlotTitle;
 
                    // Check if a file with the same title name already exists. If so then append a number to the end of the file name and recheck.
                    std::string szPlotSavePath = logging::g_szLoggingOutputPath + "/path_plots/" + m_szPlotTitle;
                    m_szCurrentPlotSavePath    = logging::g_szLoggingOutputPath + "/path_plots/CurrentPlot.png";
                    int nFileNum               = 0;
                    while (std::filesystem::exists(szPlotSavePath + std::to_string(nFileNum) + ".png"))
                    {
                        ++nFileNum;
                    }
                    // Add the file number to the file name.
                    szPlotSavePath = szPlotSavePath + std::to_string(nFileNum);
                    // Check if the final directory exists. If not then create it.
                    if (!std::filesystem::exists(logging::g_szLoggingOutputPath + "/path_plots"))
                    {
                        std::filesystem::create_directory(logging::g_szLoggingOutputPath + "/path_plots");
                    }
 
                    // Configure the matplotplusplus gnuplot backend to not display the plot, instead save it to a file.
                    m_mtRoverPathPlot->backend()->output(szPlotSavePath + ".png");
 
                    // Make sure plot title is not empty.
                    if (m_szPlotTitle.empty())
                    {
                        // Submit logger message.
                        LOG_WARNING(logging::g_qSharedLogger, "Plot title is empty. Setting title to default.");
                        m_szPlotTitle = "RoverPath";
                    }
 
                    // Configure plot.
                    m_mtRoverPathPlot->title(m_szPlotTitle);
                    m_mtRoverPathAxes->xlabel("Easting");
                    m_mtRoverPathAxes->ylabel("Northing");
                }
 
 
                ~PathTracer()
                {
                    // Nothing to do yet.
                }
 
 
                bool CreatePathLayer(const std::string& szLayerName, const std::string& szStyleString = "-o")
                {
                    // Check if the layer name exists in the map.
                    if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
                    {
                        // Submit logger message.
                        LOG_WARNING(logging::g_qSharedLogger, "Layer already exists. Cannot create layer.");
                        return false;
                    }
 
                    // Add the layer to the maps.
                    m_umPathMap[szLayerName]               = std::vector<std::pair<double, double>>();
                    m_umPathLineStyleMap[szLayerName]      = szStyleString;
                    m_umLastPlotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
 
                    return true;
                }
 
 
                bool CreateDotLayer(const std::string& szLayerName, const std::string& szColorString = "blue", const bool bFillMarkerFace = true)
                {
                    // Check if the layer name exists in the map.
                    if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
                    {
                        // Submit logger message.
                        LOG_WARNING(logging::g_qSharedLogger, "Layer already exists. Cannot create layer.");
                        return false;
                    }
 
                    // Add the layer to the maps.
                    m_umDotMap[szLayerName]               = std::vector<std::tuple<double, double, double>>();
                    m_umDotLineStyleMap[szLayerName]      = std::make_pair(szColorString, bFillMarkerFace);
                    m_umLastDotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
 
                    return true;
                }
 
 
                bool DeleteLayer(const std::string& szLayerName)
                {
                    // Check if the layer name exist in the path or dot maps.
                    if (m_umPathMap.find(szLayerName) == m_umPathMap.end() && m_umDotMap.find(szLayerName) == m_umDotMap.end())
                    {
                        // Submit logger message.
                        LOG_WARNING(logging::g_qSharedLogger, "Layer does not exist. Cannot delete layer.");
                        return false;
                    }
 
                    // Remove the appropriate layers from the maps.
                    if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
                    {
                        m_umPathMap.erase(szLayerName);
                        m_umPathLineStyleMap.erase(szLayerName);
                        m_umLastPlotUpdateTimeMap.erase(szLayerName);
                    }
                    if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
                    {
                        m_umDotMap.erase(szLayerName);
                        m_umDotLineStyleMap.erase(szLayerName);
                        m_umLastDotUpdateTimeMap.erase(szLayerName);
                    }
 
                    return true;
                }
 
 
                bool ClearLayer(const std::string& szLayerName)
                {
                    // Check if the layer name exist in the path or dot maps.
                    if (m_umPathMap.find(szLayerName) == m_umPathMap.end() && m_umDotMap.find(szLayerName) == m_umDotMap.end())
                    {
                        // Submit logger message.
                        LOG_WARNING(logging::g_qSharedLogger, "Layer does not exist. Cannot clear layer.");
                        return false;
                    }
 
                    // Clear the appropriate layer.
                    if (m_umPathMap.find(szLayerName) != m_umPathMap.end())
                    {
                        m_umPathMap[szLayerName].clear();
                    }
                    if (m_umDotMap.find(szLayerName) != m_umDotMap.end())
                    {
                        m_umDotMap[szLayerName].clear();
                    }
 
                    return true;
                }
 
 
                void AddPathPoint(const geoops::Waypoint& stWaypoint, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1)
                {
                    // Check the update time.
                    if (!this->CheckPathUpdateTime(szLayerName, nMaxWaypointsPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Add the waypoint to the path.
                    m_umPathMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting, stWaypoint.GetUTMCoordinate().dNorthing);
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddPathPoint(const geoops::UTMCoordinate& stCoordinate, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1)
                {
                    // Check the update time.
                    if (!this->CheckPathUpdateTime(szLayerName, nMaxWaypointsPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Add the waypoint to the path.
                    m_umPathMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing);
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddPathPoint(const geoops::GPSCoordinate& stCoordinate, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1)
                {
                    // Check the update time.
                    if (!this->CheckPathUpdateTime(szLayerName, nMaxWaypointsPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Create instance variables.
                    geoops::UTMCoordinate stUTMCoordinate = geoops::ConvertGPSToUTM(stCoordinate);
 
                    // Add the waypoint to the path.
                    m_umPathMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing);
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddPathPoints(const std::vector<geoops::Waypoint>& stWaypoints, const std::string& szLayerName, const uint unMaxUpdatesPerSecond = 1)
                {
                    // Check the update time.
                    if (!this->CheckPathUpdateTime(szLayerName, unMaxUpdatesPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Add the waypoints to the vector or double pairs at the given layer name in the map.
                    for (const geoops::Waypoint& stWaypoint : stWaypoints)
                    {
                        m_umPathMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting, stWaypoint.GetUTMCoordinate().dNorthing);
                    }
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddPathPoints(const std::vector<geoops::UTMCoordinate>& vCoordinates, const std::string& szLayerName, const uint unMaxUpdatesPerSecond = 1)
                {
                    // Check the update time.
                    if (!this->CheckPathUpdateTime(szLayerName, unMaxUpdatesPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Add the waypoints to the vector or double pairs at the given layer name in the map.
                    for (const geoops::UTMCoordinate& stCoordinate : vCoordinates)
                    {
                        m_umPathMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing);
                    }
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddPathPoints(const std::vector<geoops::GPSCoordinate>& vCoordinates, const std::string& szLayerName, const uint unMaxUpdatesPerSecond = 1)
                {
                    // Check the update time.
                    if (!this->CheckPathUpdateTime(szLayerName, unMaxUpdatesPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Add the waypoints to the vector or double pairs at the given layer name in the map.
                    for (const geoops::GPSCoordinate& stCoordinate : vCoordinates)
                    {
                        geoops::UTMCoordinate stUTMCoordinate = geoops::ConvertGPSToUTM(stCoordinate);
                        m_umPathMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing);
                    }
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddDot(const geoops::Waypoint& stWaypoint, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1)
                {
                    // Check the update time.
                    if (!this->CheckDotUpdateTime(szLayerName, nMaxWaypointsPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Check the radius of the waypoint. It shouldn't be less than 0.
                    if (stWaypoint.dRadius <= 0)
                    {
                        m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting, stWaypoint.GetUTMCoordinate().dNorthing, 5);
                    }
                    else
                    {
                        m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting, stWaypoint.GetUTMCoordinate().dNorthing, stWaypoint.dRadius);
                    }
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddDot(const geoops::UTMCoordinate& stCoordinate, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1, const double dDotRadius = 5)
                {
                    // Check the update time.
                    if (!this->CheckDotUpdateTime(szLayerName, nMaxWaypointsPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Add the waypoint to the path.
                    m_umDotMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, dDotRadius);
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddDot(const geoops::GPSCoordinate& stCoordinate, const std::string& szLayerName, const uint nMaxWaypointsPerSecond = 1, const double dDotRadius = 5)
                {
                    // Check the update time.
                    if (!this->CheckDotUpdateTime(szLayerName, nMaxWaypointsPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Create instance variables.
                    geoops::UTMCoordinate stUTMCoordinate = geoops::ConvertGPSToUTM(stCoordinate);
 
                    // Add the waypoint to the path.
                    m_umDotMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, dDotRadius);
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddDots(const std::vector<geoops::Waypoint>& stWaypoints, const std::string& szLayerName, const uint unMaxUpdatesPerSecond = 1)
                {
                    // Check the update time.
                    if (!this->CheckDotUpdateTime(szLayerName, unMaxUpdatesPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Add the waypoints to the vector or double pairs at the given layer name in the map.
                    for (const geoops::Waypoint& stWaypoint : stWaypoints)
                    {
                        // Check the radius of the waypoint. It shouldn't be less than 0.
                        if (stWaypoint.dRadius <= 0)
                        {
                            m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting, stWaypoint.GetUTMCoordinate().dNorthing, 5);
                        }
                        else
                        {
                            m_umDotMap[szLayerName].emplace_back(stWaypoint.GetUTMCoordinate().dEasting, stWaypoint.GetUTMCoordinate().dNorthing, stWaypoint.dRadius);
                        }
                    }
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddDots(const std::vector<geoops::UTMCoordinate>& vCoordinates,
                             const std::string& szLayerName,
                             const uint unMaxUpdatesPerSecond = 1,
                             const double dDotRadius          = 5)
                {
                    // Check the update time.
                    if (!this->CheckDotUpdateTime(szLayerName, unMaxUpdatesPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Add the waypoints to the vector or double pairs at the given layer name in the map.
                    for (const geoops::UTMCoordinate& stCoordinate : vCoordinates)
                    {
                        m_umDotMap[szLayerName].emplace_back(stCoordinate.dEasting, stCoordinate.dNorthing, dDotRadius);
                    }
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
 
                void AddDots(const std::vector<geoops::GPSCoordinate>& vCoordinates,
                             const std::string& szLayerName,
                             const uint unMaxUpdatesPerSecond = 1,
                             const double dDotRadius          = 5)
                {
                    // Check the update time.
                    if (!this->CheckDotUpdateTime(szLayerName, unMaxUpdatesPerSecond))
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return;
                    }
 
                    // Add the waypoints to the vector or double pairs at the given layer name in the map.
                    for (const geoops::GPSCoordinate& stCoordinate : vCoordinates)
                    {
                        geoops::UTMCoordinate stUTMCoordinate = geoops::ConvertGPSToUTM(stCoordinate);
                        m_umDotMap[szLayerName].emplace_back(stUTMCoordinate.dEasting, stUTMCoordinate.dNorthing, dDotRadius);
                    }
 
                    // Update the plot.
                    this->UpdatePlot();
                }
 
            private:
                // Declare private member variables.
                matplot::figure_handle m_mtRoverPathPlot;
                matplot::axes_handle m_mtRoverPathAxes;
                std::unordered_map<std::string, std::string> m_umPathLineStyleMap;
                std::unordered_map<std::string, std::pair<std::string, bool>> m_umDotLineStyleMap;
                std::unordered_map<std::string, std::chrono::system_clock::time_point> m_umLastPlotUpdateTimeMap;
                std::unordered_map<std::string, std::chrono::system_clock::time_point> m_umLastDotUpdateTimeMap;
                std::unordered_map<std::string, std::vector<std::pair<double, double>>> m_umPathMap;
                std::unordered_map<std::string, std::vector<std::tuple<double, double, double>>> m_umDotMap;
                std::string m_szPlotTitle;
                std::string m_szCurrentPlotSavePath;
 
 
                void UpdatePlot()
                {
                    // Create instance variables.
                    std::vector<std::string> vLayerNames;
 
                    // Clear the plot.
                    m_mtRoverPathAxes->clear();
 
                    /*
                        PATHS
                    */
                    // Loop through each of the layer name keys in the map.
                    for (const std::pair<const std::string, const std::string>& stdLayer : m_umPathLineStyleMap)
                    {
                        // Check if the vector or coordinates has more than one point.
                        if (m_umPathMap[stdLayer.first].size() > 1)
                        {
                            // Add the layer name to the vector.
                            vLayerNames.push_back(stdLayer.first);
 
                            // Get the x and y coordinates for the layer.
                            std::vector<double> vEasting, vNorthing;
                            for (const std::pair<double, double>& stCoordinate : m_umPathMap[stdLayer.first])
                            {
                                vEasting.push_back(stCoordinate.first);
                                vNorthing.push_back(stCoordinate.second);
                            }
 
                            // Plot the path.
                            m_mtRoverPathAxes->plot(vEasting, vNorthing, std::string_view(stdLayer.second));
                            // Set the hold to true.
                            m_mtRoverPathAxes->hold(true);
                        }
                    }
 
                    /*
                        DOTS
                    */
                    // Loop through each of the layer name keys in the map.
                    for (const std::pair<const std::string, const std::pair<std::string, bool>>& stdLayer : m_umDotLineStyleMap)
                    {
                        // Check if the vector or coordinates has at least one point.
                        if (m_umDotMap[stdLayer.first].size() > 0)
                        {
                            // Add the layer name to the vector.
                            vLayerNames.push_back(stdLayer.first);
 
                            // Get the x and y coordinates for the layer.
                            std::vector<double> vEasting, vNorthing, vRadius;
                            for (const std::tuple<double, double, double>& stCoordinate : m_umDotMap[stdLayer.first])
                            {
                                vEasting.push_back(std::get<0>(stCoordinate));
                                vNorthing.push_back(std::get<1>(stCoordinate));
                                vRadius.push_back(std::get<2>(stCoordinate));
                            }
 
                            // Plot the path.
                            matplot::line_handle mtLineHandle = m_mtRoverPathAxes->scatter(vEasting, vNorthing, vRadius);
                            mtLineHandle->color(stdLayer.second.first);
                            mtLineHandle->marker_face(stdLayer.second.second);
                            // Set the hold to true.
                            m_mtRoverPathAxes->hold(true);
                        }
                    }
 
                    // Update legend names.
                    m_mtRoverPathAxes->legend(vLayerNames);
                    matplot::legend_handle mtLegend = m_mtRoverPathAxes->legend();
                    mtLegend->font_size(8);
                    mtLegend->num_columns(2);
                    // Set axis options.
                    m_mtRoverPathAxes->grid(true);
                    m_mtRoverPathAxes->xtickangle(45);
                    m_mtRoverPathAxes->axis(matplot::square);
                    m_mtRoverPathAxes->xtickformat("%.0f");    // No decimal places for x-axis
                    m_mtRoverPathAxes->ytickformat("%.0f");    // No decimal places for y-axis
                    // Set the hold to false.
                    m_mtRoverPathAxes->hold(false);
                    // Plot the path.
                    m_mtRoverPathPlot->draw();
                    m_mtRoverPathPlot->save(m_szCurrentPlotSavePath);
                }
 
 
                bool CheckPathUpdateTime(const std::string& szLayerName, const uint unMaxUpdatesPerSecond)
                {
                    // Check if the layer name exists in the map.
                    if (m_umLastPlotUpdateTimeMap.find(szLayerName) == m_umLastPlotUpdateTimeMap.end())
                    {
                        // Submit logger message.
                        LOG_WARNING(logging::g_qSharedLogger, "Layer does not exist. Cannot add waypoints.");
                        return false;
                    }
 
                    // Check if the maximum number of waypoints per second has been exceeded.
                    if (unMaxUpdatesPerSecond != 0 &&
                        std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - m_umLastPlotUpdateTimeMap[szLayerName]).count() <
                            1.0 / unMaxUpdatesPerSecond)
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return false;
                    }
 
                    // Update the last plot time.
                    m_umLastPlotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
 
                    // Return true.
                    return true;
                }
 
 
                bool CheckDotUpdateTime(const std::string& szLayerName, const uint unMaxUpdatesPerSecond)
                {
                    // Check if the layer name exists in the map.
                    if (m_umLastDotUpdateTimeMap.find(szLayerName) == m_umLastDotUpdateTimeMap.end())
                    {
                        // Submit logger message.
                        LOG_WARNING(logging::g_qSharedLogger, "Layer does not exist. Cannot add waypoints.");
                        return false;
                    }
 
                    // Check if the maximum number of waypoints per second has been exceeded.
                    if (unMaxUpdatesPerSecond != 0 &&
                        std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - m_umLastDotUpdateTimeMap[szLayerName]).count() <
                            1.0 / unMaxUpdatesPerSecond)
                    {
                        // Return if the maximum number of waypoints per second has been exceeded.
                        return false;
                    }
 
                    // Update the last plot time.
                    m_umLastDotUpdateTimeMap[szLayerName] = std::chrono::system_clock::now();
 
                    // Return true.
                    return true;
                }
        };
    }    // namespace graphing
}    // namespace logging
 
#endif