#include <alloca.h>
//
// Created by xemon on 23.11.22.
//

#include <SystemDelayInterface.h>
#include "Accel.h"
#include "CmsisRtosThreadUtils.h"

bool Accel_GetNext(tAccel *env, vector3 *value, uint32_t timeout) {
    return osMessageQueueGet(env->dataQueue, value, 0, timeout) == osOK;
}

void Accel_Clear(tAccel *env) {
    osMessageQueueReset(env->dataQueue);
}

void Accel_Init(tAccel *env, tSpiPortIO *accelIO) {

    AccelQma6100P_Init(&env->accel, accelIO);
    InitThreadAtrStatic(&env->thread.attr, "Accel", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
    env->thread.id = 0;
    env->dataQueue = osMessageQueueNew(128, sizeof(vector3), NULL);

    env->accelDataFlow = (tAccelDataFlowInterface) {
            .env = env,
            .getNext = (accelDataGetNextCall) Accel_GetNext,
            .clear = (accelDataClearCall) Accel_Clear,
    };
}


void Accel_StartDevice(tAccel *env) {
    AccelQma6100P_ConfigureDefaults(&env->accel);
}

bool Accel_WaitReady(tAccel *env, uint32_t timeout) {

    uint32_t time_end = SystemGetMs() + timeout;

    float len;
    do {
        len = vector3Len(env->current);
    } while ((time_end > SystemGetMs()) && len < 0.1f);

    return len > 0.1f;
}

void DataAccel_Vector(tAccelCoor *xyz);

static _Noreturn void Accel_Thread(tAccel *env) {
    //todo нужно переделать под таймер и семафор
    for (;;) {
        if (AccelQma6100P_Read(&env->accel, &env->current)) {

            DataAccel_Vector(&env->accel.xyz);

            osMessageQueuePut(env->dataQueue, &env->current, 0, 0);
            env->errorDataFlag = true;
        } else {
            env->errorDataFlag = false;
        }
        SystemDelayMs(1);
    }
}

void Accel_StartThread(tAccel *env) {
    if (!env->thread.id) {
        env->thread.id = osThreadNew((osThreadFunc_t) (Accel_Thread), (void *) (env), &env->thread.attr);
    }
}