#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <nds.h>


#include "../../ipc6.h"
#include "../../share/ipctool.h"
#include "_console.h"

#ifdef __cplusplus
extern "C" {
#endif
extern void VolNoAGC(s16 *lbuf,s16 *rbuf,u32 count,s32 vol);
extern void fix_PCM2_0(s16 *plsrc,s16 *prsrc,s16 *plbuf,s16 *prbuf,s32 *l,s32 *r,u32 LastSamples,u32 mask);
extern void fix_PCM2_1(s16 *plsrc,s16 *prsrc,s16 *plbuf,s16 *prbuf,s32 *l,s32 *r,u32 LastSamples,u32 mask);
extern void fix_PCM4_0(s16 *plsrc,s16 *prsrc,s16 *plbuf,s16 *prbuf,s32 *l,s32 *r,u32 LastSamples,u32 mask);
extern void fix_PCM4_1(s16 *plsrc,s16 *prsrc,s16 *plbuf,s16 *prbuf,s32 *l,s32 *r,u32 LastSamples,u32 mask);
#ifdef __cplusplus
}
#endif

extern void MemCopy16DMA3(void *src,void *dst,u32 len);
extern void MemSet16DMA3(u16 v,void *dst,u32 len);
extern u8 PM_ReadRegister( u8 registerIndex );
extern void PM_WriteRegister( u8 registerIndex, u8 controlCode );

//extern void VcountHandler(void) ;

static u32 strpcmCursorFlag=0;

static u32 strpcmFreq,strpcmSamples,strpcmChannels;
static EstrpcmFormat strpcmFormat=(EstrpcmFormat)0;

static s16 *strpcmL0=NULL,*strpcmL1=NULL,*strpcmR0=NULL,*strpcmR1=NULL;
static s32 strpcmLastL,strpcmLastR;
static int strpcmdatalen=0;
static bool strpcmPlayFlag=false;
static bool strpcmPlayFlagempty=false;
u32 strpcmVolume64;
//static s32 agc;

static s32 InterruptTest=0;


#undef SOUND_FREQ
#define SOUND_FREQ(n)	(0x10000 - (16777216 / (n)))

extern u8* strpcmL[4];
extern u8* strpcmR[4];

