#include "motion_gesture.h"

#include "ui_nav.h"

#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"

#include <assert.h>
#include <math.h>

static const char *TAG = "motion";

#define SAMPLE_PERIOD_MS         10
#define SAMPLE_RATE_HZ           100

/* Calibration: keep a sliding window of the last 500 ms and finish only when
 * the window is stable (low variance). This avoids calibrating while the user
 * is picking up or moving the device. */
#define CALIBRATION_MS           500
#define CALIBRATION_WINDOW_SAMPLES (CALIBRATION_MS / SAMPLE_PERIOD_MS)
#define CALIBRATION_MAX_VAR_G2   0.010f

/* Shake is detected on the horizontal plane (X/Y) so that picking the device
 * up or putting it down (Z-axis jolt) does not trigger navigation. */
#define THRESHOLD_LOW_G          0.6f
#define THRESHOLD_MED_G          0.8f
#define THRESHOLD_HIGH_G         1.0f

/* A valid shake must produce enough strong samples within a short window. */
#define WINDOW_MS                300
#define MIN_SAMPLES              5

/* A real shake oscillates; require at least two zero-crossings on the
 * dominant horizontal axis to distinguish it from a single jolt. */
#define MIN_ZERO_CROSSINGS       2

/* Cooldown prevents one physical shake from generating multiple events. */
#define COOLDOWN_MS              350

typedef enum {
    GESTURE_IDLE = 0,
    GESTURE_COOLDOWN,
} gesture_state_t;

static QueueHandle_t s_nav_queue;
static bool s_enabled = true;
static float s_threshold = THRESHOLD_MED_G;
static gesture_state_t s_state = GESTURE_IDLE;

/* Baseline calibration */
static float s_ref_x;
static float s_ref_y;
static float s_ref_z;
static bool s_calibrated;

static float s_cal_buf_x[CALIBRATION_WINDOW_SAMPLES];
static float s_cal_buf_y[CALIBRATION_WINDOW_SAMPLES];
static float s_cal_buf_z[CALIBRATION_WINDOW_SAMPLES];
static int s_cal_head;
static int s_cal_count;

/* Detection window state */
static int s_over_count;
static int64_t s_window_start_us;
static int s_zero_crossings;
static int s_last_x_sign;
static int s_last_y_sign;
static int64_t s_cooldown_until_us;

/* Last horizontal magnitude, exposed for UI/debug. */
static float s_last_horizontal_mag;

static void nav_handler_task(void *arg)
{
    (void)arg;
    motion_nav_event_t ev;

    while (xQueueReceive(s_nav_queue, &ev, portMAX_DELAY) == pdTRUE) {
        if (ev == MOTION_NAV_NEXT) {
            ui_nav_next();
        }
    }
}

static void post_nav_next(void)
{
    motion_nav_event_t ev = MOTION_NAV_NEXT;
    if (xQueueSend(s_nav_queue, &ev, 0) != pdTRUE) {
        ESP_LOGW(TAG, "nav queue full");
    } else {
        ESP_LOGI(TAG, "Shake detected -> next screen");
    }
}

static void reset_window(void)
{
    s_over_count = 0;
    s_window_start_us = 0;
    s_zero_crossings = 0;
    s_last_x_sign = 0;
    s_last_y_sign = 0;
}

static void run_calibration(float ax, float ay, float az)
{
    s_cal_buf_x[s_cal_head] = ax;
    s_cal_buf_y[s_cal_head] = ay;
    s_cal_buf_z[s_cal_head] = az;
    s_cal_head = (s_cal_head + 1) % CALIBRATION_WINDOW_SAMPLES;
    if (s_cal_count < CALIBRATION_WINDOW_SAMPLES) {
        s_cal_count++;
    }

    if (s_cal_count < CALIBRATION_WINDOW_SAMPLES) {
        return;
    }

    float sum_x = 0.0f;
    float sum_y = 0.0f;
    float sum_z = 0.0f;
    float sum_sq_x = 0.0f;
    float sum_sq_y = 0.0f;
    float sum_sq_z = 0.0f;

    for (int i = 0; i < CALIBRATION_WINDOW_SAMPLES; i++) {
        sum_x += s_cal_buf_x[i];
        sum_y += s_cal_buf_y[i];
        sum_z += s_cal_buf_z[i];
        sum_sq_x += s_cal_buf_x[i] * s_cal_buf_x[i];
        sum_sq_y += s_cal_buf_y[i] * s_cal_buf_y[i];
        sum_sq_z += s_cal_buf_z[i] * s_cal_buf_z[i];
    }

    const int n = CALIBRATION_WINDOW_SAMPLES;
    float var_x = (sum_sq_x / n) - (sum_x / n) * (sum_x / n);
    float var_y = (sum_sq_y / n) - (sum_y / n) * (sum_y / n);
    float var_z = (sum_sq_z / n) - (sum_z / n) * (sum_z / n);
    float max_var = fmaxf(var_x, fmaxf(var_y, var_z));

    if (max_var > CALIBRATION_MAX_VAR_G2) {
        static int64_t s_last_cal_warn_us;
        int64_t now_us = esp_timer_get_time();
        if ((now_us - s_last_cal_warn_us) >= 1000 * 1000) {
            s_last_cal_warn_us = now_us;
            ESP_LOGW(TAG, "Calibration: motion detected (var=%.3f), waiting", max_var);
        }
        return;
    }

    s_ref_x = sum_x / n;
    s_ref_y = sum_y / n;
    s_ref_z = sum_z / n;
    s_calibrated = true;

    ESP_LOGI(TAG, "Baseline calibrated (ref=%.3f,%.3f,%.3f g)", s_ref_x, s_ref_y, s_ref_z);
}

