#include "platform.h"
#include "hal_headfile.h"
#include "system.h"
#include "debug.h"
#include "resample.h"
#include "heap_5.h"

void init_resample(resample_info* res_info, wav_info info);
uint16_t resample16(resample_info* res_info, short* in_buffer, unsigned long buffer_length, short* out_buffer);

sound_infor_def_type  *sound_infor;
soundplay_def_type *soundplay;
wav_pcm_header44* wav_header;

short* resample_input_buff;
short* resample_output_buff;
wav_info*  g_wav_in_info;
resample_info* g_res_info;

static const int kNumPhaseBits = 29;
static const int kNumInterpBits = 15;

static unsigned int kPhaseMask;
static double kPhaseMultiplier;
static int kPreInterpShift;


void sound_init(void)
{
	sound_infor = (sound_infor_def_type  *)pvPortMalloc(sizeof(sound_infor_def_type));
	soundplay = (soundplay_def_type  *)pvPortMalloc(sizeof(soundplay_def_type));
}

int8_t sound_action(uint16_t id,uint32_t SampleRate,uint32_t BitsPerSample,uint32_t Channels,uint32_t frame_byte_length,uint8_t option)
{
  uint32_t playback_addr_base;
  sound_infor_def_type temp_infor;
  if(option == ENABLE)
  {
	  playback_addr_base = (uint32_t)(FW_HEADER->BIN[2].ADDR + 0x4000000);
	  temp_infor = *(sound_infor_def_type *)(playback_addr_base + (id * 4 *4));

	  sound_debug("sound_infor->id:%d\r\n",temp_infor.id);
	  sound_debug("sound_infor->type:%d\r\n",temp_infor.type);
	  sound_debug("sound_infor->addr:%x\r\n",temp_infor.addr + playback_addr_base);
	  sound_debug("sound_infor->length:%x\r\n",temp_infor.length);

	  if(sound_infor->length != 0)
	  {
		   vPortFree(g_wav_in_info);
		   vPortFree(resample_input_buff);
		   vPortFree(resample_output_buff);//输出要pingpong
		   vPortFree(g_res_info);
	  }

      if(temp_infor.type == WAV)
      {
    	  wav_header = (wav_pcm_header44*)(temp_infor.addr + playback_addr_base);
    	  sound_debug("wav_header->SampleRate:%d\r\n",wav_header->SampleRate);
    	  sound_debug("wav_header->BitsPerSample:%d\r\n",wav_header->BitsPerSample);
    	  sound_debug("wav_header->NumChannels:%d\r\n",wav_header->NumChannels);
    	  if(wav_header->BitsPerSample != 16)
    	  {
    		  sound_debug("wav file  BitsPerSample should be 16\r\n");
    		  return -1;
    	  }
    	  if(wav_header->NumChannels != 1)
    	  {
    		  sound_debug("wav_header->NumChannels:%d\r\n",wav_header->NumChannels);
    		  return -1;
    	  }
    	  soundplay->SampleRate = SampleRate;
    	  soundplay->BitsPerSample = BitsPerSample;
    	  soundplay->Channels = Channels;
    	  soundplay->frame_byte_length = frame_byte_length;
    	  *sound_infor = temp_infor;
    	  sound_infor->addr = temp_infor.addr + playback_addr_base;
    	  sound_debug("sound_infor->addr:%x\r\n",sound_infor->addr);
    	  sound_infor->addr += 44;

    	  g_wav_in_info = (wav_info  *)pvPortMalloc(sizeof(wav_info));
    	  if(g_wav_in_info == NULL)
    	  {
    		  sound_debug("pvPortMalloc for g_wav_in_info err\r\n");
    		  return -1;
    	  }
    	  g_wav_in_info->BitsPerSample = wav_header->BitsPerSample;
    	  g_wav_in_info->in_SampleRate = wav_header->SampleRate;
    	  g_wav_in_info->NumChannels = wav_header->NumChannels;

    	  g_wav_in_info->out_SampleRate = soundplay->SampleRate;

    	  g_wav_in_info->buffer_output_size = soundplay->frame_byte_length;
    	  g_wav_in_info->buffer_input_size = (uint32_t)((float)g_wav_in_info->buffer_output_size/(((float)(((float)g_wav_in_info->out_SampleRate) / (float)(g_wav_in_info->in_SampleRate)))));


    	  resample_input_buff = (short*)pvPortMalloc(g_wav_in_info->buffer_input_size);
    	  resample_output_buff = (short*)pvPortMalloc(g_wav_in_info->buffer_output_size);//输出要pingpong
    	  g_res_info = (resample_info*)pvPortMalloc(sizeof(resample_info));
    	  init_resample(g_res_info, *g_wav_in_info);
      }
  }
  else
  {
	  ;
  }
  return 0;
}

