#ifndef NAV_ESTIMATOR_H
#define NAV_ESTIMATOR_H

#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>

#define useSatInfo

#define NAV_DRIFT_HISTORY 20
#define NAV_STD_IMU_WINDOW 20
#define NAV_EARTH_RADIUS_M 6371000.0
#define NAV_DEG_TO_RAD (M_PI / 180.0)  // Константа для преобразования градусов в радианы

#define STATIC_IMU_THRESHOLD 0.01
#define LOOP_THRESHOLD 100.0
#define HDOP_THRESHOLD 5
#define PDOP_THRESHOLD 6

#ifdef useSatInfo

#define SAT_MAX_NUMBER (20)
typedef struct {
    int id;
    int in_use;
    int elv;
    int azimuth;
    int sig;
} tSatellite;

typedef struct {
    int inuse;
    int inview;
    tSatellite sat[SAT_MAX_NUMBER];
} tSatellitesInfo;
#endif

typedef enum {
    Quality_Invalid = 0,
    Quality_Fix,
    Quality_Differential,
    Quality_Sensitive
} eGpsQuality;

typedef enum {
    FixNotAvailable = 0,
    Fix_Differential,
    Fix_2D,
    Fix_3D
} eFixType;

typedef struct {
    double latitude;
    double longitude;
}tNavPosition;

typedef struct {
    unsigned long timestamp;

    eGpsQuality quality;
    eFixType fix;

    double PDOP;
    double HDOP;
    double VDOP;

    double latitude;
    double longitude;
    double elv;
    double speed;
    double direction;
    double declination;
#ifdef useSatInfo
    tSatellitesInfo satinfo;
#endif
} tNavigationData;

typedef struct {
    double x;
    double y;
    double z;
} t3DVectorData;

typedef struct {
    t3DVectorData accelerometer;
    t3DVectorData gyroscope;
    t3DVectorData magnetometer;
} tIMU;

typedef struct {
    tNavigationData history[NAV_DRIFT_HISTORY];
    int head;
    int count;
} tDriftBuffer;

typedef struct {
    t3DVectorData accelHistory[NAV_STD_IMU_WINDOW];
    int accelHead;
    int accelCount;
} tIMUVarianceBuffer;

typedef struct {
    double x;
    double vx;
    double ax;
    double y;
    double vy;
    double ay;
    double bias1;
    double bias2;
} tKalmanState2D;

typedef struct {
    double P[6][6];
    double Q[6]; // Process noise
    double R[2]; // Measurement noise (lat/lon)
} tKalmanFilter2D;

typedef struct {
    tNavigationData lastNav;
    bool isNavDataUpdate;
    tIMU lastIMU;
    bool isIMUUpdate;

    tDriftBuffer drift;
    tIMUVarianceBuffer imuStats;

    tKalmanState2D kalmanState;
    tKalmanFilter2D kalmanFilter;
    unsigned long lastKalmanUpdate;

    double confidence;
} tNavEstimator;


//public
//  init
void NavEstimator_Init(tNavEstimator* ctx);

//  loop
void NavEstimator_Filter(tNavEstimator* ctx);

//  setters
void NavEstimator_SetNav(tNavEstimator* ctx, const tNavigationData* nav);
void NavEstimator_SetIMU(tNavEstimator* ctx, const tIMU* imu);

//  getters
double NavEstimator_GetConfidence(const tNavEstimator* ctx);
void NavEstimator_GetPosition(const tNavEstimator* ctx, double* lat,double* lon);

//private
bool NavEstimator_IsLoopDetected(const tDriftBuffer* buf);
bool NavEstimator_IsStaticIMU(const tIMUVarianceBuffer* buf);
double NavEstimator_CalcIMUVariance(const tIMUVarianceBuffer* buf);
double NavEstimator_Haversine(double lat1, double lon1, double lat2, double lon2);
void NavEstimator_KalmanPredict(tNavEstimator* ctx, double dt);
void NavEstimator_KalmanUpdate(tNavEstimator* ctx, double measuredLat, double measuredLon);
double NavEstimator_KalmanInnovationNorm(const tNavEstimator* ctx, double measuredLat, double measuredLon);
void NavEstimator_KalmanUpdateMatrix(tKalmanFilter2D* kf, tKalmanState2D* state, double measuredLat, double measuredLon);