static int axis_sign(float value)
{
    if (value > 0.0f) {
        return 1;
    }
    if (value < 0.0f) {
        return -1;
    }
    return 0;
}

static void process_sample(float ax, float ay, float az)
{
    int64_t now_us = esp_timer_get_time();

    if (!s_calibrated) {
        run_calibration(ax, ay, az);
        return;
    }

    float dx = ax - s_ref_x;
    float dy = ay - s_ref_y;
    float dz = az - s_ref_z;

    /* Use only horizontal magnitude for shake detection. Z-axis is dominated by
     * gravity and is the main axis for pick-up/put-down false triggers. */
    float horizontal_mag = sqrtf(dx * dx + dy * dy);
    s_last_horizontal_mag = horizontal_mag;

    if (s_state == GESTURE_COOLDOWN) {
        if (now_us >= s_cooldown_until_us) {
            s_state = GESTURE_IDLE;
            reset_window();
        }
        return;
    }

    if (horizontal_mag > s_threshold) {
        if (s_over_count == 0) {
            s_window_start_us = now_us;
        }
        s_over_count++;

        int x_sign = axis_sign(dx);
        int y_sign = axis_sign(dy);
        if (s_last_x_sign != 0 && x_sign != s_last_x_sign) {
            s_zero_crossings++;
        }
        if (s_last_y_sign != 0 && y_sign != s_last_y_sign) {
            s_zero_crossings++;
        }
        s_last_x_sign = x_sign;
        s_last_y_sign = y_sign;

        int64_t window_us = (int64_t)WINDOW_MS * 1000;
        if ((now_us - s_window_start_us) > window_us) {
            /* Window expired: start fresh with the current sample. */
            reset_window();
            s_window_start_us = now_us;
            s_over_count = 1;
            s_last_x_sign = axis_sign(dx);
            s_last_y_sign = axis_sign(dy);
        }

        if (s_over_count >= MIN_SAMPLES && s_zero_crossings >= MIN_ZERO_CROSSINGS) {
            if (s_enabled) {
                post_nav_next();
            }
            s_state = GESTURE_COOLDOWN;
            s_cooldown_until_us = now_us + (int64_t)COOLDOWN_MS * 1000;
            reset_window();
        }
    } else if (s_over_count > 0) {
        int64_t window_us = (int64_t)WINDOW_MS * 1000;
        if ((now_us - s_window_start_us) > window_us) {
            reset_window();
        }
    }

    (void)dz;
}

void motion_gesture_init(motion_nav_cb_t cb)
{
    (void)cb;

    s_nav_queue = xQueueCreate(4, sizeof(motion_nav_event_t));
    xTaskCreate(nav_handler_task, "nav_handler", 2048, NULL, 5, NULL);

    s_calibrated = false;
    s_cal_head = 0;
    s_cal_count = 0;

    ESP_LOGI(TAG, "Motion gesture ready (calibrating %d ms)", CALIBRATION_MS);
}

void motion_gesture_set_enabled(bool enabled)
{
    s_enabled = enabled;
    ESP_LOGI(TAG, "Shake navigation %s", enabled ? "enabled" : "disabled");
}

void motion_gesture_set_sensitivity(shake_sensitivity_t sens)
{
    switch (sens) {
    case SHAKE_SENS_LOW:
        s_threshold = THRESHOLD_LOW_G;
        break;
    case SHAKE_SENS_MED:
        s_threshold = THRESHOLD_MED_G;
        break;
    case SHAKE_SENS_HIGH:
        s_threshold = THRESHOLD_HIGH_G;
        break;
    default:
        assert(false && "unexpected shake_sensitivity_t");
        break;
    }
    ESP_LOGI(TAG, "Sensitivity set (threshold=%.2f g)", s_threshold);
}

void motion_gesture_on_sample(float ax_g, float ay_g, float az_g)
{
    process_sample(ax_g, ay_g, az_g);
}

float motion_gesture_get_last_magnitude(void)
{
    return s_last_horizontal_mag;
}

bool motion_gesture_is_calibrated(void)
{
    return s_calibrated;
}