Encoder works but needs cleanup, GS checksum is not working properly somehow

communication/communication.cpp

@@ -46,10 +46,10 @@ bool Communication::close() {
 }
 
 bool Communication::sendPacket(std::vector<uint8_t>& bytes) {
-	std::cout << "\nWriting " << bytes.size() << " bytes\n" << std::hex;
-	for(size_t i = 0; i < bytes.size(); i++)
-		std::cout << std::setw(2) << std::setfill('0') << (int)bytes[i] << (i % 16 == 15 ? '\n' : ' ');
-	std::cout << '\n' << std::endl << std::dec;
+	// std::cout << "\nWriting " << bytes.size() << " bytes\n" << std::hex;
+	// for(size_t i = 0; i < bytes.size(); i++)
+	// 	std::cout << std::setw(2) << std::setfill('0') << (int)bytes[i] << (i % 16 == 15 ? '\n' : ' ');
+	// std::cout << '\n' << std::endl << std::dec;
 	return rawWrite(bytes);
 }
 
@@ -57,7 +57,7 @@ bool Communication::sendCommand(Command cmd, std::vector<uint8_t> arguments) {
 	auto msg = std::make_shared<Message>();
 	msg->network = Network::NetID::Main51;
 	msg->data = std::move(arguments);
-	msg->data.insert(msg->data.begin(), (uint8_t)cmd);
+	msg->data.insert(msg->data.begin(), {(uint8_t)cmd, (uint8_t)(uint16_t(cmd) >> 8)});
 	auto packet = encoder->encode(msg);
 	return sendPacket(packet);
 }
@@ -134,7 +134,7 @@ void Communication::readTask() {
 			if(packetizer->input(readBytes)) {
 				for(auto& packet : packetizer->output()) {
 					auto msg = decoder->decodePacket(packet);
-					std::cout << "Got packet for " << msg->network << ", calling " << messageCallbacks.size() << " callbacks" << std::endl;
+					//std::cout << "Got packet for " << msg->network << ", calling " << messageCallbacks.size() << " callbacks" << std::endl;
 					for(auto& cb : messageCallbacks) { // We might have closed while reading or processing
 						if(!closing) {
 							cb.second.callIfMatch(msg);

communication/decoder.cpp

@@ -51,6 +51,13 @@ std::shared_ptr<Message> Decoder::decodePacket(const std::shared_ptr<Packet>& pa
 								memcpy(msg->pcbSerial, packet->data.data() + 15, sizeof(msg->pcbSerial));
 							return msg;
 						}
+						break;
+						default:
+							auto msg = std::make_shared<Main51Message>();
+							msg->network = packet->network;
+							msg->command = Command(packet->data[0]);
+							msg->data.insert(msg->data.begin(), packet->data.begin() + 1, packet->data.end());
+							return msg;
 					}
 					break;
 				}
@@ -64,7 +71,7 @@ std::shared_ptr<Message> Decoder::decodePacket(const std::shared_ptr<Packet>& pa
 					 */
 					uint16_t length = packet->data[0] | (packet->data[1] << 8);
 					packet->network = Network(packet->data[2] & 0xF);
-					std::cout << "Got an old format packet, decoding against " << packet->network << std::endl;
+					//std::cout << "Got an old format packet, decoding against " << packet->network << std::endl;
 					packet->data.erase(packet->data.begin(), packet->data.begin() + 3);
 					if(packet->data.size() != length)
 						packet->data.resize(length);

communication/encoder.cpp

@@ -25,13 +25,26 @@ std::vector<uint8_t> Encoder::encode(const std::shared_ptr<Message>& message) {
 		default:
 			switch(message->network.getNetID()) {
 				case Network::NetID::Main51:
-					if(message->data.size() <= 0xF)
+					if(message->data.size() > 0xF) {
+						// Main51 can be sent as a long message without setting the NetID to RED first
+						// Size in long format is the size of the entire packet
+						// So +1 for AA header, +1 for short format header, and +2 for long format size
+						uint16_t size = uint16_t(message->data.size()) + 1 + 1 + 2;
+						size += 1; // Even though we are not including the NetID bytes, the device expects them to be counted in the length
+						message->data.insert(message->data.begin(), {
+							(uint8_t)Network::NetID::Main51, // 0x0B for long message
+							(uint8_t)size, // Size, little endian 16-bit
+							(uint8_t)(size >> 8)
+						});
+						return packetizer->packetWrap(message->data, shortFormat);
+					} else {
 						shortFormat = true;
+					}
 					break;
 				case Network::NetID::RED_OLDFORMAT: {
 					// See the decoder for an explanation
 					// We expect the network byte to be populated already in data, but not the length
-					uint16_t length = message->data.size() - 1;
+					uint16_t length = uint16_t(message->data.size()) - 1;
 					message->data.insert(message->data.begin(), {(uint8_t)length, (uint8_t)(length >> 8)});
 				}
 				default:

communication/message/filter/include/main51messagefilter.h

@@ -17,7 +17,7 @@ public:
 
 	bool match(const std::shared_ptr<Message>& message) const {
 		if(!MessageFilter::match(message)) {
-			std::cout << "message filter did not match base for " << message->network << std::endl;
+			//std::cout << "message filter did not match base for " << message->network << std::endl;
 			return false;
 		}
 		const auto main51Message = std::dynamic_pointer_cast<Main51Message>(message);