//
// Created by ilya on 25.03.24.
//
#include <alloca.h>
#include <SystemDelayInterface.h>
#include "Accel.h"
#include "CmsisRtosThreadUtils.h"
#include "ext_telematica.h"
#include "at32f435_437_clock.h"

crm_clocks_freq_type crm_clocks_freq_struct = {0};
tAccel ACCEL;

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) {

    env->errorDataFlag = true;
    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;
}


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

            if (EXT_ENV_ADR_TELE.META_EXT_ENV_TELE)
                EXT_ENV_ADR_TELE.tele_func(&env->accel.xyz, TELE_MODULE_ACCEL_DATA);

            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);
//    }
}

void TMR1_Accel_TmrStart(tAccel *env){
    /* get system clock */
    crm_clocks_freq_get(&crm_clocks_freq_struct);

    /* enable tmr1 clock */
    crm_periph_clock_enable(CRM_TMR1_PERIPH_CLOCK, TRUE);

    /* tmr1 configuration */
    /* time base configuration */
    uint32_t tmr = (crm_clocks_freq_struct.apb2_freq * 2 / 10000) - 1;
    tmr_base_init(TMR1, 9998, tmr/1000);
    tmr_cnt_dir_set(TMR1, TMR_COUNT_UP);

    /* overflow interrupt enable */
    tmr_interrupt_enable(TMR1, TMR_OVF_INT, TRUE);

    /* tmr1 hall interrupt nvic init */
    nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);
    nvic_irq_enable(TMR1_OVF_TMR10_IRQn, 0x59, 0);
    /* enable tmr1 */
    tmr_counter_enable(TMR1, TRUE);
}

//uint16_t ms[1000];
//volatile  uint16_t i = 0;
//volatile  uint16_t o = 0;
//volatile uint32_t ttt = 0;
//volatile uint32_t aa = 0;
osStatus_t stat = osError;

void TMR1_OVF_TMR10_IRQHandler(void)
{
    if(tmr_interrupt_flag_get(TMR1, TMR_OVF_FLAG) == SET)
    {
        if(ACCEL.errorDataFlag == true) {
            if (AccelQma6100P_Read(&ACCEL.accel, &ACCEL.current)) {

                if (EXT_ENV_ADR_TELE.META_EXT_ENV_TELE)
                    EXT_ENV_ADR_TELE.tele_func(&ACCEL.accel.xyz, TELE_MODULE_ACCEL_DATA);

                stat = osMessageQueuePut(ACCEL.dataQueue, &ACCEL.current, 0, 0);
                if (stat != osOK) {
                    asm("nop");
                }
                ACCEL.errorDataFlag = true;

//                if(o == 1000){
//                    o=0;
//                }
//                aa = SystemGetMs();
//                if(ttt > aa){
//                    ++i;
//                } else {
//                    ms[o] = i;
//                    i=0;
//                    o++;
//                    ttt = SystemGetMs()+1000;
//                }

            } else {
                ACCEL.errorDataFlag = false;
            }
        }
        tmr_flag_clear(TMR1, TMR_OVF_FLAG);
    }
}