static inline void strpcmPlay()
{
    char *strpcmFormatstr;
  
  IPC6->IR=IR_NULL;

  strpcmCursorFlag=0;
  strpcmFormat=IPC6->strpcmFormat;
  strpcmFreq=IPC6->strpcmFreq;
  if (strpcmFreq == 0)
  {
      strpcmFreq= 32678;
  }
  strpcmSamples=IPC6->strpcmSamples;
  if(strpcmSamples == 0)
 {
      strpcmSamples = 4096;
 }

  strpcmChannels=IPC6->strpcmChannels;
  strpcmdatalen = strpcmSamples * 2;
  
  switch(strpcmFormat){
    case SPF_PCMx1: {
#ifdef ENABLE_DEBUG  
        strpcmFormatstr="SPF_PCMx1";
#endif
    } break;
    case SPF_PCMx2: {
      strpcmFreq*=2;
      strpcmSamples*=2;
#ifdef ENABLE_DEBUG  
        strpcmFormatstr="SPF_PCMx2";
#endif
    } break;
    case SPF_PCMx4: {
      strpcmFreq*=4;
      strpcmSamples*=4;
#ifdef ENABLE_DEBUG  
        strpcmFormatstr="SPF_PCMx4";
#endif
    } break;
  }
#ifdef ENABLE_DEBUG  
  _consolePrintf("strpcm freq=%d samples=%d chs=%d\n",strpcmFreq,strpcmSamples,strpcmChannels);
  _consolePrintf("%s\n", strpcmFormatstr); 
#endif
  //strpcmL0=(s16*)strpcmL[0];//(s16*)malloc(strpcmSamples*2);
  //strpcmL1=(s16*)strpcmL[1];//(s16*)malloc(strpcmSamples*2);
  //strpcmR0=(s16*)strpcmR[0];//(s16*)malloc(strpcmSamples*2);
  //strpcmR1=(s16*)strpcmR[1];//(s16*)malloc(strpcmSamples*2);
  MemSet16DMA3(0,strpcmL0,strpcmSamples*2);
  MemSet16DMA3(0,strpcmL1,strpcmSamples*2);
  MemSet16DMA3(0,strpcmR0,strpcmSamples*2);
  MemSet16DMA3(0,strpcmR1,strpcmSamples*2);
#ifdef ENABLE_DEBUG   
  _consolePrintf("strpcm buf 0x%x,0x%x 0x%x,0x%x\n",strpcmL0,strpcmL1,strpcmR0,strpcmR1);
#endif
  strpcmLastL=0;
  strpcmLastR=0;

  TIMER0_CR = 0;
  TIMER1_CR = 0;
 
  SCHANNEL_CR(0) = 0;
  SCHANNEL_CR(1) = 0;
  SCHANNEL_CR(2) = 0;
  SCHANNEL_CR(3) = 0;
  
  TIMER0_DATA = SOUND_FREQ(strpcmFreq);
  TIMER0_CR = TIMER_DIV_1 | TIMER_ENABLE;
  
  TIMER1_DATA = 0x10000 - (strpcmSamples*2);
  TIMER1_CR = TIMER_CASCADE | TIMER_IRQ_REQ | TIMER_ENABLE;
  
  for(u32 ch=0;ch<4;ch++){
    SCHANNEL_CR(ch) = 0;
    SCHANNEL_TIMER(ch) = SOUND_FREQ(strpcmFreq);
    SCHANNEL_SOURCE(ch) = 0;
    SCHANNEL_LENGTH(ch) = (strpcmSamples*2) >> 2;
    SCHANNEL_REPEAT_POINT(ch) = 0;
  }
  //agc=0;
  /*
    for (int channel=0;channel<4 ;channel+=2 )
    {
            SCHANNEL_CR(channel)     = 0;
            SCHANNEL_TIMER(channel)  = SOUND_FREQ(strpcmFreq);
            SCHANNEL_SOURCE(channel) = 0;
            SCHANNEL_LENGTH(channel) = (strpcmSamples*2) >> 2 ;
            SCHANNEL_REPEAT_POINT(channel) = 0;

            SCHANNEL_CR(channel+1)     = 0;
            SCHANNEL_TIMER(channel+1)  = SOUND_FREQ(strpcmFreq);
            SCHANNEL_SOURCE(channel+1) = 0;
            SCHANNEL_LENGTH(channel+1) = (strpcmSamples*2) >> 2 ;
            SCHANNEL_REPEAT_POINT(channel+1) = 0;

    }
  //IPC6->strpcmWriteRequest=0;
  */
#ifdef ENABLE_DEBUG    
  _consolePrintf("strpcm initialized.\n");
#endif
}

__attribute__((noinline)) static void strpcmStop()
{
  TIMER0_CR = 0;
  TIMER1_CR = 0;
  
  SCHANNEL_CR(0) = 0;
  SCHANNEL_CR(1) = 0;
  SCHANNEL_CR(2) = 0;
  SCHANNEL_CR(3) = 0;
  

  //MemSet16DMA3(0,strpcmL0,32768);
  //MemSet16DMA3(0,strpcmL1,32768);
  //MemSet16DMA3(0,strpcmR0,32768);
  //MemSet16DMA3(0,strpcmR1,32768);
  //strpcmLastL=0;
  //strpcmLastR=0;


  IPC6->IR=IR_NULL;
#ifdef ENABLE_DEBUG    
  _consolePrintf("strpcm Stopped.\n");
#endif
}

//////////////////////////////////////////////////////////////////////

#define MAX( x, y ) ( ( x > y ) ? x : y )
#define MIN( x, y ) ( ( x < y ) ? x : y )