#endif // NAV_ESTIMATOR_H

extern unsigned long SystemGetMs();

void NavEstimator_Init(tNavEstimator* ctx)
{

    if (!ctx) return;

    // Обнуляем все поля структуры
    memset(ctx, 0, sizeof(tNavEstimator));

    // Инициализируем кольцевой буфер дрейфа
    ctx->drift.head = 0;
    ctx->drift.count = 0;

    // Инициализируем буфер статистики IMU
    ctx->imuStats.accelHead = 0;
    ctx->imuStats.accelCount = 0;

    // Инициализируем состояние Калмана
    tKalmanState2D* state = &ctx->kalmanState;
    state->x = 0;
    state->vx = 0;
    state->ax = 0;
    state->y = 0;
    state->vy = 0;
    state->ay = 0;

    // Инициализация матрицы ковариаций ошибок
    tKalmanFilter2D* kf = &ctx->kalmanFilter;
    memset(kf->P, 0, sizeof(kf->P));
    for (int i = 0; i < 6; ++i) {
        kf->P[i][i] = 1.0; // начальная неопределенность
    }

    // Настройка шумов процесса (можно подбирать)
    for (int i = 0; i < 6; ++i) {
        kf->Q[i] = 1e-3;
    }

    // Настройка шумов измерений (широта/долгота)
    kf->R[0] = 1e-4; // лат
    kf->R[1] = 1e-4; // лон

    ctx->confidence = 0.0;
}

void NavEstimator_SetNav(tNavEstimator* ctx, const tNavigationData* nav)
{
    if (!ctx || !nav) return;

    // Если буфер полон, сдвигаем его вперед
    if (ctx->drift.count == NAV_DRIFT_HISTORY) {
        ctx->drift.head = (ctx->drift.head + 1) % NAV_DRIFT_HISTORY;
    } else {
        ctx->drift.count++;
    }

    // Записываем новые данные в буфер
    ctx->drift.history[(ctx->drift.head + ctx->drift.count - 1) % NAV_DRIFT_HISTORY] = *nav;

    // Обновляем последние полученные данные
    ctx->lastNav = *nav;

    // Выставляем флаг обновления навигации
    ctx->isNavDataUpdate = true;
}

void NavEstimator_SetIMU(tNavEstimator* ctx, const tIMU* imu)
{
    if (!ctx || !imu) return;

    // Если буфер полон, сдвигаем его вперед
    if (ctx->imuStats.accelCount == NAV_STD_IMU_WINDOW) {
        ctx->imuStats.accelHead = (ctx->imuStats.accelHead + 1) % NAV_STD_IMU_WINDOW;
    } else {
        ctx->imuStats.accelCount++;
    }

    // Записываем новые данные в буфер
    ctx->imuStats.accelHistory[(ctx->imuStats.accelHead + ctx->imuStats.accelCount - 1) % NAV_STD_IMU_WINDOW] = imu->accelerometer;

    // Обновление последних данных IMU
    ctx->lastIMU = *imu;

    // Пример дальнейших действий:
    // Вычисление статистики для выявления аномального дрейфа
    double variance = NavEstimator_CalcIMUVariance(&ctx->imuStats);

    // Например, можно обновить состояние фильтра Калмана или принять решение
    // о стабильности IMU (например, если variance слишком высок, это может
    // указывать на нестабильность)
    ctx->isIMUUpdate = true;
}

