rustplus-camera-decoder/main.cpp at main · danielbanar/rustplus-camera-decoder · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
#include "math.h"

int main()
{
	uint32_t checksumBuffer[64] = { 0 };
	std::vector<Ray> rays;
	//std::string input = "C0FFC01580FEC610F4C015E14A737977C60E22C255A2C27F2221C01ED074537453A113C61B24C6102215C60A3";
	std::string input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
	while (input.size() >= 4) // TODO: edit the value 6 later
	{
		byte firstByte = HexToByte(input.substr(0, 2));
		byte secondByte = HexToByte(input.substr(2, 2));
		byte thirdByte;
			byte fourthByte;
		if (input.size() > 4)
		{
			thirdByte = HexToByte(input.substr(4, 2));
			fourthByte = HexToByte(input.substr(6, 2));
		}

		if (firstByte >> 6 == 0)
		{
			//	Is checksum
			//
			//	if (material < 63)
			//  {
			//      checksumBuffer[checksum] = ray;
			//      output[value++] = (byte)(192 | material);
			//      output[value++] = (byte)(distance >> 2);
			//      output[value++] = (byte)((int)(distance & 3) << 6 | (int)alignment);
			//  }
			//
			uint32_t raw = checksumBuffer[firstByte];
			auto ray = IntToRay(raw);
			DEBUG(input.substr(0, 2) + " Checksum detected ");
			rays.push_back(ray);
			//ray.Print();
			input.erase(0, 2);
		}
		else if (firstByte >> 6 == 1)
		{
			//if (material == RayMaterial(newRay) && newDistance >= -15 && newDistance <= 16 && newAlignment >= -3 && newAlignment <= 4)
			//{
			//	output[value++] = (byte)(64 | checksum);
			//	output[value++] = (byte)(newDistance + 15 << 3 | newAlignment + 3);
			//}
			//material = firstbyte - checksum
			//distance <-15;16>
			//alignment <-3;4>

			byte checksum = firstByte & 0x3F;
			int newRay = checksumBuffer[checksum];
			byte material = RayMaterial(newRay);
			int newAlignment = (secondByte & 7) - 3;
			int newDistance = ((secondByte >> 3)) - 15;
			newDistance += RayDistance(newRay);
			newAlignment += RayAlignment(newRay);
			auto ray = Ray(newDistance, newAlignment, material);
			DEBUG(input.substr(0, 2) + " 1. condition detected ");
			rays.push_back(ray);
			//ray.Print();
			input.erase(0, 4);
		}
		else if (firstByte >> 6 == 2)
		{
			//
			//	else if (material == RayMaterial(newRay) && newAlignment == 0 && newDistance >= -127 && newDistance <= 128)
			//	{
			//	    output[value++] = (byte)(128 | checksum);
			//	    output[value++] = (byte)(newDistance + 127);
			//	}
			//
			//distance <= 128
			//checksum == 0 except for distance = 128
			//allignment == 0
			byte checksum = firstByte & 0x3F;
			int newRay = checksumBuffer[checksum];
			byte material = RayMaterial(newRay);
			byte alignment = RayAlignment(newRay);
			int newDistance = secondByte - 127;
			int distance = newDistance + RayDistance(newRay); //Well holt
			auto ray = Ray(distance, alignment, material);
			DEBUG(input.substr(0, 2) + " 2. condition detected ");
			rays.push_back(ray);
			//ray.Print();
			input.erase(0, 4);
		}
		else if (firstByte >> 6 == 3)
		{
			//rays.push_back(HexDecode(input.substr(0, 6)));
			//else if (material < 63)
			//{
			//	checksumBuffer[checksum] = ray;
			//	output[value++] = (byte)(192 | material);
			//	output[value++] = (byte)(distance >> 2);
			//	output[value++] = (byte)((int)(distance & 3) << 6 | (int)alignment); - zoberem prve dva bity, posuniem ich do lava o 6 a pridam alignment
			//
			//}

			byte material = firstByte - 192;
			uint16_t distance = (secondByte << 2) | (thirdByte >> 6);
			byte alignment = thirdByte & 0x3F;
			auto ray = Ray(distance, alignment, material);
			DEBUG(input.substr(0, 2) + " 3. condition detected ");
			rays.push_back(ray);
			//ray.Print();
			checksumBuffer[ray.GetChecksum()] = ray.GetRawRay();
			input.erase(0, 6);

		}
		else if (firstByte == 0xFF)
		{
			uint16_t distance = (secondByte << 2) | (thirdByte >> 6);
			byte alignment = thirdByte & 0x3F;
			byte material = fourthByte;
			auto ray = Ray(distance, alignment, material);
			DEBUG(input.substr(0, 2) + " 4. condition detected ");
			rays.push_back(ray);
			//ray.Print();
			checksumBuffer[ray.GetChecksum()] = ray.GetRawRay();
			input.erase(0, 8);
		}
		else
			std::cout << "Something is wrong i can feel it.\n";
	}
	std::cout << input.size();
	std::ofstream outfile("output.txt");

	// Check if the file was opened successfully
	if (!outfile.is_open()) {
		std::cerr << "Error opening file" << std::endl;
	}


	for (size_t i = 0; i < rays.size(); i+=2)
	{
		std::swap(rays[i], rays[rays.size()-2 - i]);
	}
	for (auto& ray : rays)
		outfile << "distance:" << (int)ray.m_distance << ", material: " << (int)ray.m_material << ", angle: " << (int)ray.m_alignment << std::endl;

	outfile.close();
}