__attribute__((noinline)) static void InterruptHandler_Timer1_SetSwapChannel(void)
{
  s16 *lbuf,*rbuf;
  u32 channel=strpcmCursorFlag;

  if(channel==0){
    lbuf=strpcmL0;
    rbuf=strpcmR0;
    }else{
    lbuf=strpcmL1;
    rbuf=strpcmR1;
  }
  
  
  //u32 chcnt=IPC6->SoundChannels;
  
  //if((chcnt==0)||(chcnt==1)){
  // Left channel
  SCHANNEL_CR(channel) = 0;
  SCHANNEL_SOURCE(channel) = (uint32)lbuf;
  SCHANNEL_CR(channel) = SCHANNEL_ENABLE | SOUND_ONE_SHOT | SOUND_VOL(0x7F) | SOUND_PAN(0) | SOUND_16BIT;
  //}
  
  channel+=2;
  
  //if((chcnt==0)||(chcnt==2)){
  // Right channel
  SCHANNEL_CR(channel) = 0;
  SCHANNEL_SOURCE(channel) = (uint32)rbuf;
  SCHANNEL_CR(channel) = SCHANNEL_ENABLE | SOUND_ONE_SHOT | SOUND_VOL(0x7F) | SOUND_PAN(0x7F) | SOUND_16BIT;
  //}
  
}

  static const s32 LogVolumeCount=128;
  static const s32 LogVolume[LogVolumeCount]={
  0, 32, 64, 96,128,160,192,224,256,288,320,352,384,416,448,480,
512,544,576,608,640,672,704,736,768,800,832,864,896,928,960,992,
1024,1056,1088,1120,1152,1184,1216,1248,1280,1312,1344,1376,1408,1440,1472,1504,
1536,1568,1600,1632,1664,1696,1728,1760,1792,1824,1856,1888,1920,1952,1984,2016,
2048,2123,2202,2283,2368,2455,2546,2641,2738,2840,2945,3054,3167,3284,3405,3531,
3662,3798,3938,4084,4235,4392,4554,4723,4898,5079,5267,5462,5664,5873,6091,6316,
6550,6792,7043,7304,7574,7854,8145,8446,8759,9083,9419,9768,10129,10504,10893,11296,
11714,12147,12597,13063,13546,14047,14567,15106,15665,16244,16846,17469,18115,18785,19481,20201,};

__attribute__((noinline)) static void InterruptHandler_Timer1_ApplyVolume(s16 *lbuf,s16 *rbuf,u32 count)//not use
{
  //return;
  if((lbuf==NULL)||(rbuf==NULL)) return;
  
  s32 vol=strpcmVolume64;
  if(vol <= 0x40) return;
  if((LogVolumeCount-1)<vol) vol=LogVolumeCount-1;
  
  if(vol==0){
    for(u32 idx=0;idx<count;idx++){
      lbuf[idx]=0;
      rbuf[idx]=0;
    }
    return;
  }
  
  u32 MasterVolume=LogVolume[vol];
  
  VolNoAGC(lbuf,rbuf,count,MasterVolume);
}
// --------------------------------------------------------------------

static void InterruptHandler_Timer1_PCMx1(void)
{
  if(strpcmPlayFlag==false)return;
  if (strpcmPlayFlagempty==false)
  {
      InterruptHandler_Timer1_SetSwapChannel();
  }
  else
  {
#ifdef ENABLE_DEBUG  
      _consolePrintf("arm9 empty\n");
#endif
  }
    strpcmCursorFlag=1-strpcmCursorFlag;

        strpcmPlayFlagempty=true;
  s16 *lbuf,*rbuf;
  
  if(strpcmCursorFlag==0){
    lbuf=strpcmL0;
    rbuf=strpcmR0;
    }else{
    lbuf=strpcmL1;
    rbuf=strpcmR1;
  }
  u32 wp= audio_buf_write;
  u32 rp= audio_buf_read;
  u32 rpaddr=audio_buf[rp&strpcmRingBufBitMask];

  s16 *lsrc=(s16 *)rpaddr;
  s16 *rsrc=(s16 *)(rpaddr + strpcmdatalen); 
  
  u32 Samples=strpcmSamples;
    int  cmpp= wp - rp;
    if (cmpp == 0)
    {
        MemSet16DMA3(0,lbuf,Samples*2);
        MemSet16DMA3(0,rbuf,Samples*2);
    }
    else
    {
        if(strpcmChannels==2){
          MemCopy16DMA3(lsrc,lbuf,Samples*2);
          MemCopy16DMA3(rsrc,rbuf,Samples*2);
          }else{
          MemCopy16DMA3(lsrc,lbuf,Samples*2);
          MemCopy16DMA3(lsrc,rbuf,Samples*2);
        }
        audio_buf_read=rp+1;
        InterruptHandler_Timer1_ApplyVolume(lbuf,rbuf,Samples);
        strpcmPlayFlagempty=false;
    }
}