double NavEstimator_GetConfidence(const tNavEstimator* ctx)
{
    if (!ctx) return 0.0;  // Возвращаем 0, если контекст не задан

    double confidence = 0.0;

    // Смотрим в буфере статус каждого измерения, если координаты были фиксированы,
    // увеличиваем доверие, иначе уменьшаем
    if (ctx->drift.count == NAV_DRIFT_HISTORY) {
        for(int i = NAV_DRIFT_HISTORY - 1; i >= 0; i--) {
            confidence += ctx->drift.history->quality == Quality_Fix ? 0.005 : -0.005;
            confidence += ctx->drift.history->HDOP > HDOP_THRESHOLD ? 0.005 : -0.005;
            confidence += ctx->drift.history->PDOP > PDOP_THRESHOLD ? 0.005 : -0.005;
        }
    }

    // Если обнаружили зацикленность маршрута, доверие снижается
    if(NavEstimator_IsLoopDetected(&ctx->drift)){
        confidence -= 0.3;
    }
    // Дополнительно учитываем данные с IMU
    if(NavEstimator_IsStaticIMU(&ctx->imuStats)){
        confidence += 0.1;
    }

    if (ctx->imuStats.accelCount > 0) {
        double imuVariance = NavEstimator_CalcIMUVariance(&ctx->imuStats);

        // Если IMU стабильное (дисперсия мала), увеличиваем доверие
        if (imuVariance < 0.1) {
            confidence += 0.05;  // Добавляем 0.2, если IMU стабильный
        }
            // Если IMU нестабильное (дисперсия велика), уменьшаем доверие
        else if (imuVariance > 1.0) {
            confidence -= 0.05;  // Уменьшаем доверие
        }
    }

    // Усредняем с имеющимся в контексте значением доверия
    confidence += ctx->confidence;
    confidence /= 2;

    // Ограничиваем доверие от 0 до 1
    if (confidence > 1.0) {
        confidence = 1.0;
    } else if (confidence < 0.0) {
        confidence = 0.0;
    }

    return confidence;
}

bool NavEstimator_IsLoopDetected(const tDriftBuffer* buf)
{
    if (!buf || buf->count < 2) {
        // Если буфер пуст или в нем меньше 2-х записей, петля не может быть обнаружена
        return false;
    }

    // Определяем максимально допустимый радиус для "петли" (например, 100 метров)
    const double loopThreshold = LOOP_THRESHOLD;  // метров

    // Получаем последние две позиции из буфера
    const tNavigationData* lastNav = &buf->history[(buf->head - 1 + NAV_DRIFT_HISTORY) % NAV_DRIFT_HISTORY];
    const tNavigationData* secondLastNav = &buf->history[(buf->head - 2 + NAV_DRIFT_HISTORY) % NAV_DRIFT_HISTORY];

    // Вычисляем расстояние между последними двумя позициями с помощью Haversine формулы
    double distance = NavEstimator_Haversine(
            secondLastNav->latitude, secondLastNav->longitude,
            lastNav->latitude, lastNav->longitude
    );

    // Если расстояние между последними двумя точками меньше порогового значения (100 метров), это может быть петля
    return distance < loopThreshold;
}

bool NavEstimator_IsStaticIMU(const tIMUVarianceBuffer* buf)
{
    if (!buf || buf->accelCount < NAV_STD_IMU_WINDOW) {
        // Если буфер пуст или в нем недостаточно данных для статистики
        return false;
    }

    // Рассчитываем дисперсию для каждого из векторов акселерометра
    double accelVarianceX = 0.0;
    double accelVarianceY = 0.0;
    double accelVarianceZ = 0.0;

    // Вычисляем среднее значение для каждого из каналов акселерометра
    double meanX = 0.0, meanY = 0.0, meanZ = 0.0;
    for (int i = 0; i < NAV_STD_IMU_WINDOW; i++) {
        meanX += buf->accelHistory[i].x;
        meanY += buf->accelHistory[i].y;
        meanZ += buf->accelHistory[i].z;
    }
    meanX /= NAV_STD_IMU_WINDOW;
    meanY /= NAV_STD_IMU_WINDOW;
    meanZ /= NAV_STD_IMU_WINDOW;

    // Вычисляем дисперсию для каждого канала
    for (int i = 0; i < NAV_STD_IMU_WINDOW; i++) {
        accelVarianceX += pow(buf->accelHistory[i].x - meanX, 2);
        accelVarianceY += pow(buf->accelHistory[i].y - meanY, 2);
        accelVarianceZ += pow(buf->accelHistory[i].z - meanZ, 2);
    }
    accelVarianceX /= NAV_STD_IMU_WINDOW;
    accelVarianceY /= NAV_STD_IMU_WINDOW;
    accelVarianceZ /= NAV_STD_IMU_WINDOW;

    // Рассчитываем общую дисперсию для акселерометра
    double totalVariance = (accelVarianceX + accelVarianceY + accelVarianceZ) / 3.0;

    // Порог для статичного состояния (например, если дисперсия меньше 0.01)
    const double staticThreshold = STATIC_IMU_THRESHOLD;

    // Если общая дисперсия ниже порога, значит, устройство, вероятно, стоит на месте
    return totalVariance < staticThreshold;
}

