package ros

import (
	"bytes"
	"testing"
)

func TestReadConnectionHeader(t *testing.T) {
	// example from ros.org
	data := []byte{
		0xb0, 0x00, 0x00, 0x00,
		0x20, 0x00, 0x00, 0x00,
		0x6d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65, 0x5f, 0x64, 0x65,
		0x66, 0x69, 0x6e, 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x3d, 0x73,
		0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x64, 0x61, 0x74, 0x61,
		0x0a, 0x0a,
		0x25, 0x00, 0x00, 0x00,
		0x63, 0x61, 0x6c, 0x6c, 0x65, 0x72, 0x69, 0x64, 0x3d, 0x2f,
		0x72, 0x6f, 0x73, 0x74, 0x6f, 0x70, 0x69, 0x63, 0x5f, 0x34,
		0x37, 0x36, 0x37, 0x5f, 0x31, 0x33, 0x31, 0x36, 0x39, 0x31,
		0x32, 0x37, 0x34, 0x31, 0x35, 0x35, 0x37,
		0x0a, 0x00, 0x00, 0x00,
		0x6c, 0x61, 0x74, 0x63, 0x68, 0x69, 0x6e, 0x67, 0x3d, 0x31,
		0x27, 0x00, 0x00, 0x00,
		0x6d, 0x64, 0x35, 0x73, 0x75, 0x6d, 0x3d, 0x39, 0x39, 0x32,
		0x63, 0x65, 0x38, 0x61, 0x31, 0x36, 0x38, 0x37, 0x63, 0x65,
		0x63, 0x38, 0x63, 0x38, 0x62, 0x64, 0x38, 0x38, 0x33, 0x65,
		0x63, 0x37, 0x33, 0x63, 0x61, 0x34, 0x31, 0x64, 0x31,
		0x0e, 0x00, 0x00, 0x00,
		0x74, 0x6f, 0x70, 0x69, 0x63, 0x3d, 0x2f, 0x63, 0x68, 0x61,
		0x74, 0x74, 0x65, 0x72,
		0x14, 0x00, 0x00, 0x00,
		0x74, 0x79, 0x70, 0x65, 0x3d, 0x73, 0x74, 0x64, 0x5f, 0x6d,
		0x73, 0x67, 0x73, 0x2f, 0x53, 0x74, 0x72, 0x69, 0x6e, 0x67,
		0x09, 0x00, 0x00, 0x00,
		0x05, 0x00, 0x00, 0x00,
		0x68, 0x65, 0x6c, 0x6c, 0x6f,
	}

	buf := bytes.NewReader(data)
	t.Log(buf.Len())
	headers, err := readConnectionHeader(buf)
	if err != nil {
		t.Error(err)
	}

	if headers[0].key != "message_definition" {
		t.Error("")
	}
	if headers[0].value != "string data\n\n" {
		t.Error("")
	}

	if headers[1].key != "callerid" {
		t.Error("")
	}
	if headers[1].value != "/rostopic_4767_1316912741557" {
		t.Error("")
	}

	if headers[2].key != "latching" {
		t.Error("")
	}
	if headers[2].value != "1" {
		t.Error("")
	}

	if headers[3].key != "md5sum" {
		t.Error("")
	}
	if headers[3].value != "992ce8a1687cec8c8bd883ec73ca41d1" {
		t.Error("")
	}

	if headers[4].key != "topic" {
		t.Error("")
	}
	if headers[4].value != "/chatter" {
		t.Error("")
	}

	if headers[5].key != "type" {
		t.Error("")
	}
	if headers[5].value != "std_msgs/String" {
		t.Error("")
	}
}

func TestWriteConnectionHeader(t *testing.T) {
	headers := []header{
		header{"message_definition", "string data\n\n"},
		header{"callerid", "/rostopic_4767_1316912741557"},
		header{"latching", "1"},
		header{"md5sum", "992ce8a1687cec8c8bd883ec73ca41d1"},
		header{"topic", "/chatter"},
		header{"type", "std_msgs/String"},
	}

	var buffer bytes.Buffer
	err := writeConnectionHeader(headers, &buffer)
	if err != nil {
		t.Error(err)
	}

	expected := []byte{
		0xb0, 0x00, 0x00, 0x00,
		0x20, 0x00, 0x00, 0x00,
		0x6d, 0x65, 0x73, 0x73, 0x61, 0x67, 0x65, 0x5f, 0x64, 0x65,
		0x66, 0x69, 0x6e, 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x3d, 0x73,
		0x74, 0x72, 0x69, 0x6e, 0x67, 0x20, 0x64, 0x61, 0x74, 0x61,
		0x0a, 0x0a,
		0x25, 0x00, 0x00, 0x00,
		0x63, 0x61, 0x6c, 0x6c, 0x65, 0x72, 0x69, 0x64, 0x3d, 0x2f,
		0x72, 0x6f, 0x73, 0x74, 0x6f, 0x70, 0x69, 0x63, 0x5f, 0x34,
		0x37, 0x36, 0x37, 0x5f, 0x31, 0x33, 0x31, 0x36, 0x39, 0x31,
		0x32, 0x37, 0x34, 0x31, 0x35, 0x35, 0x37,
		0x0a, 0x00, 0x00, 0x00,
		0x6c, 0x61, 0x74, 0x63, 0x68, 0x69, 0x6e, 0x67, 0x3d, 0x31,
		0x27, 0x00, 0x00, 0x00,
		0x6d, 0x64, 0x35, 0x73, 0x75, 0x6d, 0x3d, 0x39, 0x39, 0x32,
		0x63, 0x65, 0x38, 0x61, 0x31, 0x36, 0x38, 0x37, 0x63, 0x65,
		0x63, 0x38, 0x63, 0x38, 0x62, 0x64, 0x38, 0x38, 0x33, 0x65,
		0x63, 0x37, 0x33, 0x63, 0x61, 0x34, 0x31, 0x64, 0x31,
		0x0e, 0x00, 0x00, 0x00,
		0x74, 0x6f, 0x70, 0x69, 0x63, 0x3d, 0x2f, 0x63, 0x68, 0x61,
		0x74, 0x74, 0x65, 0x72,
		0x14, 0x00, 0x00, 0x00,
		0x74, 0x79, 0x70, 0x65, 0x3d, 0x73, 0x74, 0x64, 0x5f, 0x6d,
		0x73, 0x67, 0x73, 0x2f, 0x53, 0x74, 0x72, 0x69, 0x6e, 0x67,
	}
	if !bytes.Equal(buffer.Bytes(), expected) {
		t.Fail()
	}
}
