/// © MiroZ 2024

#include "app_config.h"
#include "TaskMgr.h"
#include "SensorService.h"
#include "Buffers.h"
#include "SensorData.h"
#include "Settings.h"
#include "Led.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();

	bool hw_fault = false;

	if(!m_bmp280->init())
	{
		hw_fault = true;
		ESP_LOGE(TAG, "bmp280 sensor error");
	}
	
	if(!m_bme68x->init())
	{
		hw_fault = true;
		ESP_LOGE(TAG, "bme68x sensor error");
	}

	if(!m_ld2410->init())
	{
		hw_fault = true;
		ESP_LOGE(TAG, "ld2410 sensor error");
	}

	if(hw_fault)
		m_app_if.getLed()->setColor(255, 0, 0);

	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);
}

	// Kalman filter variables
	static double kp_q = 0.5;    // process noise
	static double kp_r = 32;     // sensor noise  
	static double kp_p = 1023;   // estimation error
	static double kp_x = 0;    // initial value

	// Pressure monitoring variables
	static uint64_t pressure_period_started = 0;
	static float pressure_filtered_min = 0;
	static float pressure_filtered_max = 0;
	static const uint64_t pressure_window_ms = 1000; // 1000ms = 1 second

double getFilteredValue(double m) 
{

	if(kp_x == 0) kp_x=m;
    double k_k;
    kp_p = kp_p + kp_q;
    k_k = kp_p / (kp_p + kp_r);
    kp_x = kp_x + k_k * (m - kp_x);
    kp_p = (1 - k_k) * kp_p;
    return kp_x;
}



void::SensorService::processPressure(float pressure)
{
    //static uint64_t previous = esp_timer_get_time();
    
    // Get filtered pressure value using Kalman filter
    double filtered = getFilteredValue((double)pressure);
    double PRESSURE_THRESHOLD = 14.0;
    uint64_t now = esp_timer_get_time();
    uint64_t now_ms = now / 1000; // Convert to milliseconds
    
    // Initialize pressure monitoring period
    if (pressure_period_started == 0) {
        pressure_period_started = now_ms;
        pressure_filtered_min = filtered;
        pressure_filtered_max = filtered;
    }
    else {
        // Track min and max during the window
        if (filtered > pressure_filtered_max) {
            pressure_filtered_max = filtered;
			//ESP_LOGE(TAG, "pressure_filtered_max: %0.3f", filtered);
        }
        
        if (filtered < pressure_filtered_min) {
            pressure_filtered_min = filtered;
			//ESP_LOGE(TAG, "pressure_filtered_min: %0.3f", filtered);
        }
        
        // Check if window period has elapsed
        if (now_ms > (pressure_period_started + pressure_window_ms)) {
            double change = 1000*abs(pressure_filtered_max - pressure_filtered_min);
            ESP_LOGE(TAG, "@P: %0.3f", change);

            // Check for significant pressure change (door detection)
            if (change > PRESSURE_THRESHOLD) { // Threshold of 1
                //ESP_LOGE(TAG, "Door detected - pressure change: %0.3f", change);
                
                // Send MQTT notification
                struct MESSAGE_NOTIFY_PRESSURE msg;
                MQTT_MESSAGE_NOTIFY_PRESSURE(&msg);
                msg.value = change; // or you could send the change value
                m_app_if.getBuffer()->putBlock((uint8_t*)&msg, sizeof(msg));
				//ESP_LOGE(TAG, "Door detected - pressure change: %0.3f, time: %u.%06u", change, msg.header.sec, msg.header.usec);
				// Convert epoch time to human-readable format
				time_t timestamp = msg.header.sec;
				struct tm *timeinfo = localtime(&timestamp);
				
				char time_str[64];
				strftime(time_str, sizeof(time_str), "%Y-%m-%d %H:%M:%S", timeinfo);				
				ESP_LOGE(TAG, "Door detected - pressure change: %0.3f, time: %s.%06u", change, time_str, msg.header.usec);				
            }
            
            // Reset for next window
            pressure_period_started = now_ms;
            pressure_filtered_min = filtered;
            pressure_filtered_max = filtered;
        }
    }
    
    //previous = now;
    // Optional: store current filtered value for other uses
    //m_pressure_value = filtered;
}
void SensorService::postBme68xData(float pressure, float temp)
{
	uint8_t msg_buffer[sizeof(struct MESSAGE_SENSORS_BLOCK) + 10*sizeof(struct GAS_DATA)];
	struct MESSAGE_SENSORS_BLOCK * msg = (struct MESSAGE_SENSORS_BLOCK *)msg_buffer;
	MQTT_MESSAGE_BLOCK_SENSOR(msg);	
	msg->humidity = m_bme_data.humidity;
	msg->light = m_light_value;
	msg->pressure = pressure;
	msg->temperature = temp + SETTINGS.sensors.temperature.temp_offset;

	int num_total = 0;
	
	struct GAS_DATA * p = msg->data;
	for(int n = 0; n < 10; n++)
	{
		if(m_bme_data.measurement_bitmask & (1 << n))
		{
			p[num_total].resistance = m_bme_data.measurement[n].resistance;
			p[num_total++].index = n;
		}
	}
	msg->num_data = num_total;

	m_app_if.getBuffer()->putBlock((uint8_t*)msg, sizeof(*msg) + num_total * sizeof(struct GAS_DATA));

	// clear the blackboard
	memset(&m_bme_data, 0, sizeof(m_bme_data));
}

/// @brief Actual light value is minimum in 2s window
/// @param light_value 
void SensorService::processLight(int light_value)
{
	static uint16_t min_light_val = 0xffff;
	static int last_light_val = -1;
	static uint64_t last_time = esp_timer_get_time();
	uint64_t now;

	// handle light sensor
	if(light_value < min_light_val)
		min_light_val = light_value;

	now = esp_timer_get_time();

	if(now - last_time >= 2000000)	// >= 2s
	{
		if(last_light_val >= 0)
		{
			double change = abs(min_light_val - last_light_val);
            ESP_LOGE(TAG, "@L: %0.3f", change);

			if(change > 4096*2/100.0)
			{
				ESP_LOGI(TAG, "light tripped");
				struct MESSAGE_NOTIFY_LIGHT msg;
				MQTT_MESSAGE_NOTIFY_LIGHT(&msg);
				msg.value = abs(min_light_val - last_light_val);
				m_app_if.getBuffer()->putBlock((uint8_t*)&msg, sizeof(msg));
			}
		}
		last_light_val = min_light_val;
		m_light_value = min_light_val;
		min_light_val = 0xffff;
		last_time = now;
	}
}

// handles pressure and voc sensor
void SensorService::run_i2c_1()
{
	while(true)
	{
		if(m_bmp280->read(m_bmp_data.temp, m_bmp_data.pressure))
			processPressure(m_bmp_data.pressure);

		bool bme_cycle_finished = m_bme68x->read(&m_bme_data);
		uint16_t read_light_val = analogRead(LIGHT_SENSOR_PIN);

		if(bme_cycle_finished)
			postBme68xData(m_bmp_data.pressure, m_bmp_data.temp);

		processLight(read_light_val);

		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 MESSAGE_RADAR_BLOCK msg;
				MQTT_MESSAGE_BLOCK_RADAR(&msg);
				
				for(int n = 0; n < 24; n++)
				{
					if(n < 14)
						msg.vals[n] = m_ld2410->motion_energy[n] > 0xffff ? 0xffff : (uint16_t)m_ld2410->motion_energy[n];
					else
						msg.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*)&msg, sizeof(msg));

				// 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);
	}
}