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

#ifndef LEUZE_SCAN_DATA_PARSER_H
#define LEUZE_SCAN_DATA_PARSER_H

#include <array>
#include <condition_variable>
#include <deque>
#include <iostream>
#include <mutex>
#include <string>

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

#include <boost/bind/bind.hpp>
#include <boost/asio.hpp>
#include <boost/thread.hpp>
#include <boost/circular_buffer.hpp>

#include "leuze_rod_driver/common.h"

constexpr std::uint32_t SCANRECV_PACKET_MDI_HEADER_SIZE     = 31;       

constexpr std::uint8_t  SCANRECV_PACKET_MDI_SYNC0           = 'L'; 
constexpr std::uint8_t  SCANRECV_PACKET_MDI_SYNC1           = 'E';
constexpr std::uint8_t  SCANRECV_PACKET_MDI_SYNC2           = 'U'; 
constexpr std::uint8_t  SCANRECV_PACKET_MDI_SYNC3           = 'Z'; 

using namespace boost::placeholders;

namespace leuze_rod_ros2_drivers
{
    /************************************************************************************************************
     * @class   ScanDataParser
     * 
     * @brief   This class ensures parsing of messages captured in its internal circle buffer
     ************************************************************************************************************/
    class ScanDataParser
    {
        public: 

            boost::circular_buffer<char> ring_buffer_;

            std::mutex data_mutex_;
            std::deque<ScanData> scan_data_;
            std::vector<unsigned short> data_distances_;
            std::vector<unsigned short> data_intensities_;
            int scan_spots_sum_;       
            
            std::condition_variable data_notifier_;

            /**
             *@brief constructor
            */
            ScanDataParser();

            /**
             *@brief destructor
            */
            ~ScanDataParser();

            /****************************************************************************************************/

        protected: 

            /**
             *@brief read data from the internal ring buffer
            */
            void readFromRingBuff(char* dest, std::size_t numbytes);

            /**
             *@brief write data to the internal ring buffer
            */
            void writeToRingBuff(const char* src, std::size_t numbytes);

            /**
             *@brief    get a packet from a data queue and parse scan data from it
             *@return   true if success
             */
            bool parsePacketsFromRingBuff();

            /****************************************************************************************************/

        private:

            /**
             *@brief    find a start of packet in internal ring buffer
            *@return   index of packet start
            */
            int findPacketStart();

            /**
             *@brief conver data from network byte order to host byte order
            */
            void packetHeaderToLittleEndian(PacketCommon *pHead);

            /**
             *@brief    retrieve packet from ring buffer
            *@return   true if success 
            */
            bool retrievePacket(std::size_t start, PacketCommon* p);

            /****************************************************************************************************/

        public:
        
            /**
             *@brief    get scan
            *@return   scan data
            */
            ScanData getScan();

            /**
             *@brief    get number of available scans
            *@return   number of available scans
            */
            std::size_t getScansAvailable() const { return scan_data_.size(); }

            /**
             *@brief    get complete scan
            *@return   scan data
            */            
            ScanData getCompleteScan();

            /**
             *@brief    get number of available complete scans
            *@return   number of available scans
            */
            std::size_t getCompleteScansAvailable() const;

            /****************************************************************************************************/
    };
}  

#endif