static void InterruptHandler_Timer1_PCMx2(void)
{
  if(strpcmPlayFlag==false)return;
  if (strpcmPlayFlagempty==false)
  {
      InterruptHandler_Timer1_SetSwapChannel();
  }
  else
  {
#ifdef ENABLE_DEBUG  
      _consolePrintf("arm9 empty\n");
#endif
  }
    strpcmCursorFlag=1-strpcmCursorFlag;
    strpcmPlayFlagempty=true;

  s16 *lbuf,*rbuf;
  
  if(strpcmCursorFlag==0){
    lbuf=strpcmL0;
    rbuf=strpcmR0;
    }else{
    lbuf=strpcmL1;
    rbuf=strpcmR1;
  }
  
  u32 wp= audio_buf_write;
  u32 rp= audio_buf_read;
  u32 rpaddr=audio_buf[rp&strpcmRingBufBitMask];

  s16 *lsrc=(s16 *)rpaddr;
  s16 *rsrc=(s16 *)(rpaddr + strpcmdatalen); 
  
  u32 Samples=strpcmSamples;
    int  cmpp= wp - rp;
  
  if (cmpp == 0){
    MemSet16DMA3(0,lbuf,Samples*2);
    MemSet16DMA3(0,rbuf,Samples*2);
    }else{
    if(strpcmChannels==2){
        fix_PCM2_0(lsrc,rsrc,lbuf,rbuf,&strpcmLastL,&strpcmLastR,Samples,0xffff);
      }else{
        fix_PCM2_1(lsrc,rsrc,lbuf,rbuf,&strpcmLastL,&strpcmLastR,Samples,0xffff);
    }
    
        audio_buf_read=rp+1;
        InterruptHandler_Timer1_ApplyVolume(lbuf,rbuf,Samples);
        strpcmPlayFlagempty=false;

  }
  
}

