#include "leuze_rsl_driver/rsl400_interface.hpp"
#include <gtest/gtest.h>


struct RSL400TestParseData : public testing::Test
{
    std::basic_string<unsigned char> test_buffer;
    ros::NodeHandle nh;
    RSL400Interface *interface;
    void SetUp()
    {
      nh.setParam("/test_parse_data/scanner_frame","test_frame");
      interface = new RSL400Interface("192.168.0.1","9990", "scan1", &nh);
    }
    void TearDown()
    {
        delete interface;
    }

};

TEST_F(RSL400TestParseData, test_id_1)
{
    test_buffer.resize(48);
    ros::NodeHandle nh;
    test_buffer = {0x30 ,0x00 ,0x00 ,0x00 ,0x08 ,0x00 ,0xaf ,0xfe ,  //H1
                   0x04 ,0x15 ,0x02 ,0xc8 , //H2
                   0x01 , 0x00 ,  //ID
                   0x00 ,0x00 , //Block
                   0x6e ,0x29 ,0x05 ,0x00 , //Scan
                   0x01 ,0x01 ,0x02 ,0x80 ,0x00 ,0x00 ,0x10 ,0x80 ,0x6e ,0x29 ,0x05 ,0x00 ,0xe8 ,0x11 ,0x00 ,0xf0 ,0x00 ,0x00 ,0x00 ,0x00 , //Status profile
                   0x00 ,0x00 ,0x8b ,0x0a ,0x01 ,0x00 ,0x00 ,0x00 // Measurement Contour Description
                  }; //Captured from wireshark, the datagram extended status profile
    EXPECT_EQ(interface->parseBuffer(test_buffer) , 1);
}

TEST_F(RSL400TestParseData, test_id_3)
{
    test_buffer.resize(48);
    ros::NodeHandle nh;
    test_buffer = {0x30 ,0x00 ,0x00 ,0x00 ,0x08 ,0x00 ,0xaf ,0xfe ,  //H1
                   0x04 ,0x15 ,0x02 ,0xc8 , //H2
                   0x03 , 0x00 ,  //ID
                   0x00 ,0x00 , //Block
                   0x6e ,0x29 ,0x05 ,0x00 , //Scan
                   0x01 ,0x01 ,0x02 ,0x80 ,0x00 ,0x00 ,0x10 ,0x80 ,0x6e ,0x29 ,0x05 ,0x00 ,0xe8 ,0x11 ,0x00 ,0xf0 ,0x00 ,0x00 ,0x00 ,0x00 , //Status profile
                   0x00 ,0x00 ,0x8b ,0x0a ,0x01 ,0x00 ,0x00 ,0x00 // Measurement Contour Description
                  }; //Captured from wireshark, wrong type of datagram for ID 3, but we only test ID control flow here so it's okay
    EXPECT_EQ(interface->parseBuffer(test_buffer) , 3);
}

TEST_F(RSL400TestParseData, test_id_6)
{
    test_buffer.resize(48);
    ros::NodeHandle nh;
    test_buffer = {0x30 ,0x00 ,0x00 ,0x00 ,0x08 ,0x00 ,0xaf ,0xfe ,  //H1
                   0x04 ,0x15 ,0x02 ,0xc8 , //H2
                   0x06 , 0x00 ,  //ID
                   0x00 ,0x00 , //Block
                   0x6e ,0x29 ,0x05 ,0x00 , //Scan
                   0x01 ,0x01 ,0x02 ,0x80 ,0x00 ,0x00 ,0x10 ,0x80 ,0x6e ,0x29 ,0x05 ,0x00 ,0xe8 ,0x11 ,0x00 ,0xf0 ,0x00 ,0x00 ,0x00 ,0x00 , //Status profile
                   0x00 ,0x00 ,0x8b ,0x0a ,0x01 ,0x00 ,0x00 ,0x00 // Measurement Contour Description
                  }; //Captured from wireshark, wrong type of datagram for ID 3, but we only test ID control flow here so it's okay
    EXPECT_EQ(interface->parseBuffer(test_buffer) , 6);
}

//Unrecogized ID
TEST_F(RSL400TestParseData, test_id_4)
{
    test_buffer.resize(48);
    ros::NodeHandle nh;
    test_buffer = {0x30 ,0x00 ,0x00 ,0x00 ,0x08 ,0x00 ,0xaf ,0xfe ,  //H1
                   0x04 ,0x15 ,0x02 ,0xc8 , //H2
                   0x04 , 0x00 ,  //ID
                   0x00 ,0x00 , //Block
                   0x6e ,0x29 ,0x05 ,0x00 , //Scan
                   0x01 ,0x01 ,0x02 ,0x80 ,0x00 ,0x00 ,0x10 ,0x80 ,0x6e ,0x29 ,0x05 ,0x00 ,0xe8 ,0x11 ,0x00 ,0xf0 ,0x00 ,0x00 ,0x00 ,0x00 , //Status profile
                   0x00 ,0x00 ,0x8b ,0x0a ,0x01 ,0x00 ,0x00 ,0x00 // Measurement Contour Description
                  }; //Captured from wireshark
    EXPECT_EQ(interface->parseBuffer(test_buffer) , -1);
}
