#include <math.h>
#include <stdlib.h>
#include "EraGlonassUveos.h"
#include "AsciiStringAssmeblingUtils.h"
#include "SystemDelayInterface.h"

#define LOGGER env->logger
#define LOG_SIGN "УВЭОС,DO_NOT_DISTAN"

uint64_t tim= 0;

void EraGlonassUveos_DoNotingTestingStarted(tEraGlonassUveos *env) {
    env->doNothing.mode = UVEOS_DO_NOTHING_TESTING;
    NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.beginPosition, true);
}

void EraGlonassUveos_GarageStarted(tEraGlonassUveos *env) {
    env->doNothing.mode = UVEOS_DO_NOTHING_GARAGE;
    NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.beginPosition, true);
}

void EraGlonassUveos_DoNotingReset(tEraGlonassUveos *env) {
    env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
    NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.beginPosition, true);
}

void EraGlonassUveos_DoNotingSetStartPos(tEraGlonassUveos *env) {
    env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
    NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.movePosition, true);
}

uint32_t getAbsOfPosition(EraGlonassUveosNavData *now, tEraGlonassUveos *env, bool flagSet){
    if(flagSet == true) {
        return abs(now->latitude - env->doNothing.beginPosition.latitude);
    } else {
        return abs(now->longitude - env->doNothing.beginPosition.longitude);
    }
}

void EraGlonassUveos_DoNothingModeDistance(tEraGlonassUveos *env, bool sourseNaData) {
    uint32_t latDistance = 0;
    uint32_t lonDistance = 0;
    size_t str_len = 0;
    char step[5] = {0};

    if (env->doNothing.stopLimitTime < SystemGetMs()) {
        NavDataProvider_GetNavData(env->navDataProvider, &env->doNothing.movePosition, sourseNaData);
        LoggerInfoStatic(LOGGER, LOG_SIGN, "Установлена новая точка отсчёта движения ТС");
        env->doNothing.stopLimitTime = SystemGetMs() + (40 * 1000);
    }

    EraGlonassUveosNavData movDinst;
    NavDataProvider_GetNavData(env->navDataProvider, &movDinst, sourseNaData);
    if (
            (env->doNothing.mode != UVEOS_DO_NOTHING_GARAGE) &&
            (env->doNothing.mode != UVEOS_DO_NOTHING_TESTING) &&
            (env->doNothing.movePosition.valid > 1)
            ) {

        latDistance = abs(movDinst.latitude - env->doNothing.movePosition.latitude);
        lonDistance = abs(movDinst.longitude - env->doNothing.movePosition.longitude);

        double x = latDistance * 0.1;
        double y = lonDistance * 0.1;
        double move = sqrt(x * x + y * y);
        env->doNothing.movDist = (uint32_t) move;

        if ( env->doNothing.movDist > 25) {
            env->doNothing.mode = UVEOS_DO_NOTHING_MOVE_TO;
        } else {
            env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
        }
    }

///===================

    EraGlonassUveosNavData now;
    NavDataProvider_GetNavData(env->navDataProvider, &now, sourseNaData);
    if(now.valid > 0) {
        if(env->doNothing.beginPosition.valid<1){
            env->doNothing.beginPosition = now;
        }

        latDistance = abs(now.latitude - env->doNothing.beginPosition.latitude);
        lonDistance = abs(now.longitude - env->doNothing.beginPosition.longitude);

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

        if((env->doNothing.mode == UVEOS_DO_NOTHING_GARAGE) && (tim < SystemGetMs())) {
            vAsciiStringInit(step, &str_len, 5);
            vAsciiStringAddDecimalInt64(step, &str_len, res, 5);
            LoggerInfoStatic(LOGGER, LOG_SIGN, "Текущее отклонение (режим ГАРАЖ) = ");
            LoggerInfo(LOGGER, LOG_SIGN, step, str_len);

            if (env->settings->GARAGE_MODE_END_DISTANCE < res) {
                env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
            }
            tim = SystemGetMs() + 10000;
        }

        if(env->doNothing.mode == UVEOS_DO_NOTHING_TESTING) {
            vAsciiStringInit(step, &str_len, 5);
            vAsciiStringAddDecimalInt64(step, &str_len, res, 5);
            LoggerInfoStatic(LOGGER, LOG_SIGN, "Текущее отклонение (режим тестирования) = ");
            LoggerInfo(LOGGER, LOG_SIGN, step, str_len);
            if (env->settings->TEST_MODE_END_DISTANCE < res) {
                env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
            }
        }

        if (
                (env->settings->GARAGE_MODE_END_DISTANCE < res) &&
                (env->doNothing.mode = UVEOS_DO_NOTHING_GARAGE)
                ) {
            env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
        }
        if (
                (env->settings->TEST_MODE_END_DISTANCE < res) &&
                (env->doNothing.mode = UVEOS_DO_NOTHING_TESTING)
                ) {
            env->doNothing.mode = UVEOS_DO_NOTHING_DISABLED;
        }

        env->offsetToDistance = res;
    }
}