/*  Copyright 2025 Leuze electronic GmbH + Co. KG
 *
 *  Licensed under the Apache License, Version 2.0
 */

#ifndef LEUZE_COMMON_H
#define LEUZE_COMMON_H

#include <rclcpp/rclcpp.hpp>

#include <cstdint>
#include <chrono>
#include <string>
#include <vector>
#include <iostream>
#include <fstream>
#include <sstream>

using namespace std::chrono_literals;

/* constants */
constexpr std::float_t              CONST_1_DEGREE_IN_RADIANS           = 0.017453292222222222;

/* scan data related pre-defined constants */
constexpr std::uint32_t             SCANPAR_BUFF_SIZE_RING              = 65536;       // [bytes] 
constexpr std::uint32_t             SCANPAR_BUFF_SIZE_IN                = 4096;        // [bytes] 
constexpr std::uint32_t             SCANPAR_PUBLISHER_QUE_SIZE          = 128;
constexpr std::int32_t              SCANPAR_PUBLISH_TIMEOUT_S           = 1;           // [s] 
constexpr std::uint32_t             SCANPAR_RXQUE_CNT_WARN              = 64;
constexpr std::uint32_t             SCANPAR_RXQUE_CNT_ERR               = 128;
constexpr std::chrono::milliseconds SCANPAR_PUBLISHER_PERIOD            = 2ms;         // [ms]  
constexpr std::uint32_t             SCANPAR_TIMEOUT_MDI_PACKET          = 3;           // [s] 

/* status data related pre-defined constants */
constexpr std::uint32_t             STATPAR_MAX_FAILED_COUNT            = 10;
constexpr std::uint32_t             STATPAR_MAX_FAILED_TIMEPERIOD_S     = 60;          // [s] 
constexpr std::uint32_t             STATPAR_PUBLISHER_QUE_SIZE          = 32;
constexpr std::chrono::milliseconds STATPAR_PUBLISHER_PERIOD            = 100ms;       // [ms] 

/* ROD scanner pre-defined parameter values */
constexpr std::uint32_t             RODPAR_DIR_CLOCKWISE                = 0;
constexpr std::uint32_t             RODPAR_DIR_COUNTERCLOCKWISE         = 1;
constexpr std::uint32_t             RODPAR_FILTER_MEDIAN                = 0;
constexpr std::uint32_t             RODPAR_FILTER_AVERAGE               = 1;
constexpr std::uint32_t             RODPAR_FILTER_MAX                   = 2;
constexpr std::uint32_t             RODPAR_FILTER_COMBO                 = 3;
constexpr std::uint32_t             RODPAR_PROTO_UDP                    = 0;
constexpr std::uint32_t             RODPAR_PROTO_TCP                    = 1;
constexpr std::uint32_t             RODPAR_PTYPE_DISTANCES_ONLY         = 0;
constexpr std::uint32_t             RODPAR_PTYPE_DISTANCE_AND_AMPLITUDE = 1;
constexpr std::uint32_t             RODPAR_RESOL_0200_80_Hz             = 0; 
constexpr std::uint32_t             RODPAR_RESOL_0200_50_Hz             = 4;
constexpr std::uint32_t             RODPAR_RESOL_0100_40_Hz             = 1;
constexpr std::uint32_t             RODPAR_RESOL_0050_20_Hz             = 2;
constexpr std::uint32_t             RODPAR_RESOL_0025_10_Hz             = 3;

/* ROD scanner parameter ranges */
constexpr std::int32_t              RODRNG_ANGLE_MIN                    = -13760;       // [0.01°] 
constexpr std::int32_t              RODRNG_ANGLE_MAX                    =  13760;       // [0.01°]
constexpr std::uint32_t             RODRNG_CONT_MAX                     = 100;          // [%] 
constexpr std::uint32_t             RODRNG_FILTER_SPOTSNGBH_MAX         = 7;
constexpr std::uint32_t             RODRNG_FILTER_SPOTSHIST_MAX         = 7;
constexpr std::int32_t              RODRNG_SKIPSPOT_MAX                 = 1376;

