//
// Created by cfif on 16.05.2024.
//
#include "CarPositionUpdate.h"
#include "math.h"
#include "stdlib.h"
#include "SystemDelayInterface.h"
#include "EgtsOutputCommands.h"

#define LOG_SIGN "EGTS MOVE"
#define LOGGER &env->slog->logger

static double nmeaLocationToDeg(double dec) {
    double _dec = dec;
    int deg = (int) (_dec / 100);
    int min = (int) (_dec) - (deg * 100);

    double sec = (double) (_dec - min - 100 * deg) * 60.0;

    return deg + min / 60.0 + sec / 3600.0;
}

void CarEventResetPosition(tEgtsProcessing *env) {
    env->carEventPosition.carPosition = CAR_POSITION_FIX;
}

void CarEventMovingPosition(tEgtsProcessing *env) {
    env->carEventPosition.carPosition = CAR_POSITION_MOVE;

    env->carEventPosition.timerMoving = CAR_EVENT_TIME_MOVING;
    env->carEventPosition.isTimerMoving = true;
}

bool isMovingSpeed(tEgtsProcessing *env, uint16_t speed) {

    for (uint8_t i = 0; i < CAR_POINT_COUNT_SPEED; ++i) {
        if (env->carEventPosition.dataSpeed[i] < speed)
            return false;
    }

    return true;
}

void CarEventPosition(tEgtsProcessing *env, tNmeaRmc *position, uint16_t totalSpeed) {

    if (env->carEventPosition.isTimerMoving) {
        if (env->carEventPosition.timerMoving < SystemGetMs()) {
            LoggerInfoStatic(LOGGER, LOG_SIGN, "Сброс признака движения")
            env->carEventPosition.isTimerMoving = false;
            CarEventResetPosition(env);
        }
    }

    if (position->status != 'A')
        return;

    if (env->egtsPointEnv.timeOutCheckTeledata_TimeSpeedUpdate < SystemGetMs()) {

        env->egtsPointEnv.timeOutCheckTeledata_TimeSpeedUpdate = SystemGetMs() + CAR_EVENT_TIME_SPEED;

        env->carEventPosition.dataSpeed[env->carEventPosition.dataSpeedStep] = totalSpeed;
        ++env->carEventPosition.dataSpeedStep;

        if (env->carEventPosition.dataSpeedStep >= CAR_POINT_COUNT_SPEED) {
            env->carEventPosition.dataSpeedStep = 0;
        }
    }

    if (isMovingSpeed(env, CAR_TRESHOLD_DETECT_SPEED)) {
        CarEventMovingPosition(env);
    }


    // Установка точки отсчета
    if (env->carEventPosition.stopPositionLimitTime < SystemGetMs()) {

        env->carEventPosition.beginPosition.location.latitude = nmeaLocationToDeg(position->location.latitude);
        env->carEventPosition.beginPosition.location.longitude = nmeaLocationToDeg(position->location.longitude);

        env->carEventPosition.stopPositionLimitTime = SystemGetMs() + CAR_EVENT_TIME_DISTANCE;
    }

    env->carEventPosition.movePosition.location.latitude = nmeaLocationToDeg(position->location.latitude);
    env->carEventPosition.movePosition.location.longitude = nmeaLocationToDeg(position->location.longitude);

    double latDistance = fabs(env->carEventPosition.movePosition.location.latitude -
                              env->carEventPosition.beginPosition.location.latitude);
    double lonDistance = fabs(env->carEventPosition.movePosition.location.longitude -
                              env->carEventPosition.beginPosition.location.longitude);

    double x = latDistance * 0.1;
    double y = lonDistance * 0.1;
    double move = sqrt(x * x + y * y);

    if ((uint32_t) move > CAR_EVENT_DISTANCE) {
        CarEventMovingPosition(env);
    }

}