#include "correction.h"
#include <string.h>
#include "stddef.h"

static float input_vector[3];
static float output_vector[3];

/**
  * @brief 初始化校正参数为默认值
  * @param params: 参数结构体指针
  * @retval None
  */
void Init_CorrectionParams(CorrectionParams_t *params)
{
    if (params == NULL) return;
    
    // 初始化偏移为0
    params->offset_x = 0.0f;
    params->offset_y = 0.0f;
    params->offset_z = 0.0f;
    
    // 初始化为单位矩阵
    memset(params->correction_matrix, 0, sizeof(params->correction_matrix));
    params->correction_matrix[0] = 5.0f / 0x7FFFFFFF; // [0,0]
    params->correction_matrix[4] = 5.0f / 0x7FFFFFFF; // [1,1]
    params->correction_matrix[8] = 5.0f / 0x7FFFFFFF; // [2,2]
    
    #if USE_ARM_DSP
    // 初始化ARM DSP矩阵实例 (3x3矩阵)
    arm_mat_init_f32(&params->matrix_inst, 3, 3, params->correction_matrix);
    #endif
    
    params->params_valid = 1;
}

/**
  * @brief 设置校正矩阵
  * @param params: 参数结构体指针
  * @param matrix: 3x3校正矩阵(行优先存储)
  * @retval None
  */
void Set_CorrectionMatrix(CorrectionParams_t *params, const float *matrix)
{
    if (params == NULL || matrix == NULL) return;
    
    memcpy(params->correction_matrix, matrix, sizeof(params->correction_matrix));
    
    #if USE_ARM_DSP
    // 重新初始化ARM DSP矩阵实例
    arm_mat_init_f32(&params->matrix_inst, 3, 3, params->correction_matrix);
    #endif
}

/**
  * @brief 设置校正偏移
  * @param params: 参数结构体指针
  * @param offset_x, offset_y, offset_z: 偏移值
  * @retval None
  */
void Set_CorrectionOffsets(CorrectionParams_t *params, float offset_x, float offset_y, float offset_z)
{
    if (params == NULL) return;
    
    params->offset_x = offset_x;
    params->offset_y = offset_y;
    params->offset_z = offset_z;
}

/**
  * @brief 应用三分量校正 - 使用ARM DSP库优化
  * @param raw_x, raw_y, raw_z: 原始 ADC 数据
  * @param result: 校正结果结构体指针
  * @param params: 校正参数
  * @retval HAL_StatusTypeDef
  */
HAL_StatusTypeDef Apply_Correction(int32_t raw_x, int32_t raw_y, int32_t raw_z,
                                   CorrectionResult_t *result,
                                   const CorrectionParams_t *params)
{
    if (result == NULL || params == NULL || !params->params_valid) {
        return HAL_ERROR;
    }
    
    // 1. 将原始数据转换为浮点数并归一化
    input_vector[0] = (float)raw_x;
    input_vector[1] = (float)raw_y;
    input_vector[2] = (float)raw_z;
    
    #if USE_ARM_DSP
    // 2. 使用ARM DSP库进行矩阵乘法运算
    arm_matrix_instance_f32 input_mat, output_mat;
    arm_mat_init_f32(&input_mat, 3, 1, input_vector);
    arm_mat_init_f32(&output_mat, 3, 1, output_vector);
    
    arm_status status = arm_mat_mult_f32(&params->matrix_inst, &input_mat, &output_mat);
    if (status != ARM_MATH_SUCCESS) {
        return HAL_ERROR;
    }
    
    // 3. 应用偏移校正
    result->corrected_x = output_vector[0] - params->offset_x;
    result->corrected_y = output_vector[1] - params->offset_y;
    result->corrected_z = output_vector[2] - params->offset_z;
    
    #else
    // 传统矩阵乘法实现(当ARM DSP库不可用时)
    const float *matrix = params->correction_matrix;
    
    output_vector[0] = matrix[0] * input_vector[0] + matrix[1] * input_vector[1] + matrix[2] * input_vector[2];
    output_vector[1] = matrix[3] * input_vector[0] + matrix[4] * input_vector[1] + matrix[5] * input_vector[2];
    output_vector[2] = matrix[6] * input_vector[0] + matrix[7] * input_vector[1] + matrix[8] * input_vector[2];
    
    // 应用偏移校正
    result->corrected_x = output_vector[0] - params->offset_x;
    result->corrected_y = output_vector[1] - params->offset_y;
    result->corrected_z = output_vector[2] - params->offset_z;
    #endif
    
    // 4. 设置时间戳和状态
    result->timestamp = HAL_GetTick();
    result->correction_applied = 1;
    
    return HAL_OK;
}

/**
  * @brief 从Flash加载校正参数
  * @param params: 参数结构体指针
  * @retval None
  */
void Load_CorrectionParams_FromFlash(CorrectionParams_t *params)
{
    if (params == NULL) return;
    
    // TODO: 实现从Flash读取校正参数
    // 这里可以使用STM32的内部Flash或外部Flash存储
    // 暂时使用默认参数
    Init_CorrectionParams(params);
}

/**
  * @brief 保存校正参数到Flash
  * @param params: 参数结构体指针
  * @retval HAL_StatusTypeDef
  */
HAL_StatusTypeDef Save_CorrectionParams_ToFlash(const CorrectionParams_t *params)
{
    if (params == NULL || !params->params_valid) {
        return HAL_ERROR;
    }

    // TODO: 实现保存校正参数到Flash
    // 这里可以使用STM32的内部Flash或外部Flash存储

    return HAL_OK;
}