int8_t sound_play_handle(void)
{
  short*outbuff = NULL;
  uint32_t out_length = 0;
  if(sound_infor == NULL)
  {
	 return -1;
  }
  if(sound_infor->length == 0)
  {
	  return -1;
  }
  else if(sound_infor->length > g_wav_in_info->buffer_input_size)//取一帧
  {
	  brom_memcpy((uint8_t*)resample_input_buff,(uint8_t*)sound_infor->addr,g_wav_in_info->buffer_input_size);
	  (void)resample16(g_res_info, (short*)resample_input_buff, g_wav_in_info->buffer_input_size, (short*)resample_output_buff);
	  sound_infor->addr += g_wav_in_info->buffer_input_size;
	  sound_infor->length -= g_wav_in_info->buffer_input_size;
  }
  else if(sound_infor->length >0)//取最后一帧
  {
	  brom_memcpy((uint8_t*)resample_input_buff,(uint8_t*)sound_infor->addr,sound_infor->length);
	  brom_memset((uint8_t*)resample_input_buff + sound_infor->length,0,g_wav_in_info->buffer_input_size - sound_infor->length);
	  (void)resample16(g_res_info, (short*)resample_input_buff, g_wav_in_info->buffer_input_size, (short*)resample_output_buff);
	  sound_infor->length = 0;
  }
  outbuff = (short*) resample_output_buff;
  out_length = g_res_info->wav_in_info.buffer_output_size;
  sound_callback(outbuff,out_length);
  if(sound_infor->length ==0)
  {
	   vPortFree(g_wav_in_info);
	   vPortFree(resample_input_buff);
	   vPortFree(resample_output_buff);//输出要pingpong
	   vPortFree(g_res_info);
#ifdef  SOUND_ENABLE
	   sound_stop_callbak();
#endif
  }
  return 0;
}

short Interp(int x0, int x1, unsigned int f)
{
	int a = (x0 + (((x1 - x0) * (f >> kPreInterpShift)) >> kNumInterpBits));
	return (short)a;
}

void Advance(unsigned int* index, unsigned int* frac, unsigned int inc)
{
	*frac += inc;
	*index += (unsigned int)(*frac >> kNumPhaseBits);
	*frac &= kPhaseMask;
}

void init_resample(resample_info* res_info, wav_info info)
{
	res_info->wav_in_info = info;
	//res_info->wav_in_info.out_buffer = (short*)malloc(res_info->wav_in_info.buffer_output_size);  //已更改为固定长度
	//memset(res_info->wav_in_info.out_buffer_PING,0,res_info->wav_in_info.buffer_output_size);
	//memset(res_info->wav_in_info.out_buffer_PONG,0,res_info->wav_in_info.buffer_output_size);

	kPhaseMask = (1LU << kNumPhaseBits) - 1;
	kPhaseMultiplier = 1L << kNumPhaseBits;
	kPreInterpShift = kNumPhaseBits - kNumInterpBits;

	res_info->mPhaseIncrement = (unsigned int)(kPhaseMultiplier * info.buffer_input_size / info.buffer_output_size);
	res_info->mX0L = 0;
	res_info->mX0R = 0;
	res_info->mInputIndex = 0;
	res_info->out_frame_count = res_info->wav_in_info.buffer_output_size / 2;
}

uint16_t  resampleMono16(resample_info* info, short* in_buffer, unsigned long buffer_length, short* out_buffer)
{
	short* read_p16;
	short* out_p16;
	unsigned int outputIndex = 0;
	unsigned int inputIndex = info->mInputIndex;
	unsigned int input_frame_count = buffer_length / (info->wav_in_info.BitsPerSample / 8) / info->wav_in_info.NumChannels;
	unsigned int phaseIncrement = info->mPhaseIncrement;
	unsigned int phaseFraction = info->mPhaseFraction;

	read_p16 = in_buffer;
	out_p16 = out_buffer;

	while (outputIndex < info->out_frame_count && inputIndex < input_frame_count)
	{
		while (inputIndex == 0)
		{
			out_p16[outputIndex++] = Interp(info->mX0L, read_p16[0], phaseFraction);
			Advance(&inputIndex, &phaseFraction, phaseIncrement);
		}
		while (outputIndex < info->out_frame_count && inputIndex < input_frame_count)
		{
			out_p16[outputIndex++] = Interp(read_p16[inputIndex - 1], read_p16[inputIndex], phaseFraction);
			Advance(&inputIndex, &phaseFraction, phaseIncrement);
		}
		if (inputIndex >= input_frame_count || outputIndex >= info->out_frame_count)
		{
			info->wav_in_info.out_frame_indeed = outputIndex / info->wav_in_info.NumChannels;
			info->mX0L = read_p16[input_frame_count - 1];
			info->mPhaseFraction = 0;//phaseFraction;
			info->mInputIndex = 0;//inputIndex - input_frame_count;
		}
	}
	return outputIndex;
}

uint16_t resample16(resample_info* res_info, short* in_buffer, unsigned long buffer_length, short* out_buffer)
{
	return resampleMono16(res_info, in_buffer, buffer_length, out_buffer);
}
