/*
 * energy.c
 *
 *  Created on: 2024年12月6日
 *      Author: yongchaoz
 */
#include "platform.h"
#include "hal_headfile.h"

extern const uint32_t energy_tab_16bit[80];
float RV32IMACFP_log2_float(float arg_in);
uint32_t sound_energy_real_time_count(uint8_t *sound_addr, uint32_t samplepoints, uint32_t bits, uint32_t samplerate)
{
    static uint8_t  left_index_bak = 0,left_now_index = 0;
    static uint8_t  right_index_bak = 0,right_now_index = 0;
    static uint32_t point_8k_num = 0;
    static uint64_t left_sum_energy = 0,right_sum_energy = 0;
    uint32_t count_index_step = (samplerate / 8000)*2;
    uint32_t index = 0;
    uint32_t left_data,right_data;
    index = 0;

    for (uint32_t i = 0; i < (samplepoints/2); i += count_index_step)//抽点，抽成8K采样率,每个声道单独计算
    {
        if (bits == 16)
        {
            left_data = *(((int16_t *)sound_addr) + i);
            right_data = *(((int16_t *)sound_addr) + i +1);
        }
        else
        {
            left_data = (*((int32_t *)sound_addr + i)) >> 16;
            right_data = (*((int32_t *)sound_addr + i +1)) >> 16;
        }

        left_sum_energy  += left_data * left_data;
        right_sum_energy += right_data * right_data;
        index ++;
    }

    point_8k_num += index;

    if (point_8k_num < 512)
    {
        return  (((uint32_t)left_index_bak<<8) + right_index_bak);
    }
    else
    {
        uint32_t left_temp = (uint32_t)(left_sum_energy / point_8k_num); //log10(x) = log2(x)/log2(10)
        uint32_t right_temp = (uint32_t)(right_sum_energy / point_8k_num); //log10(x) = log2(x)/log2(10)
        point_8k_num = 0;
        left_sum_energy = 0;
        right_sum_energy = 0;

        float left_log_energy = RV32IMACFP_log2_float((float)left_temp) / 3.322;
        float right_log_energy = RV32IMACFP_log2_float((float)right_temp) / 3.322;

        uint32_t left_data3 = (uint32_t)(left_log_energy * 1000000) +2;
        uint32_t right_data3 = (uint32_t)(right_log_energy * 1000000) +2;

        //rt_kprintf("reft_enery:%d,right_enery:%d  %d\r\n",left_data3,right_data3);

        for (uint8_t j = 0; j < 80; j++)
        {
            if (energy_tab_16bit[79 - j] < left_data3)
            {
                left_now_index = 79 - j;
                if((left_now_index + 2) <= left_index_bak)/*減小*/
                {
                  left_index_bak = left_now_index;
                }
                else if(left_now_index >= (left_index_bak + 2))
                {
                  left_index_bak = left_now_index;
                }
                break;
            }
        }

        for (uint8_t j = 0; j < 80; j++)
        {
            if (energy_tab_16bit[79 - j] < right_data3)
            {
                right_now_index = 79 - j;
                if((right_now_index + 2) <= right_index_bak)/*減小*/
                {
                  right_index_bak = right_now_index;
                }
                else if(right_now_index >= (right_index_bak + 2))
                {
                  right_index_bak = right_now_index;
                }
                break;
            }
        }
    }

    if(left_index_bak > 68)
    {
        left_index_bak -=1;
    }
    if(right_index_bak > 68)
    {
        right_index_bak -=1;
    }
    return  (((uint32_t)left_index_bak<<8) + right_index_bak);
}

const uint32_t energy_tab_16bit[80] ={
0,
1250000,
1500000,
1750000,
2000000,
2250000,
2500000,
2650000,
2700000,
2850000,
2912500,
3075000,
3137500,
3200000,
3362500,
3425000,
3587500,
3650000,
3712500,
3875000,
3937500,
4000000,
4062500,
4125000,
4187500,
4250000,
4312500,
4375000,
4437500,
4500000,
4562500,
4625000,
4687500,
4750000,
4812500,
4875000,
4937500,
5000000,
5062500,
5125000,
5177083,
5229166,
5281249,
5333332,
5385415,
5437498,
5489581,
5541664,
5593747,
5645830,
5697913,
5749996,
5802079,
5854162,
5906245,
5958328,
6010411,
6062494,
6114577,
6166660,
6218743,
6370826,
6422909,
6574992,
6627075,
6779158,
6831241,
6983324,
7035407,
7187490,
7239573,
7391656,
7543739,
7895822,
8047905,
8399988,
8552071,
8704154,
8908320,
8956237
};

/**
* @brief        根据当前音频的能量等级，计算出当前MIC增益应该加还是减或者保持不变
* @details      每100个数据计算一次,从100个数据里面抽取N个大于UP_LIMT_BASE点位，如果N大于阈值UP_LIMT_CNT,返回需要下调增益，如果不能满足，则判断最大的N个是否都小于DOWN_LIMT_BASE，如果N大于DOWN_LIMT_CNT，则上调增益

* @param[in]    sound_addr：
* @return       none
* @note
*/


/*************************************************/
// #define UP_LIMT_BASE     68//测试稳定版本
// #define UP_LIMT_CNT      4

// #define DOWN_LIMT_BASE   58
// #define DOWN_LIMT_CNT    8
/************************************************/

#define FRAMES_TOTAL_CNT  10

#define UP_LIMT_BASE     70
#define UP_LIMT_CNT      4

#define DOWN_LIMT_BASE   30
#define DOWN_LIMT_CNT    8

adc_gain_agc_typedef sound_agc_deal(uint16_t temp_data,uint32_t samplerate)
{
   static uint8_t points_cnt,up_points_cnt,down_points_cnt,keep_points_cnt;
   static uint8_t now_gain;
   uint32_t samplerate_up_limt_cnt,samplerate_down_limt_cnt,samplerate_total_cnt;
   uint8_t temp,temp1,temp2;
#if 0 //适用于双声道
   temp1 = (uint8_t)(temp_data>>8);
   temp2 = (uint8_t)temp_data;

   if(temp1 >= temp2)
   {
      temp = temp1;
   }
   else
   {
      temp = temp2;
   }
#endif

#if 1
   temp = temp_data;
#endif
   //rt_kprintf("agc_num_total:%d\r\n",temp);
   samplerate_up_limt_cnt = (samplerate/8000)*UP_LIMT_CNT;
   samplerate_down_limt_cnt = (samplerate/8000)*DOWN_LIMT_CNT;
   samplerate_total_cnt = (samplerate/8000)*FRAMES_TOTAL_CNT;

   now_gain = ADC_GAIN_KEEP;

   if(temp >= UP_LIMT_BASE)
   {
     up_points_cnt ++;
   }
   else if(temp <= DOWN_LIMT_BASE)
   {
     down_points_cnt ++;
   }
   else
   {
     keep_points_cnt++;
   }

   if(points_cnt < samplerate_total_cnt)
   {
     points_cnt ++;
     if(points_cnt >= samplerate_total_cnt)
     {
        points_cnt = 0;
        if(up_points_cnt > samplerate_up_limt_cnt)
        {
          now_gain = ADC_GAIN_DECREASE;
        }
        else if((down_points_cnt > samplerate_down_limt_cnt)&&(up_points_cnt == 0)&&(keep_points_cnt == 0))
        {
           now_gain = ADC_GAIN_INCREASE;
        }
        else
        {
          now_gain = ADC_GAIN_KEEP;
        }
        up_points_cnt = 0;
        down_points_cnt = 0;
        keep_points_cnt = 0;
     }
   }
   return now_gain;
}



