/*----------------------------------------------------------------------------
 * Copyright (c) TJD Technologies Co., Ltd. 2020. All rights reserved.
 *
 * Description: service_hrsensor.c 
 *
 * Author: liangjianfei
 *
 * Create: 2024-4-26
 *--------------------------------------------------------------------------*/

#include "securec.h"
#include "sys_config.h"
#include "common_def.h"
#include "soc_osal.h"
#include "cmsis_os2.h"
#include "sys_typedef.h"
#include "task_service_timer.h"
#include "msensor_api.h"
#include "service_msensor.h"
#include "service_gsensor.h"
#include <stdio.h>

#define ENABLE_PRINT_INFO 1
#define ENABLE_DEBUG      1

#if ENABLE_PRINT_INFO
#define static_print_info(...)      sys_ms_log_i(__VA_ARGS__)	//一般信息打印宏控制
#define static_print_warn(...)      sys_ms_log_w(__VA_ARGS__)	//警告信息打印一般常开
#define static_print_error(...)     sys_ms_log_e(__VA_ARGS__)	//错误信息打印一般常开
#if ENABLE_DEBUG
#define static_print_debug(...)     sys_ms_log_d(__VA_ARGS__)
#else
#define static_print_debug(...)
#endif
#else
#define static_print_info(...)
#define static_print_warn(...)
#define static_print_error(...)
#endif

static msensor_info *g_ms_driver = NULL;
static uint8_t g_calibration_status = 0;
static float gsen_data[3] = {0};

orientation_info_t ori_data = {0};  // 方位角

static signed int tjd_service_msensor_handle(void *param)
{
    if (tjd_service_gs_get_fifo_count(GSEN_SERVICE_MSEN) != 0) {
        gsensor_xyz_info gsen_synt_data;
        tjd_service_gs_get_xyz_fifo(GSEN_SERVICE_MSEN, &gsen_synt_data, 1);
        gsen_data[0] = -(gsen_synt_data.gsensor_x.raw_data*0.244)/1000;
        gsen_data[1] = (gsen_synt_data.gsensor_y.raw_data*0.244)/1000;
        gsen_data[2] = (gsen_synt_data.gsensor_z.raw_data*0.244)/1000;

        // static_print_debug("gsen_data: %4.2f %4.2f %4.2f", gsen_data[0], gsen_data[1], gsen_data[2]);
    }

    if(g_calibration_status < 3){
        g_calibration_status = g_ms_driver->timer_callback(gsen_data, &ori_data);
        return 20;     //定时执行间隔单位为ms
    }
    else{
         g_calibration_status = g_ms_driver->timer_callback(gsen_data, &ori_data);
         return 80;     //定时执行间隔单位为ms
    }
}

static int32_t tjd_service_msensor_start_send(void)
{
    if (!g_ms_driver) {
        static_print_error("service_msensor has not been open!");
        return -1;
    }

    g_ms_driver->resume();
    if(g_calibration_status < 3) g_ms_driver->alg_init();
    queue_default_info_t msg_data = { tjd_service_msensor_handle, NULL, 20, NULL};
    int32_t ret = osal_msg_queue_write_copy(tjd_task_service_timer_get_queue_id(), (void *)&msg_data, sizeof(queue_default_info_t), 0);
    if (ret != OSAL_SUCCESS) {
        static_print_error("osal_msg_queue_write_copy fail! ret:%d", ret);
        return ret;
    }

    return 0;
}

static void tjd_service_msensor_end_send(void)
{
    if (!g_ms_driver) {
        static_print_error("service_msensor has not been open!");
        return;
    }

    g_ms_driver->suspend();
    queue_default_info_t msg_data = { tjd_service_msensor_handle, NULL, 0, NULL};
    osal_msg_queue_write_copy(tjd_task_service_timer_get_queue_id(), (void *)&msg_data, sizeof(queue_default_info_t), 0);
}

uint8_t tjd_service_msensor_open(void)
{
    g_ms_driver = tjd_driver_ms_api_get_ops();

    if (tjd_service_msensor_start_send() != 0) {
        return -1;
    }

    static_print_info("tjd_service_msensor_open succ!");
    return 0;
}

void tjd_service_msensor_close(void)
{
    tjd_service_msensor_end_send();
    g_ms_driver = NULL;
}

float tjd_service_msensor_get_azimuth_data(void)
{
    if (g_ms_driver == NULL) {
        static_print_error("tjd_service_msensor is not open");
        return 0;
    }
    return ori_data.Azimuth;
}

float tjd_service_msensor_get_pitch_data(void)
{
    if (g_ms_driver == NULL) {
        static_print_error("tjd_service_msensor is not open");
        return 0;
    }
    return ori_data.Pitch;
}

float tjd_service_msensor_get_roll_data(void)
{
    if (g_ms_driver == NULL) {
        static_print_error("tjd_service_msensor is not open");
        return 0;
    }
    return ori_data.Roll;
}

int tjd_service_msensor_get_accuracy(void) {return g_ms_driver->get_accuracy();}