//
// Created by xemon on 22.11.22.
//
#include <SystemDelayInterface.h>
#include "CarFlipDetection.h"

#define NUM_0_8_SQR 0.64
#define NUM_1_2_SQR 1.44
#define FLIP_ANTI_FLAY_DELAY_TIME_MS 3000
#define FLIP_MINIMAL_ACCEL_SQR_LENGTH NUM_0_8_SQR
#define FLIP_MAXIMAL_ACCEL_SQR_LENGTH NUM_1_2_SQR


void CarFlipDetection_InitSettings(tCarFlipDetectionSettings *settings) {
    settings->enabled = false;
    settings->gravity = (vector3) {0.f, 0.f, 0.f};
    settings->thresholds.antiFlightMs = 3000;
    settings->thresholds.angleCos = -1.f;

}

void CarFlipDetection_Init(tCarFlipDetection *env, tCarFlipDetectionSettings *settings) {
    env->current.durationMs = 0;
    env->flipDetected = false;
    env->settings = settings;
}

bool CarFlipDetection_IsAccelLenSqrInBounds(float accelSqr) {
    return (accelSqr > FLIP_MINIMAL_ACCEL_SQR_LENGTH) && (accelSqr < FLIP_MAXIMAL_ACCEL_SQR_LENGTH);
}

void CarFlipDetection_ApplyCurrentAccel(tCarFlipDetection *env, vector3 currentValue) {
    env->current.value = currentValue;

    if (CarFlipDetection_IsAccelLenSqrInBounds(vector3LenSquare(currentValue))) {

        env->current.angleCos = vector3AngleCos(env->settings->gravity, currentValue);

//		LOGF("ANGLE COS %i \n",(int)(angleCos*100))

        if (!env->settings->enabled) {
            env->flipDetected = false;
            return;
        }

        if (env->current.angleCos < env->settings->thresholds.angleCos) {
            if (env->current.durationMs == 0) {
                env->current.durationMs = SystemGetMs() + env->settings->thresholds.antiFlightMs;
            } else if (env->current.durationMs < SystemGetMs()) {
                env->flipDetected = true;
            }
        } else {
            env->current.durationMs = 0;
            env->flipDetected = 0;
        }
    }
}