//
// Created by xemon on 22.11.22.
//
#include "EraGlonassAsi15.h"
#include "math.h"

#define ASI_15_DEFAULT_THRESHOLD 0.9

#define AX 12
#define AY 9
#define AZ 10
#define DELTA 50


static float fA_t(float fIntegralValue) {
    return fIntegralValue / DELTA;
}

static float fASI(float fAX_tValue, float fAY_tValue, float fAZ_tValue) {
    float _x = fA_t(fAX_tValue) / AX;
    float _y = fA_t(fAY_tValue) / AY;
    float _z = fA_t(fAZ_tValue) / AZ;
    return (float) sqrt((double) ((_x * _x) + (_y * _y) + (_z * _z)));
}

void EraGlonassAsi15_Init(tEraGlonassAsi15 *env, float *threshold) {
    *env = (tEraGlonassAsi15) {
            .asi = {0},
            .asi15 = {0},
            .counter = 0,
    };

    env->tLIS331HHFirstEventArray.count = 0;
    env->tLIS331HHSecondEventArray.count = 75;
    env->threshold = threshold;
}


float EraGlonassAsi15_GetCurrentValue(tEraGlonassAsi15 *env) {
    return env->asi15[env->counter];
}

void EraGlonassAsi15_ApplyCurrentAccel(tEraGlonassAsi15 *env, vector3 currentAcceleration) {
    env->tLIS331HHFirstEventArray.elements[env->tLIS331HHFirstEventArray.count] = currentAcceleration;
    env->tLIS331HHFirstEventArray.count++;
    if (env->tLIS331HHFirstEventArray.count > 149) {
        env->tLIS331HHFirstEventArray.count = 0;
    }
    env->tLIS331HHSecondEventArray.elements[env->tLIS331HHSecondEventArray.count] = currentAcceleration;
    env->tLIS331HHSecondEventArray.count++;
    if (env->tLIS331HHSecondEventArray.count > 149) {
        env->tLIS331HHSecondEventArray.count = 0;
    }

    if ((env->counter + 49) < 150) {
        env->aT[env->counter].x = (env->tLIS331HHFirstEventArray.elements[env->counter].x +
                                   env->tLIS331HHFirstEventArray.elements[env->counter + 49].x) / 2;
        env->aT[env->counter].y = (env->tLIS331HHFirstEventArray.elements[env->counter].y +
                                   env->tLIS331HHFirstEventArray.elements[env->counter + 49].y) / 2;
        env->aT[env->counter].z = (env->tLIS331HHFirstEventArray.elements[env->counter].z +
                                   env->tLIS331HHFirstEventArray.elements[env->counter + 49].z) / 2;
    } else {
        env->aT[env->counter].x = (env->tLIS331HHFirstEventArray.elements[env->counter].x +
                                   env->tLIS331HHSecondEventArray.elements[env->counter - 75 + 49].x) / 2;
        env->aT[env->counter].y = (env->tLIS331HHFirstEventArray.elements[env->counter].y +
                                   env->tLIS331HHSecondEventArray.elements[env->counter - 75 + 49].y) / 2;
        env->aT[env->counter].z = (env->tLIS331HHFirstEventArray.elements[env->counter].z +
                                   env->tLIS331HHSecondEventArray.elements[env->counter - 75 + 49].z) / 2;
    }

    for (int i = 1; i < 49; i++) {
        if ((env->counter + i) < 150) {
            env->aT[env->counter].x += env->tLIS331HHFirstEventArray.elements[env->counter + i].x;
            env->aT[env->counter].y += env->tLIS331HHFirstEventArray.elements[env->counter + i].y;
            env->aT[env->counter].z += env->tLIS331HHFirstEventArray.elements[env->counter + i].z;
        } else {
            env->aT[env->counter].x += env->tLIS331HHSecondEventArray.elements[env->counter - 75 + i].x;
            env->aT[env->counter].y += env->tLIS331HHSecondEventArray.elements[env->counter - 75 + i].y;
            env->aT[env->counter].z += env->tLIS331HHSecondEventArray.elements[env->counter - 75 + i].z;
        }
    }

    env->asi[env->counter] = fASI(env->aT[env->counter].x, env->aT[env->counter].y, env->aT[env->counter].z);

    if ((env->counter + 14) < 150) {
        env->asi15[env->counter] = (env->asi[env->counter] +
                                    env->asi[env->counter + 14]) / 2;
    } else {
        env->asi15[env->counter] = (env->asi[env->counter] +
                                    env->asi[env->counter + 14 - 150]) / 2;
    }
    for (int i = 1; i < 14; i++) {
        if ((env->counter + i) < 150) {
            env->asi15[env->counter] += env->asi[env->counter + i];
        } else {
            env->asi15[env->counter] += env->asi[i];
        }
    }
    env->asi15[env->counter] = env->asi15[env->counter] / 15;

    env->counter++;
    if (env->counter >= 150) {
        env->counter = 0;
    }

    if (env->asi15[env->counter] > *env->threshold) {
        env->impactDetected = true;
    }
}