/// © MiroZ 2024

#include "app_config.h"
#include "TaskMgr.h"
#include "SensorService.h"
#include "Buffers.h"
#include "SensorData.h"

static const char *TAG = "sensors";

#define ms_to_us(ms) ((ms)*1000)
#define LIGHT_SENSOR_PIN	36

struct BMP_DATA m_bmp_data;
struct BME_DATA m_bme_data;


SensorService::SensorService(AppIF & app_if) : m_app_if(app_if)
{

}

void SensorService::start()
{
	ESP_LOGW(TAG, "Starting sensor service...");

	esp_log_level_set("gpio", ESP_LOG_WARN);

	memset(&m_bmp_data, 0, sizeof(m_bmp_data));
	memset(&m_bme_data, 0, sizeof(m_bme_data));

	m_bmp280 = new Bmp280(Wire);
	m_bme68x = new Bme68x(Wire);
	m_ld2410 = new LD2410();

	if(!m_bmp280->init())
		ESP_LOGE(TAG, "bmp280 sensor error");
	
	if(!m_bme68x->init())
		ESP_LOGE(TAG, "bme68x sensor error");

	if(!m_ld2410->init())
		ESP_LOGE(TAG, "ld2410 sensor error");

	assert(m_i2c1_task = TaskMgr::getInstance().createTask(std::bind(&SensorService::run_i2c_1, this), 
		I2C1_TASK_NAME, I2C1_TASK_STACK_SIZE, I2C1_TASK_PRIORITY, I2C1_TASK_CORE));

	assert(m_i2c2_task = TaskMgr::getInstance().createTask(std::bind(&SensorService::run_uart, this), 
		UART_TASK_NAME, UART_TASK_STACK_SIZE, UART_TASK_PRIORITY, UART_TASK_CORE));

	pinMode(LIGHT_SENSOR_PIN, INPUT);
}

// handles pressure and voc sensor
void SensorService::run_i2c_1()
{
	uint16_t light_val = 0xffff;
	int last_light_val = -1;
	uint64_t last_time = esp_timer_get_time(), now;

	while(true)
	{
		m_bmp280->read(m_bmp_data.temp, m_bmp_data.pressure);
		m_bme68x->read(&m_bme_data);
		uint16_t read_light_val = analogRead(LIGHT_SENSOR_PIN);

		// handle light sensor
		if(read_light_val < light_val)
			light_val = read_light_val;

		now = esp_timer_get_time();

		if(now - last_time >= 2000000)	// >= 2s
		{
			ESP_LOGI(TAG, "light: %d", light_val);
			if(last_light_val >= 0)
			{
				if(abs(light_val - last_light_val) > 4096*5/100)
				{
					ESP_LOGI(TAG, "light tripped");
					struct msg
					{
						struct MESSAGE_HEADER header;
						struct MESSAGE_TYPE_VAL_REASON light;
					};

					struct msg m;
					HEADER_MESSAGE_P(&m.header);
					LIGHT_MESSAGE_P(&m.light, light_val, 1);
					m_app_if.getBuffer()->putBlock((uint8_t*)&m, sizeof(m));
				}
			}
			last_light_val = light_val;
			light_val = 0xffff;
			last_time = now;
		}

		
		delay(10);
	}
}

// handles radar only
void SensorService::run_uart()
{
	int64_t last_read = esp_timer_get_time();

	while(true)
	{
		bool has_read = m_ld2410->read();

		if(has_read && esp_timer_get_time() - last_read >= ms_to_us(10000-50))
		{
			int64_t now = esp_timer_get_time();
			ESP_LOGI(TAG, "count %d", (int)m_ld2410->stationary_energy[0]);

			if(m_ld2410->stationary_energy[0] != 0)
			{
				struct msg
				{
					struct MESSAGE_HEADER header;
					struct RADAR radar;
				};

				struct msg m;
				HEADER_MESSAGE_P(&m.header);
				RADAR_MESSAGE_P(&m.radar);
				
				for(int n = 0; n < 24; n++)
				{
					if(n < 14)
						m.radar.vals[n] = m_ld2410->motion_energy[n] > 0xffff ? 0xffff : (uint16_t)m_ld2410->motion_energy[n];
					else
						m.radar.vals[n] = m_ld2410->stationary_energy[n-14] > 0xffff ? 0xffff : (uint16_t)m_ld2410->stationary_energy[n-14];
				}

				m_app_if.getBuffer()->putBlock((uint8_t*)&m, sizeof(m));

				ESP_LOGI(TAG, "delta t: %lld", (now - last_read)/1000);
				last_read = now;

#if 0	
				ESP_LOGI("stationary energy", "%0.0f %0.0f %0.0f %0.0f %0.0f %0.0f %0.0f", 
				m_ld2410->stationary_energy[3]/m_ld2410->stationary_energy[0],
				m_ld2410->stationary_energy[4]/m_ld2410->stationary_energy[0],
				m_ld2410->stationary_energy[5]/m_ld2410->stationary_energy[0],
				m_ld2410->stationary_energy[6]/m_ld2410->stationary_energy[0],
				m_ld2410->stationary_energy[7]/m_ld2410->stationary_energy[0],
				m_ld2410->stationary_energy[8]/m_ld2410->stationary_energy[0],
				m_ld2410->stationary_energy[9]/m_ld2410->stationary_energy[0]);
			

				ESP_LOGW("motion energy", "%0.0f %0.0f %0.0f %0.0f %0.0f %0.0f %0.0f %0.0f %0.0f", 
				m_ld2410->motion_energy[5]/m_ld2410->motion_energy[0],
				m_ld2410->motion_energy[6]/m_ld2410->motion_energy[0],
				m_ld2410->motion_energy[7]/m_ld2410->motion_energy[0],
				m_ld2410->motion_energy[8]/m_ld2410->motion_energy[0],
				m_ld2410->motion_energy[9]/m_ld2410->motion_energy[0],
				m_ld2410->motion_energy[10]/m_ld2410->motion_energy[0],
				m_ld2410->motion_energy[11]/m_ld2410->motion_energy[0],
				m_ld2410->motion_energy[12]/m_ld2410->motion_energy[0],
				m_ld2410->motion_energy[13]/m_ld2410->motion_energy[0]);
#endif
				m_ld2410->resetGates();
			}
		}
		if(has_read)	
			// next read will happen in 100ms. sleep untill just before then.
			delay(95);
	}
}