#include <sched.h>
/*
 * asi15.c
 *
 *  Created on: Aug 25, 2020
 *      Author: FICOM-IT LTD
 */

#include "CarCrashDetection.h"
#include "SystemDelayInterface.h"

#define COS_30_DEG 0.86602540
#define FLIP_DEFAULT_THRESHOLD_ANGLE_COS COS_30_DEG

//Use only from CCD task cause input accel concurrency
void CarCrashDetection_AutoFlipThreshold(tCarCrashDetection *env) {
    int initialSteps = 10;
    int initialStepsLeft = initialSteps;

    vector3 initialGravity = {0, 0, 0};

    for (;;) {
        if (AccelDataFlow_GetNext(env->accelData, &env->currentAcceleration, SystemWaitForever)) {
            initialGravity = vector3Add(
                    initialGravity,
                    env->currentAcceleration
            );
            --initialStepsLeft;
            if (initialStepsLeft <= 0)break;
        }
    }

    initialGravity = vector3DivNum(initialGravity, (float) initialSteps);

//    CarFlipDetection_SetThreshold(&env->flip, initialGravity, FLIP_DEFAULT_THRESHOLD_ANGLE_COS);
}


void CarCrashDetection_Init(
        tCarCrashDetection *env,
        tAccelDataFlowInterface *accelData,
        float *asi15threshold,
        tCarFlipDetectionSettings *settings
) {
    env->outEvents = osMessageQueueNew(128, sizeof(tUveosEmergencyEvent), NULL);
    env->accelData = accelData;
    EraGlonassAsi15_Init(&env->asi15, asi15threshold);
    CarFlipDetection_Init(&env->flip, settings);
    env->crashEventGenerated = false;
    env->isStarted = false;
}

void CarCrashDetection_Processing(tCarCrashDetection *env, uint32_t timeout) {
    if(env->isStarted == false){
        env->isStarted = true;
    }
    if (AccelDataFlow_GetNext(env->accelData, &env->currentAcceleration, timeout)) {
        EraGlonassAsi15_ApplyCurrentAccel(&env->asi15, env->currentAcceleration);
        CarFlipDetection_ApplyCurrentAccel(&env->flip, env->currentAcceleration);
        env->asi15This = env->asi15.asi15[env->asi15.counter];

        if (!env->crashEventGenerated && (env->flip.flipDetected || env->asi15.impactDetected)) {
            tUveosEmergencyEvent crash = {
                    .impactValue = EraGlonassAsi15_GetCurrentValue(&env->asi15),
                    .flipAngleCos = env->flip.current.angleCos,
                    .isFlip = env->flip.flipDetected,
                    .isImpact = env->asi15.impactDetected,
            };

            if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) {
                env->crashEventGenerated = true;
            }
        }
    }
}

bool CarCrashDetection_GetNext(tCarCrashDetection *env, tUveosEmergencyEvent *event, uint32_t timeout) {
    return osMessageQueueGet(env->outEvents, event, 0, timeout) == osOK;
}


tUveosEmergencyEventInterface CarCrashDetection_GetInterface(tCarCrashDetection *env) {
    tUveosEmergencyEventInterface result = {
            .env = env,
            .getNext = (UveosEmergencyGetNextCall) CarCrashDetection_GetNext,
    };
    return result;
}

static void CarCrashDetection_SimulateImpact(tCarCrashDetection *env, float asi15) {
    tUveosEmergencyEvent crash = {
            .impactValue = asi15,
            .flipAngleCos = 0,
            .isFlip = false,
            .isImpact = true,
    };

    if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) {

    }
}

static void CarCrashDetection_SimulateFlip(tCarCrashDetection *env, float angle) {
    tUveosEmergencyEvent crash = {
            .impactValue = 0,
            .flipAngleCos = angle,
            .isFlip = true,
            .isImpact = false,
    };

    if (osMessageQueuePut(env->outEvents, &crash, 0, 1) == osOK) {
    }
}

tUveosEmergencySimulationInterface CarCrashDetection_GetSimInterface(tCarCrashDetection *env) {
    tUveosEmergencySimulationInterface result = {
            .env = env,
            .simulateImpact = (UveosEmergencySimulate) CarCrashDetection_SimulateImpact,
            .simulateFlip= (UveosEmergencySimulate) CarCrashDetection_SimulateFlip,
    };
    return result;
}