// https://arduino.stackexchange.com/questions/78714/bluetooth-modulehc-05-issues

void setup()
{
	Serial.begin(9600);
	Serial.println(F("\n\nSerial Packet Test\n"));
	pinMode(LED_BUILTIN, OUTPUT);
}

void loop()
{
	// 
	// TASK 1: Blink without delay to indicate that MCU hasn't frozen.
	//
	const unsigned int INTERVAL = 250;
	unsigned long current_timestamp = millis();
	static unsigned long previous_timestamp = current_timestamp;
	static bool led_state = false;
	if (current_timestamp - previous_timestamp >= INTERVAL)
	{
		led_state = !led_state;
		digitalWrite(LED_BUILTIN, led_state);
		previous_timestamp += INTERVAL;
	}

	//
	// TASK 2: Read and parse data packet.
	//
	static char packet[100];
	const byte NUM_FIELDS = 2;
	unsigned int fields[NUM_FIELDS];
	static unsigned int x = 512;   // Full resolution. No need to divide by 4.
	static unsigned int y = 512;   // Full resolution. No need to divide by 4.
	bool new_x = false;
	bool new_y = false;
	if (ReadPacket(Serial, packet, sizeof(packet)))
	{
		Serial.print(F("Received packet >"));
		Serial.print(packet);
		Serial.println(F("<"));

		// Parse the packet for x and y values at full resolution.
		const char delimiters[] = "{,}";
		char* field;
		int i = 0;
		field = strtok(packet, delimiters);
		while (field != NULL && i < NUM_FIELDS)
		{
			Serial.print(F("field = "));
			Serial.println(field);
			fields[i] = atoi(field);
			Serial.print(F("fields["));
			Serial.print(i);
			Serial.print(F("] = "));
			Serial.println(fields[i]);
			field = strtok(NULL, delimiters);
			i++;
		}
		if (i == NUM_FIELDS)
		{
			if (x != fields[0])
			{
				new_x = true;
				x = fields[0];  // Full resolution. No need to multiply by 4.
				Serial.print(F("New x = "));
				Serial.println(x);
			}
			if (y != fields[1])
			{
				new_y = true;
				y = fields[1];  // Full resolution. No need to multiply by 4.
				Serial.print(F("New y = "));
				Serial.println(y);
			}
		}
	}

	Serial.print(F("x = "));
	Serial.println(x);
	Serial.print(F("y = "));
	Serial.println(y);

	//
	// TASK 3: Process x and y values.
	//
	if (new_x || new_y)
	{
		Serial.println(F("Processing new values."));
	}
}

bool ReadPacket(Stream& stream, char *const packet, const unsigned int SIZE)
{
	const char FIRST = '{';
	const char LAST = '}';
	static bool read_until_last = false;
	static unsigned int i = 0;
	char ch;

	if (stream.available())
	{
		ch = stream.read();

		if (ch == FIRST)
		{
			Serial.println(F("Received FIRST."));
			i = 0;
			packet[0] = ch;
			read_until_last = true;
			return false;
		}
		if (read_until_last)
		{
			i++;
			if (i > SIZE - 2)
			{
				Serial.println(F("Buffer overrun. Resetting to look for next packet."));
				i = 0;
				read_until_last = false;
				return false;
			}
			if (ch == LAST)
			{
				Serial.println(F("Received LAST."));
				packet[i++] = ch;
				packet[i] = 0;
				read_until_last = false;
				return true;
			}
			else
			{
				Serial.print(F("Received char >"));
				Serial.print(ch);
				Serial.println(F("<"));
				packet[i] = ch;
				return false;
			}
		}
	}
	return false;
}