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

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

#include "leuze_rod_driver/scan_data_parser.h"

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

    /**
     *@brief constructor
     */
    ScanDataParser::ScanDataParser() : ring_buffer_(SCANPAR_BUFF_SIZE_RING), scan_data_()
    {
        scan_spots_sum_ = 0;
    } 

    /**
     *@brief destructor
     */
    ScanDataParser::~ScanDataParser()
    {
    } 

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

    /**
     *@brief read data from the internal ring buffer
     */
    void ScanDataParser::readFromRingBuff(char *dest, std::size_t length)
    {
        if(ring_buffer_.size() < length)
        {
            DEBUG_ERR("[readFromRingBuff] Expected data length is bigger then buffer size!");
            throw std::runtime_error("Unable to read from internal ring buffer! Given data length is bigger then buffer size!");
        }
        else
        {
            char* p1 = ring_buffer_.array_one().first;
            char* p2 = ring_buffer_.array_two().first;

            std::size_t p1_size = ring_buffer_.array_one().second;

            if(p1_size >= length)
            {
                std::memcpy(dest, p1, length);
            }
            else
            {
                std::memcpy(dest, p1, p1_size);
                std::memcpy(dest + p1_size, p2, length - p1_size);
            }
        } 
    }

    /**
     *@brief write data to the internal ring buffer
     */
    void ScanDataParser::writeToRingBuff(const char *src, std::size_t length)
    {
        if(ring_buffer_.size() + length > ring_buffer_.capacity())
        {
            DEBUG_ERR("[writeToRingBuff] Given data length is bigger then current buffer capacity!");
            throw std::runtime_error("Unable to write to internal ring buffer! Given data length is bigger then current buffer capacity!");
        }
        else
        {
            ring_buffer_.resize(ring_buffer_.size() + length);

            char* p1 = ring_buffer_.array_one().first;
            char* p2 = ring_buffer_.array_two().first;

            std::size_t p1_size = ring_buffer_.array_one().second;
            std::size_t p2_size = ring_buffer_.array_two().second;
            
            if( p2_size >= length )
            {
                std::memcpy(p2 + p2_size - length, src, length);
            }
            else
            {
                std::memcpy(p1 + p1_size + p2_size - length, src, length - p2_size);
                std::memcpy(p2, src + length - p2_size, p2_size);
            }
        } 
    }

    /**
     *@brief    get a packet from a data queue and parse scan data from it
     *@return   true if success
     */
    bool ScanDataParser::parsePacketsFromRingBuff()
    {
        bool retval = false; 
        bool stop   = false;

        do
        {
            int packet_start = findPacketStart();                                   // search for a packet
            if(packet_start >= 0)
            {
                char buf[SCANPAR_BUFF_SIZE_RING];
                PacketCommon* p = (PacketCommon*) buf;

                if(retrievePacket(packet_start, p))                                 // try to retrieve packet
                {
                    std::unique_lock<std::mutex> lock(data_mutex_);                 // lock outgoing data queue (automatically unlocked at end of function)

                    /* Create new scan container if necessary */
                    if(scan_data_.empty())                                          // init
                    {
                        scan_data_.emplace_back();
                    }
                    else
                    {
                        auto scan_count = scan_data_.size();
                        if(scan_count > SCANPAR_RXQUE_CNT_WARN && scan_count < SCANPAR_RXQUE_CNT_ERR)
                        {
                            DEBUG_WRN("[parsePacketsFromRingBuff] " << scan_count << " scans in receiver queue");
                        }
                        else if(scan_count > SCANPAR_RXQUE_CNT_ERR)
                        {
                            scan_data_.pop_front();
                            DEBUG_ERR("[parsePacketsFromRingBuff] Too many scans (" << scan_count << ") in receiver queue!");
                        } 
                        else
                        {
                            // OK
                        } 
                        scan_data_.emplace_back();
                        data_notifier_.notify_one();
                    }
                    
                    ScanData& current_scan_data = scan_data_.back();

                    /* parse packet */
                    std::uint16_t* p_scan_data = (std::uint16_t*) &buf[SCANRECV_PACKET_MDI_HEADER_SIZE]; 
                    int num_scan_spots = p->header.scan_spots;

                    if(p->header.sub_number != 1)
                    {
                        // OK
                    }
                    else
                    {
                        scan_spots_sum_ = 0;
                        std::vector<unsigned short>().swap(data_distances_);
                        std::vector<unsigned short>().swap(data_intensities_);
                    } 
                    scan_spots_sum_ += p->header.scan_spots;

                    if(p->header.packet_type == 1)                              // packet contains distances and intensities
                    {
                        for(int i = 0; i < num_scan_spots; i++)
                        {
                            unsigned short data = ntohs(p_scan_data[i]);
                            data_distances_.push_back(data);
                        }
                        for(int i = num_scan_spots; i < num_scan_spots * 2; i++)
                        {
                            unsigned short data1 = ntohs(p_scan_data[i]);
                            data_intensities_.push_back(data1);
                        }
                    }
                    else                                                        // packet contains only distances
                    {
                        for(int i = 0; i < num_scan_spots; i++)
                        {
                            unsigned short data = ntohs(p_scan_data[i]);
                            data_distances_.push_back(data);
                        }
                    }

                    if(p->header.sub_number == p->header.total_number)          // complete scan
                    {
                        int scan_length = data_distances_.size();
                        
                        if(p->header.packet_type == 1)
                        {
                            for(int i = 0; i < scan_length; i++)
                            {
                                current_scan_data.distances.push_back(data_distances_[i]);
                                current_scan_data.intensities.push_back(data_intensities_[i]);
                            }
                        }
                        else
                        {
                            for(int i = 0; i < scan_length; i++)
                            {
                                current_scan_data.distances.push_back(data_distances_[i]);
                            }
                        }

                        current_scan_data.headers.push_back(p->header);         // save header of last packet
                    }
                    else
                    {
                        // OK
                    }

                    retval = true;                                              // some data has been parsed
                }
                else 
                {
                    stop = true;                                                // unable to retrieve packet from ring buffer
                }
            } 
            else
            {
                stop = true;                                                    // unable to find start of packet
            }
        } while (!stop);

        return retval;
    }

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

    /**
     *@brief    find a start of packet in internal ring buffer
     *@return   index of packet start
     */
    int ScanDataParser::findPacketStart()
    {        
        int packet_start_index = 0;

        if(ring_buffer_.size() < SCANRECV_PACKET_MDI_HEADER_SIZE)
        {
            packet_start_index = -1;
        }
        else
        {
            for(std::size_t i = 0; i < ring_buffer_.size() - 4; i++)
            {
                if(((unsigned char) ring_buffer_[i]) == SCANRECV_PACKET_MDI_SYNC0
                    && ((unsigned char) ring_buffer_[i + 1]) == SCANRECV_PACKET_MDI_SYNC1
                    && ((unsigned char) ring_buffer_[i + 2]) == SCANRECV_PACKET_MDI_SYNC2
                    && ((unsigned char) ring_buffer_[i + 3]) == SCANRECV_PACKET_MDI_SYNC3)
                {
                    packet_start_index = i;
                    break;
                }
                else
                {
                    packet_start_index = -2;
                } 
            }
        } 

        return packet_start_index;
    }

    /**
     *@brief conver data from network byte order to host byte order
     */
    void ScanDataParser::packetHeaderToLittleEndian(PacketCommon *pHead)
    {
        pHead->header.packet_size   = ntohs(pHead->header.packet_size);
        pHead->header.reserve_a     = ntohs(pHead->header.reserve_a);
        pHead->header.reserve_b     = ntohs(pHead->header.reserve_b);
        pHead->header.reserve_c     = ntohs(pHead->header.reserve_c);
        pHead->header.packet_number = ntohs(pHead->header.packet_number);
        pHead->header.scan_freq     = ntohs(pHead->header.scan_freq);
        pHead->header.scan_spots    = ntohs(pHead->header.scan_spots);
        pHead->header.first_angle   = ntohl(pHead->header.first_angle);
        pHead->header.delta_angle   = ntohl(pHead->header.delta_angle);
        pHead->header.timestamp     = ntohs(pHead->header.timestamp);
    }

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

        if(ring_buffer_.size() < SCANRECV_PACKET_MDI_HEADER_SIZE)
        {
            DEBUG_ERR("[retrievePacket] size is smaller then expected size of MDI packet");
            retval = false;
        }
        else
        {
            ring_buffer_.erase_begin(start);
            char* pp = (char*) p;

            readFromRingBuff(pp, SCANRECV_PACKET_MDI_HEADER_SIZE);
            packetHeaderToLittleEndian(p);

            if(ring_buffer_.size() < p->header.packet_size)
            {
                retval = false;
            }
            else
            { 
                readFromRingBuff(pp, p->header.packet_size);
                packetHeaderToLittleEndian(p);

                ring_buffer_.erase_begin(p->header.packet_size);
            } 
        }        

        return retval;
    }

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

    /**
     *@brief    get scan
     *@return   scan data
     */
    ScanData ScanDataParser::getScan()
    {
        std::unique_lock<std::mutex> lock(data_mutex_);

        ScanData scan_data(std::move(scan_data_.front()));
        scan_data_.pop_front();

        return scan_data;
    }

    /**
     *@brief    get complete scan
     *@return   scan data
     */
    ScanData ScanDataParser::getCompleteScan()
    {
        ScanData complete_scan_data;

        std::unique_lock<std::mutex> lock(data_mutex_);

        if(scan_data_.size() >= 1)
        {
            complete_scan_data = ScanData(std::move(scan_data_.front()));
            scan_data_.pop_front();
        }
        else
        {
            complete_scan_data = ScanData{};
        } 

        return complete_scan_data;
    }

    /**
     *@brief    get number of available complete scans
     *@return   number of available scans
     */
    std::size_t ScanDataParser::getCompleteScansAvailable() const
    {
        std::size_t complete_scans_available = scan_data_.size();

        if(complete_scans_available > 0)
        {
            complete_scans_available = complete_scans_available - 1;
        }
        else
        {
            complete_scans_available = 0;
        }

       return complete_scans_available;
    }

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

} 