enum color {
	GREEN,
	YELLOW,
	RED
};
const uint8_t light_pin[] = {
	[GREEN] = 14,
	[YELLOW] = 27,
	[RED] = 26,
};
const uint8_t echo_pin = 12;
const uint8_t trig_pin = 13;

void setup() {
	Serial.begin(115200);
	Serial.println("Start.");

	pinMode(trig_pin, OUTPUT);
	pinMode(echo_pin, INPUT);
	for (int i = 0; i < 3; i++)
		pinMode(light_pin[i], OUTPUT);

	xTaskCreate(
		ping,
		"Ping",
		1024,
		NULL,
		1,
		NULL
	);

	xTaskCreate(
		echo,
		"Echo",
		1024,
		NULL,
		1,
		NULL
	);
}

void loop() {
	for (;;) {
		vTaskDelay(500);
		delay(500);
	}
}

void ping(void *pvParameters) {
	for (;;) {
		digitalWrite(trig_pin, LOW);
		delayMicroseconds(5);

		digitalWrite(trig_pin, HIGH);
		delayMicroseconds(10);
		digitalWrite(trig_pin, LOW);
	}

	vTaskDelete(NULL);
}

void echo(void *pvParameters) {
	for (;;) {
		const float duration = pulseIn(echo_pin, HIGH);
		const float distance = duration * 0.034 / 2;
		Serial.println(distance);

		if (distance < 50) {
			Serial.println("Опасност от сблъсък.");
			digitalWrite(light_pin[GREEN], LOW);
			digitalWrite(light_pin[YELLOW], LOW);
			digitalWrite(light_pin[RED], HIGH);
		} else if (distance < 200) {
			Serial.println("Внимание.");
			digitalWrite(light_pin[GREEN], LOW);
			digitalWrite(light_pin[YELLOW], HIGH);
			digitalWrite(light_pin[RED], LOW);
		} else if (distance < 300) {
			Serial.println("Обект в периметъра.");
			digitalWrite(light_pin[GREEN], HIGH);
			digitalWrite(light_pin[YELLOW], LOW);
			digitalWrite(light_pin[RED], LOW);
		} else {
			digitalWrite(light_pin[GREEN], LOW);
			digitalWrite(light_pin[YELLOW], LOW);
			digitalWrite(light_pin[RED], LOW);
		}
	}

	vTaskDelete(NULL);
}