static void InterruptHandler_Timer1_PCMx4(void)
{
  if(strpcmPlayFlag==false)return;
  if (strpcmPlayFlagempty==false)
  {
      InterruptHandler_Timer1_SetSwapChannel();
  }
  else
  {
#ifdef ENABLE_DEBUG  
      _consolePrintf("arm9 empty\n");
#endif
  }
  strpcmCursorFlag=1-strpcmCursorFlag;
    strpcmPlayFlagempty=true;

  //if (InterruptTest ==1)
  //{
  //    _consolePrintf("InterruptTest\n");
  //}
  //InterruptTest =1;

  s16 *lbuf,*rbuf;
  
  if(strpcmCursorFlag==0){
    lbuf=strpcmL0;
    rbuf=strpcmR0;
    }else{
    lbuf=strpcmL1;
    rbuf=strpcmR1;
  }
  
  u32 wp= audio_buf_write;
  u32 rp= audio_buf_read;
  u32 rpaddr=audio_buf[rp&strpcmRingBufBitMask];

  s16 *lsrc=(s16 *)rpaddr;
  s16 *rsrc=(s16 *)(rpaddr + strpcmdatalen); 
  
  u32 Samples=strpcmSamples;
    int  cmpp= wp - rp;

  if (cmpp == 0){
    MemSet16DMA3(0,lbuf,Samples*2);
    MemSet16DMA3(0,rbuf,Samples*2);
    }else{
    if(strpcmChannels==2){
        fix_PCM4_0(lsrc,rsrc,lbuf,rbuf,&strpcmLastL,&strpcmLastR,Samples,0xffff);
      }else{
        fix_PCM4_1(lsrc,rsrc,lbuf,rbuf,&strpcmLastL,&strpcmLastR,Samples,0xffff);
    }
    
        audio_buf_read=rp+1;
        InterruptHandler_Timer1_ApplyVolume(lbuf,rbuf,Samples);
        strpcmPlayFlagempty=false;
  }
  //InterruptTest =0;

}
static void InterruptHandler_Timer1_Null(void)
{
}
static inline void main_InitSoundDevice(void)
{
    powerON(POWER_SOUND);
    MIC_Off();
    SOUND_CR = SOUND_ENABLE | SOUND_VOL(0x7F);

    REG_POWERCNT&=~POWER_UNKNOWN; 

    swiChangeSoundBias(1,0x400);


    u8 control;
    control = PM_ReadRegister(0);
    control|=PM_SOUND_AMP;
    PM_WriteRegister(0, control&255);

      strpcmL0=(s16*)strpcmL[0];//(s16*)malloc(strpcmSamples*2);
      strpcmL1=(s16*)strpcmL[1];//(s16*)malloc(strpcmSamples*2);
      strpcmR0=(s16*)strpcmR[0];//(s16*)malloc(strpcmSamples*2);
      strpcmR1=(s16*)strpcmR[1];//(s16*)malloc(strpcmSamples*2);

}

void main_InitIRQ(void)
{
  main_InitSoundDevice();

  irqSet(IRQ_TIMER1,InterruptHandler_Timer1_Null);
  irqEnable(IRQ_TIMER1);

}




__attribute__((noinline))  void main_Proc_strpcmControl(void)
{
#ifdef ENABLE_DEBUG  
  _consolePrintf("main_Proc_strpcmControl();\nIPC6->strpcmControl=%d.\n",IPC6->strpcmControl);
#endif
  switch(IPC6->strpcmControl){
    case ESC_NOP: {
    } break;
    case ESC_Play: {
        strpcmPlay();
        switch(strpcmFormat){
          case SPF_PCMx1: 
              irqSet(IRQ_TIMER1,InterruptHandler_Timer1_PCMx1);
#ifdef ENABLE_DEBUG  
              _consolePrintf("Timer1_PCMx1\n"); 
#endif
              break;
          case SPF_PCMx2: 
              irqSet(IRQ_TIMER1,InterruptHandler_Timer1_PCMx2);
#ifdef ENABLE_DEBUG  
              _consolePrintf("Timer1_PCMx2\n"); 
#endif
              break;
          case SPF_PCMx4: 
              irqSet(IRQ_TIMER1,InterruptHandler_Timer1_PCMx4);
#ifdef ENABLE_DEBUG  
              _consolePrintf("Timer1_PCMx4\n"); 
#endif
              break;
          default: 
              irqSet(IRQ_TIMER1,InterruptHandler_Timer1_Null );
#ifdef ENABLE_DEBUG  
              _consolePrintf("Timer1_Null\n"); 
#endif
              break;
      }
      strpcmPlayFlag=true;
#ifdef ENABLE_DEBUG  
      _consolePrintf("ESC_Play\n");
#endif

    } break;
    case ESC_Stop: {
      strpcmPlayFlag=false;
      //  strpcmStop();
      //irqSet(IRQ_TIMER1,InterruptHandler_Timer1_Null);
#ifdef ENABLE_DEBUG  
      _consolePrintf("ESC_Stop\n");
#endif

    } break;
    default: {
#ifdef ENABLE_DEBUG  
        _consolePrintf("main_Proc_strpcmControl error\n");
#endif
      REG_IME=0;
      strpcmPlayFlag=false;
    } break;
  }
  IPC6->strpcmControl=ESC_NOP;

}