double NavEstimator_CalcIMUVariance(const tIMUVarianceBuffer* buf)
{
    if (!buf || buf->accelCount < NAV_STD_IMU_WINDOW) {
        // Если буфер пуст или в нем недостаточно данных для статистики
        return -1.0; // Вернем -1 для обозначения ошибки
    }

    // Рассчитываем среднее значение для каждого из каналов акселерометра (X, Y, Z)
    double meanX = 0.0, meanY = 0.0, meanZ = 0.0;

    for (int i = 0; i < NAV_STD_IMU_WINDOW; i++) {
        meanX += buf->accelHistory[i].x;
        meanY += buf->accelHistory[i].y;
        meanZ += buf->accelHistory[i].z;
    }

    meanX /= NAV_STD_IMU_WINDOW;
    meanY /= NAV_STD_IMU_WINDOW;
    meanZ /= NAV_STD_IMU_WINDOW;

    // Рассчитываем дисперсию для каждого канала (X, Y, Z)
    double varianceX = 0.0, varianceY = 0.0, varianceZ = 0.0;

    for (int i = 0; i < NAV_STD_IMU_WINDOW; i++) {
        varianceX += pow(buf->accelHistory[i].x - meanX, 2);
        varianceY += pow(buf->accelHistory[i].y - meanY, 2);
        varianceZ += pow(buf->accelHistory[i].z - meanZ, 2);
    }

    // Получаем среднюю дисперсию для всех трех каналов
    varianceX /= NAV_STD_IMU_WINDOW;
    varianceY /= NAV_STD_IMU_WINDOW;
    varianceZ /= NAV_STD_IMU_WINDOW;

    // Возвращаем среднюю дисперсию по всем каналам
    return (varianceX + varianceY + varianceZ) / 3.0;
}

double NavEstimator_Haversine(double lat1, double lon1, double lat2, double lon2)
{
    // Переводим координаты из градусов в радианы
    lat1 *= NAV_DEG_TO_RAD;
    lon1 *= NAV_DEG_TO_RAD;
    lat2 *= NAV_DEG_TO_RAD;
    lon2 *= NAV_DEG_TO_RAD;

    // Разница между широтами и долготами
    double dlat = lat2 - lat1;
    double dlon = lon2 - lon1;

    // Применяем формулу Хаверсина
    double a = sin(dlat / 2) * sin(dlat / 2) +
               cos(lat1) * cos(lat2) * sin(dlon / 2) * sin(dlon / 2);
    double c = 2 * atan2(sqrt(a), sqrt(1 - a));

    // Радиус Земли в метрах
    double R = NAV_EARTH_RADIUS_M;

    // Возвращаем расстояние между точками в метрах
    return R * c;
}


void NavEstimator_Filter(tNavEstimator* ctx)
{
    if(!ctx) return;
    if((ctx->isNavDataUpdate == false) &&(ctx->isIMUUpdate == false)) return;

    ctx->isNavDataUpdate = false;
    ctx->isIMUUpdate = false;

    unsigned long dt = 0;
    if(ctx->lastKalmanUpdate != 0) dt = SystemGetMs() - ctx->lastKalmanUpdate;

    NavEstimator_KalmanPredict(ctx,dt);
    NavEstimator_KalmanUpdate(ctx, ctx->lastNav.latitude, ctx->lastNav.latitude);
}


void NavEstimator_KalmanPredict(tNavEstimator* ctx, double dt)
{
    if (!ctx) return;

    // Прогнозирование позиции и скорости по фильтру Калмана
    tKalmanState2D* state = &ctx->kalmanState;

    // Обновляем положение с учетом скорости и ускорения
    state->x += state->vx * dt + 0.5 * state->ax * dt * dt;  // X = X + Vx * dt + 0.5 * Ax * dt^2
    state->y += state->vy * dt + 0.5 * state->ay * dt * dt;  // Y = Y + Vy * dt + 0.5 * Ay * dt^2
    state->vx += state->ax * dt;  // Vx = Vx + Ax * dt
    state->vy += state->ay * dt;  // Vy = Vy + Ay * dt

    // Прогнозируем погрешности (обновляем матрицу P)
    // Допустим, что процессное шумовое распределение (Q) и матрица ковариаций обновляются по времени
    for (int i = 0; i < 6; ++i) {
        ctx->kalmanFilter.P[i][i] += ctx->kalmanFilter.Q[i] * dt;
    }
}

