1/* Copyright Statement:
2 *
3 * (C) 2021-2025  Cubios Inc. All rights reserved.
4 */
5 
6#include "native.h"
7#include "NetworkMessage.h"
8#include <sstream> 
9 
10namespace Cubios 
11{
12NetworkMessage::NetworkMessage(uint8_t* data,int length)
13{
14    this->m_numBytes = MESSAGE_SIZE_MAX;
15	this->m_usedBits = 0;
16	this->m_curPos = 0;
17 
18    memset(this->m_bytes,0x00,MESSAGE_SIZE_MAX);
19 
20    if(data!=NULL && length>0 && length<=MESSAGE_SIZE_MAX)
21    {
22		this->m_numBytes = length;
23        memcpy(this->m_bytes,data,length);
24    }
25}
26 
27NetworkMessage::~NetworkMessage()
28{
29}
30 
31bool NetworkMessage::checkOverflow(int bits)
32{
33	int newLength = this->m_usedBits + bits;
34	if (newLength > this->m_numBytes * 8) 
35    {
36		LOG_i("NetworkMessage: maximum message size exceeded");
37 
38        return true;
39	}
40 
41    return false;
42}
43 
44void NetworkMessage::Print()
45{  
46    std::stringstream ss;
47 
48	for (int curByte = 0; curByte < this->m_numBytes; curByte++) 
49    {
50		char str = m_bytes[curByte];
51		for (int i = 128; i > 0; i = i/2) 
52        {
53			ss << (bool) ((int) str & i);
54		}
55		ss << " ";
56 
57        if(curByte==(this->m_numBytes/2)-1)
58        {
59            LOG_i("%s",ss.str().c_str());
60 
61            ss.str("");
62            ss.clear();
63        }
64	}
65 
66    LOG_i("%s",ss.str().c_str());
67}
68 
69void NetworkMessage::WriteByte(uint8_t value)
70{
71	this->WriteInt(value,8);
72}
73 
74void NetworkMessage::WriteInt(int value, int bits)
75{
76	if(this->checkOverflow(bits)) return;
77 
78	if(value<0)
79	{
80		LOG_i("NetworkMessage::WriteInt: negative value %d may be sent incorrectly, use WriteSignedInt instead ",value);
81	}
82 
83	for (int i = 0; i < bits; i++) 
84    {
85		int byte = this->curByte();
86		int bit = this->curBit();
87 
88		// Create bitmask for the correspondent bit of the integer to store here
89		int mask = 1 << (bits - i - 1);
90 
91		// Whether to set the current bit or not
92		bool set = (bool) (value & mask);
93		this->m_bytes[byte] |= (uint8_t) set << (7 - bit);
94		this->m_curPos++;
95	}
96 
97	this->m_usedBits += bits;
98}
99 
100void NetworkMessage::WriteSignedInt(int value, int bits)
101{
102	if(this->checkOverflow(bits+1)) return;
103 
104	uint8_t byte = m_bytes[this->curByte()];
105	int sign = (value>>31) & 0x1;
106	byte |= (int)sign << (7 - this->curBit());
107	this->m_bytes[this->curByte()] = byte;
108 
109	this->m_curPos += 1;
110	this->m_usedBits += 1;
111 
112	for (int i = 0; i < bits; i++) 
113    {
114		int byte = this->curByte();
115		int bit = this->curBit();
116 
117		// Create bitmask for the correspondent bit of the integer to store here
118		int mask = 1 << (bits - i - 1);
119 
120		// Whether to set the current bit or not
121		bool set = (bool) (std::abs(value) & mask);
122		this->m_bytes[byte] |= (uint8_t) set << (7 - bit);
123		this->m_curPos++;
124	}
125 
126	this->m_usedBits += bits;
127}
128 
129void NetworkMessage::WriteBool(bool value)
130{
131	if(this->checkOverflow(1)) return;
132 
133	uint8_t byte = m_bytes[this->curByte()];
134	byte |= (int) value << (7 - this->curBit());
135	this->m_bytes[this->curByte()] = byte;
136 
137	this->m_curPos += 1;
138	this->m_usedBits += 1;
139}
140 
141void NetworkMessage::WriteFloat(float value)
142{
143    uint32_t fi;
144    memcpy(&fi,(uint32_t*)&value,sizeof(float));
145    this->WriteInt(fi,32);
146}
147 
148uint8_t NetworkMessage::ReadByte()
149{
150	return this->ReadInt(8);
151}
152 
153int NetworkMessage::ReadInt(int bits)
154{
155	int value = 0;
156 
157	for (int i = 0; i < bits; i++) 
158    {
159		int byte = this->curByte();
160		int bit = this->curBit();
161 
162		// Create bitmask for the correspondent bit of the data that we want to read
163		int mask = 1 << (7 - bit);
164 
165		// Whether to set the current bit or not
166		bool set = (bool) (this->m_bytes[byte] & mask);
167		value |= (int) set << (bits - i - 1);
168		this->m_curPos++;
169	}
170 
171	return value;
172}
173 
174int NetworkMessage::ReadSignedInt(int bits)
175{
176	int value = 0;
177 
178	bool negative = this->ReadBool();
179 
180	for (int i = 0; i < bits; i++) 
181    {
182		int byte = this->curByte();
183		int bit = this->curBit();
184 
185		// Create bitmask for the correspondent bit of the data that we want to read
186		int mask = 1 << (7 - bit);
187 
188		// Whether to set the current bit or not
189		bool set = (bool) (this->m_bytes[byte] & mask);
190		value |= (int) set << (bits - i - 1);
191		this->m_curPos++;
192	}
193 
194	if(negative) value*=-1;
195 
196	return value;
197}
198 
199bool NetworkMessage::ReadBool()
200{
201	unsigned int byte = this->m_bytes[this->curByte()];
202	unsigned int mask = 1 << (7 - this->curBit());
203 
204	this->m_curPos += 1;
205	return (byte & mask) > 0;
206}
207 
208float NetworkMessage::ReadFloat()
209{
210    int fi = this->ReadInt(32);
211    float ff;
212    memcpy((uint32_t*)&ff,&fi,sizeof(float));
213 
214    return ff;
215}
216 
217uint8_t* NetworkMessage::GetData(int& length)
218{
219	if(this->m_usedBits>0)
220	{
221		length = this->m_usedBits / 8;
222 
223		//take in account remaining bits, if any
224		if(this->m_usedBits % 8 != 0) length++;
225 
226		if(length>MESSAGE_SIZE_MAX) length = MESSAGE_SIZE_MAX;
227	}
228	else
229	{
230		//it's totaly legit to send empty network message since it carries out certain information anyway (its type)
231		//if this is the case, let's send 1 dummy byte
232		length = 1;
233	}
234 
235    return this->m_bytes;
236}
237 
238void NetworkMessage::SetData(uint8_t* data, int length)
239{
240    if(data!=NULL && length>0 && length<=MESSAGE_SIZE_MAX)
241    {
242        this->Reset(true);
243		this->m_numBytes = length;
244        memcpy(this->m_bytes,data,length);
245    }
246}
247 
248}
249