/* Driver debug messages */
#ifdef DEBUG

    /* debug logs colors */
    #define DEBUG_ERR_COLOR     "\033[31m"              // red
    #define DEBUG_LOG_COLOR     "\033[90m"              // gray
    #define DEBUG_WRN_COLOR     "\033[33m"              // yellow
    #define DEBUG_STD_COLOR     "\033[37m"              // white

    /* debug messages */
    #define DEBUG_ERR(msg)      std::cerr << DEBUG_ERR_COLOR << "[DEBUG ERR] " << msg << std::endl
    #define DEBUG_LOG(msg)      std::cout << DEBUG_LOG_COLOR << "[DEBUG LOG] " << msg << std::endl
    #define DEBUG_WRN(msg)      std::cout << DEBUG_WRN_COLOR << "[DEBUG WRN] " << msg << std::endl

#else

    #define DEBUG_ERR(msg)      ((void)0)
    #define DEBUG_LOG(msg)      ((void)0)
    #define DEBUG_WRN(msg)      ((void)0)
    
#endif

namespace leuze_rod_ros2_drivers
{
    /****************************************************************************************************
     * Structures
     ****************************************************************************************************/

    /**
     *@brief    ROD scan settings 
     */
    struct RodScanParams
    {
        float   angle_min;                              //
        float   angle_max;                              //
        float   angle_delta;                            //
        float   angle_resolution;                       //
        int     packet_type;                            // type of scan data packet (0 = distances only / 1 = distances & intensities)
        int     protocol;                               //
        int     scan_direction;                         //
        double  scan_duration;                          //
        int     skip_spots;                             //
    }; 

    /**
     *@brief    packet header received from scanner
     */
    struct __attribute__((packed)) PacketHeader
    {
        std::uint32_t   sync;                           // synchronization pattern (0x4C 0x45 0x55 0x5A)
        std::uint8_t    packet_type;                    // type of scan data packet (0 = distances only / 1 = distances & intensities)
        std::uint16_t   packet_size;                    // reserve for future use
        std::uint16_t   reserve_a;                      // reserve for future use
        std::uint16_t   reserve_b;                      // reserve for future use
        std::uint16_t   reserve_c;                      // reserve for future use
        std::uint16_t   packet_number;                  // sequence number of packet (start counting from sensor startup)
        std::uint8_t    total_number;                   // total frame number of a whole scan
        std::uint8_t    sub_number;                     // index if packet in a whole scan (start counting from 1, must not be greater then total_number)
        std::uint16_t   scan_freq;                      // scan frequency [Hz] 
        std::uint16_t   scan_spots;                     // number of scan spots (points) within this packet
        std::int32_t    first_angle;                    // absolute angle of the first spot in this packet  [1/1000 °] 
        std::int32_t    delta_angle;                    // delta angle between two consecutive output spots [1/1000 °]
        std::uint16_t   timestamp;                      // timestamp of packet [ms] 
    };

    /**
     *@brief    common (TCP / UDP) packet
     */
    struct __attribute__((packed)) PacketCommon
    {
        PacketHeader header;
    };

    /**
     *@brief  scan data structure (contains one completed scan, that is given by one scanner head rotation of 360°)
     */
    struct ScanData
    {
        std::vector<PacketHeader>   headers;            // received packet headers
        std::vector<std::uint16_t>  distances;          // distances [mm] in polar coordinates
        std::vector<std::uint16_t>  intensities;        // intensities (valid range is 32 - 4095, lower intensities indicate an error / undefined value)
    };
}

/****************************************************************************************************
 * Templates
****************************************************************************************************/

namespace std
{
    template < typename T > std::string to_string( const T& n )
    {
        std::ostringstream stm ;
        stm << n ;
        return stm.str() ;
    }
}

#endif

