/**
 * @file main.cpp
 * @author Bernd Giesecke (bernd@giesecke.tk)
 * @brief Radar sensor test
 * @version 0.1
 * @date 2024-06-25
 *
 * @copyright Copyright (c) 2024
 *
 */
#include <Arduino.h>
#include <Adafruit_TinyUSB.h>

#define SENSOR_OUT WB_IO3
#define SENSOR_EN WB_IO4
#define BLUE_LED LED_BLUE
#define GREEN_LED LED_GREEN

bool g_motion_trigger = false;
bool g_motion_status = false;

SemaphoreHandle_t motion_triggered;
static BaseType_t xHigherPriorityTaskWoken = pdTRUE;

void motion_cb(void)
{
	xSemaphoreGiveFromISR(motion_triggered, &xHigherPriorityTaskWoken);
}

void setup()
{
	pinMode(WB_IO2, OUTPUT);
	digitalWrite(WB_IO2, HIGH);

	pinMode(SENSOR_EN, OUTPUT);
	digitalWrite(SENSOR_EN, HIGH); // Sensor disable input (low = disable)

	time_t timeout = millis();
	Serial.begin(115200);
	while (!Serial)
	{
		if ((millis() - timeout) < 5000)
		{
			delay(100);
		}
		else
		{
			break;
		}
	}
	Serial.println("Rader 3GHZ example");

	pinMode(SENSOR_OUT, INPUT);
	pinMode(BLUE_LED, OUTPUT);
	pinMode(GREEN_LED, OUTPUT);

	// Use semaphore for wakeup
	motion_triggered = xSemaphoreCreateBinary();
	// Initialize semaphore
	xSemaphoreGive(motion_triggered);
	xSemaphoreTake(motion_triggered, 10);

	// Attach interrupt
	attachInterrupt(SENSOR_OUT, motion_cb, CHANGE);
	Serial.println("Waiting for motion");
}

void loop()
{
	// Wait for event
	if (xSemaphoreTake(motion_triggered, portMAX_DELAY) == pdTRUE)
	{
		int val = digitalRead(SENSOR_OUT);
		if ((val > 0) && (g_motion_status == false))
		{
			digitalWrite(BLUE_LED, HIGH);
			digitalWrite(GREEN_LED, LOW);
			Serial.println("Motion Detected");
			g_motion_status = true;
		}
		if ((val == 0) && (g_motion_status == true))
		{
			digitalWrite(BLUE_LED, LOW);
			digitalWrite(GREEN_LED, HIGH);
			Serial.println("NO Motion");
			g_motion_status = false;
		}
	}
}