void NavEstimator_KalmanUpdate(tNavEstimator* ctx, double measuredLat, double measuredLon)
{
    if (!ctx) return;

    // Измерения
    double measured[2] = { measuredLat, measuredLon };

    // Калькуляция инновативной нормы (разница между предсказанными и измеренными значениями)
    double innovationNorm = NavEstimator_KalmanInnovationNorm(ctx, measured[0], measured[1]);

    // Обновление состояния с помощью фильтра Калмана
    NavEstimator_KalmanUpdateMatrix(&ctx->kalmanFilter, &ctx->kalmanState, measured[0], measured[1]);

    // Обновляем уверенность в данных на основе инновативной нормы
    if (innovationNorm < 1.0) {
        ctx->confidence = 1.0;
    } else {
        ctx->confidence = 0.5;
    }
}

void NavEstimator_KalmanUpdateMatrix(tKalmanFilter2D* kf, tKalmanState2D* state, double measuredLat, double measuredLon)
{
    // Ошибка измерения (инновация)
    double y[2] = {
            measuredLat - state->x,
            measuredLon - state->y
    };

    // S = H * P * H^T + R
    double S[2][2] = {
            {kf->P[0][0] + kf->R[0], kf->P[0][1]},
            {kf->P[1][0], kf->P[1][1] + kf->R[1]}
    };

    // Вычисление обратной матрицы S (2x2)
    double detS = S[0][0] * S[1][1] - S[0][1] * S[1][0];
    if (fabs(detS) < 1e-8) return;  // сингулярная матрица

    double invS[2][2] = {
            { S[1][1] / detS, -S[0][1] / detS },
            { -S[1][0] / detS, S[0][0] / detS }
    };

    // K = P * H^T * inv(S)
    double K[6][2];
    for (int i = 0; i < 6; ++i) {
        K[i][0] = kf->P[i][0] * invS[0][0] + kf->P[i][1] * invS[1][0];
        K[i][1] = kf->P[i][0] * invS[0][1] + kf->P[i][1] * invS[1][1];
    }

    // Обновление состояния: x = x + K * y
    state->x += K[0][0] * y[0] + K[0][1] * y[1];
    state->y += K[1][0] * y[0] + K[1][1] * y[1];
    state->vx += K[2][0] * y[0] + K[2][1] * y[1];
    state->vy += K[3][0] * y[0] + K[3][1] * y[1];
    state->bias1 += K[4][0] * y[0] + K[4][1] * y[1];
    state->bias2 += K[5][0] * y[0] + K[5][1] * y[1];

    // Обновление ковариационной матрицы: P = (I - K * H) * P
    // H — 2x6: [1 0 0 0 0 0; 0 1 0 0 0 0]
    double KH[6][6] = {0};
    for (int i = 0; i < 6; ++i) {
        KH[i][0] = K[i][0];  // K * H: только два столбца
        KH[i][1] = K[i][1];
    }

    double I_KH[6][6];
    for (int i = 0; i < 6; ++i)
        for (int j = 0; j < 6; ++j)
            I_KH[i][j] = (i == j ? 1.0 : 0.0) - KH[i][j];

    // P = (I - K * H) * P
    double newP[6][6] = {0};
    for (int i = 0; i < 6; ++i)
        for (int j = 0; j < 6; ++j)
            for (int k = 0; k < 6; ++k)
                newP[i][j] += I_KH[i][k] * kf->P[k][j];

    // Копируем обратно
    for (int i = 0; i < 6; ++i)
        for (int j = 0; j < 6; ++j)
            kf->P[i][j] = newP[i][j];
}

double NavEstimator_KalmanInnovationNorm(const tNavEstimator* ctx, double measuredLat, double measuredLon)
{
    if (!ctx) return -1.0;

    double predictLat = ctx->kalmanState.x;
    double predictLon =  ctx->kalmanState.y;

    return NavEstimator_Haversine(predictLat,predictLon,measuredLat,measuredLon);
}

void NavEstimator_GetPosition(const tNavEstimator* ctx, double* lat,double* lon)
{
    *lat = ctx->kalmanState.x;
    *lon = ctx->kalmanState.y;
}