/**************************************************************************
 * Copyright (C) 2008 Cocha                                               *
 * http://sourceforge.jp/projects/ecodecotool/                            *
 *                                                                        *
 *  This Program is free software; you can redistribute it and/or modify  *
 *  it under the terms of the GNU General Public License as published by  *
 *  the Free Software Foundation; either version 2, or (at your option)   *
 *  any later version.                                                    *
 *                                                                        *
 *  This Program is distributed in the hope that it will be useful,       *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the          *
 *  GNU General Public License for more details.                          *
 *                                                                        *
 *  You should have received a copy of the GNU General Public License     *
 *  along with GNU Make; see the file COPYING.  If not, write to          *
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. *
 *                                                                        *
 **************************************************************************/

#include "ConvertWav.h"
#include <emmintrin.h>

#define DefaultGain	255.0/256.0	
#define ALPHA 10.0               

#ifndef M_PI
#  define M_PI acos(-1.0)
#endif

struct LCM
{
   int nSamplingRate;
   int lcm[13];
};

const LCM lcm[13] = {   

   {  8000,{    1,   441,     3,     2,   441,     3,     4,   441,     6,   441,    12,   441,    24}}, 
   { 11025,{  320,     1,   160,   640,     2,   320,  1280,     4,   640,     8,  1280,    16,  2560}},
   { 12000,{    2,   147,     1,     4,   147,     2,     8,   147,     4,   147,     8,   147,    16}},
   { 16000,{    1,   441,     3,     1,   441,     3,     2,   441,     3,   441,     6,   441,    12}},
   { 22050,{  160,     1,    80,   320,     1,   160,   640,     2,   320,     4,   640,     8,  1280}},
   { 24000,{    1,   147,     1,     2,   147,     1,     4,   147,     2,   147,     4,   147,     8}},
   { 32000,{    1,   441,     3,     1,   441,     3,     1,   441,     3,   441,     3,   441,     6}},
   { 44100,{   80,     1,    40,   160,     1,    80,   320,     1,   160,     2,   320,     4,   640}},
   { 48000,{    1,   147,     1,     1,   147,     1,     2,   147,     1,   147,     2,   147,     4}},
   { 88200,{   40,     1,    20,    80,     1,    40,   160,     1,    80,     1,   160,     2,   320}},
   { 96000,{    1,   147,     1,     1,   147,     1,     1,   147,     1,   147,     1,   147,     2}},
   {176400,{   20,     1,    10,    40,     1,    20,    80,     1,    40,     1,    80,     1,   160}},
   {192000,{    1,   147,     1,     1,   147,     1,     1,   147,     1,   147,     1,   147,     1}}
};


HRESULT CConvertSamples::ConvertInit(WAVEFORMATEX *pIn, WAVEFORMATEX *pOut, int nAccurate)
{
   ::CopyMemory(&m_inFormat, pIn, sizeof(m_inFormat));
   ::CopyMemory(&m_outFormat, pOut, sizeof(m_outFormat));

   long i;
   double regv, wfnc;

   double frqc; 
   double frqs; 

   frqc = 0;
   firodrv = 0;


   for(int i=0;i<13;i++)
   {
      for(int j=0;j<13;j++)
      {
         if( (m_inFormat.nSamplesPerSec == lcm[i].nSamplingRate) &&
             (m_outFormat.nSamplesPerSec == lcm[j].nSamplingRate))
         {
            isrm = lcm[i].lcm[j];
            osrm = lcm[j].lcm[i];

            break;
         }
      }
   }

   frqs = m_outFormat.nSamplesPerSec * osrm;

   frqc = min(m_inFormat.nSamplesPerSec, m_outFormat.nSamplesPerSec) / 2;

   if( (65536/isrm) >= (nAccurate*20+120))
      firodrv = isrm * (nAccurate*20+120);
   else
      firodrv = 65535;

   double divisor;
   divisor = bessel0(ALPHA);


   double hgain;
   hgain = 2.0 * isrm * frqc / frqs;
   m_hfir[0] = hgain;
   for(i=1; i<=firodrv; i++)
   {
      regv = (double)i/(double)(firodrv);
      regv = regv * regv;
      wfnc = bessel0(sqrt(1.0-regv) * ALPHA) / divisor;   
      regv = 2.0 * M_PI * frqc / frqs * (double)i;
      m_hfir[i] = hgain * sin(regv) / regv * wfnc;
   }

   ismd = osrm / isrm;
   ismr = osrm % isrm;

   sptr  = -(firodrv/isrm);
   sptrr = -(firodrv%isrm);
   eptr  =   firodrv/isrm;
   eptrr =   firodrv%isrm;

   iptr = 0;

   if(m_outFormat.wFormatTag == WAVE_FORMAT_PCM)
   {
      if(m_outFormat.wBitsPerSample == 8)
      {
         m_dMinData = -127.5;
         m_dMaxData =  126.5;
      }
      else if(m_outFormat.wBitsPerSample == 16)
      {
         m_dMinData = -32767.5;
         m_dMaxData =  32766.5;
      }
      else if(m_outFormat.wBitsPerSample == 24)
      {
         m_dMinData = -8388607.5;
         m_dMaxData =  8388606.5;
      }
      else 
      {
         m_dMinData = -2147483647.5;
         m_dMaxData =  2147483646.5;
      }
   }
   else 
   {
       m_dMinData = -1.0;
       m_dMaxData =  1.0;
   }

   if(m_outFormat.wBitsPerSample == m_inFormat.wFormatTag && m_outFormat.wBitsPerSample == m_inFormat.wBitsPerSample)
   {
      m_dRatioPlus = DefaultGain;
      m_dRatioMinus = DefaultGain;
   }
   else
   {
      if(m_inFormat.wFormatTag == WAVE_FORMAT_PCM)
      {
         if(m_outFormat.wFormatTag == WAVE_FORMAT_PCM)
         {  
            m_dRatioPlus = DefaultGain * (pow(2.0, m_outFormat.wBitsPerSample-1) - 1) / (pow(2.0, m_inFormat.wBitsPerSample-1) - 1);
            m_dRatioMinus = DefaultGain * pow(2.0, m_outFormat.wBitsPerSample-1) / pow(2.0, m_inFormat.wBitsPerSample-1);
         }
         else
         {  
            m_dRatioPlus = DefaultGain / (pow(2.0, m_inFormat.wBitsPerSample-1) - 1);
            m_dRatioMinus = DefaultGain / pow(2.0, m_inFormat.wBitsPerSample-1);
         }
      }
      else  
      {
         if(m_outFormat.wFormatTag == WAVE_FORMAT_PCM)
         {  
            m_dRatioPlus = DefaultGain * (pow(2.0, m_outFormat.wBitsPerSample-1) - 1);
            m_dRatioMinus = DefaultGain * pow(2.0, m_outFormat.wBitsPerSample-1);
         }
         else 
         {  
            m_dRatioPlus = DefaultGain;
            m_dRatioMinus = DefaultGain;
         }
      }
   }

   return S_OK;
}
void CConvertSamples::Convert(struct SmplData *smpld, int nSmplSize, BYTE *pOutData, int *pOutLength)
{  

   float fData;
   int i, j, l, nData;
   double dLeft, dRight;

   *pOutLength = 0;

   iptr += nSmplSize;

   while(true)
   {  

      if(eptr >= iptr)
      {   
         return;
      }

      dLeft = 0.0;
      dRight = 0.0;

      j = -firodrv - sptrr;   

      if(m_inFormat.nChannels == 1)
      {
         for(i=sptr; i<=eptr; i++, j+=isrm)
         {
            l=j;
            if(j<0)   l=-j;   

            dLeft = dLeft + m_hfir[l] * smpld[i&0x01ffff].l;
         }

         if(dLeft < 0.0)
            dLeft *= m_dRatioMinus;
         else
            dLeft *= m_dRatioPlus;
      }
      else 
      {
         for(i=sptr; i<=eptr; i++, j+=isrm)
         {
            l=j;
            if(j<0)   l=-j;   

            dLeft = dLeft + m_hfir[l] * smpld[i&0x1ffff].l;
            dRight = dRight + m_hfir[l] * smpld[i&0x1ffff].r;
         }

         if(dLeft < 0.0)
            dLeft *= m_dRatioMinus;
         else
            dLeft *= m_dRatioPlus;

         if(dRight < 0.0)
            dRight *= m_dRatioMinus;
         else
            dRight *= m_dRatioPlus;
      }

      if(m_inFormat.nChannels == 2 && m_outFormat.nChannels == 1)
         dLeft = dLeft + dRight;

      if(dLeft < m_dMinData)
         dLeft = m_dMinData;

      if(dLeft > m_dMaxData)
         dLeft = m_dMaxData;

      if(m_outFormat.wBitsPerSample == 8)
         dLeft += 128.0;

      if(m_outFormat.wFormatTag == WAVE_FORMAT_PCM)
      {
         if(dLeft > 0.0) nData = int(dLeft + 0.5);
         else            nData = int(dLeft - 0.5);

         for(i=0;i<m_outFormat.wBitsPerSample;i+=8)
         {
            pOutData[*pOutLength] = nData >> i;
            (*pOutLength)++;
         }
      }
      else  
      {
         if(m_outFormat.wBitsPerSample == 32)
         {
            fData = (float)dLeft;
            ::CopyMemory(&pOutData[*pOutLength], &fData, 4);
            *pOutLength += 4;
         }
         else 
         {
            ::CopyMemory(&pOutData[*pOutLength], &dLeft, 8);
            *pOutLength += 8;
         }

      }

      if(m_outFormat.nChannels == 2)
      {
         if(m_inFormat.nChannels == 1)
         {
            dRight = dLeft; 
         }
         else
         {
            if(dRight < m_dMinData)
               dRight = m_dMinData;

            if(dRight > m_dMaxData)
               dRight = m_dMaxData;

            if(m_outFormat.wBitsPerSample == 8)
               dRight += 128.0;
         }

         if(m_outFormat.wFormatTag == WAVE_FORMAT_PCM)
         {
            if(dRight > 0.0) nData = int(dRight + 0.5);
            else             nData = int(dRight - 0.5);

            for(i=0;i<m_outFormat.wBitsPerSample;i+=8)
            {
               pOutData[*pOutLength] = nData >> i;
               (*pOutLength)++;
            }
         }
         else  
         {
            if(m_outFormat.wBitsPerSample == 32)
            {
               fData = (float)dLeft;
               ::CopyMemory(&pOutData[*pOutLength], &fData, 4);
               *pOutLength += 4;
            }
            else 
            {
               ::CopyMemory(&pOutData[*pOutLength], &dLeft, 8);
               *pOutLength += 8;
            }
         }
      }

      sptr += ismd;
      sptrr += ismr;
      if(sptrr>0)
      {
         sptrr -= isrm;
         sptr++;
      }

      eptr += ismd;
      eptrr += ismr;
      if(eptrr>=isrm)
      {
         eptrr-=isrm;
         eptr++;
      }
   }
}
void CConvertSamples::ConvertSSE2(struct SmplData *smpld, int nSmplSize, BYTE *pOutData, int *pOutLength)
{  

   float fData;
   int i, j, l, nData;
   SmplData outSmplData;
   __m128d t1, t2, t3; 

   *pOutLength = 0;

   iptr += nSmplSize;

   while(true)
   {  

      if(eptr >= iptr)
      {   
         return;
      }

      outSmplData.l = 0.0;
      outSmplData.r = 0.0;

      j = -firodrv - sptrr;   

      if(m_inFormat.nChannels == 1)
      {
         for(i=sptr; i<=eptr; i++, j+=isrm)
         {
            l=j;
            if(j<0)   l=-j;   

            outSmplData.l = outSmplData.l + m_hfir[l] * smpld[i&0x01ffff].l;
         }

         if(outSmplData.l < 0.0)
            outSmplData.l *= m_dRatioMinus;
         else
            outSmplData.l *= m_dRatioPlus;
      }
      else 
      {
         t1 = _mm_load_pd((double *)&outSmplData);

         for(i=sptr; i<=eptr; i++, j+=isrm)
         {
            l=j;
            if(j<0)   l=-j;   

            t2 = _mm_load_pd((double *)&smpld[i&0x1ffff]);  
            t3 = _mm_load1_pd((double *)&m_hfir[l]);        

            t3 = _mm_mul_pd(t2, t3);                        
            t1 = _mm_add_pd(t1, t3);                        
         }

         _mm_store_pd((double *)&outSmplData, t1); 

         if(outSmplData.l < 0.0)
            outSmplData.l *= m_dRatioMinus;
         else
            outSmplData.l *= m_dRatioPlus;

         if(outSmplData.r < 0.0)
            outSmplData.r *= m_dRatioMinus;
         else
            outSmplData.r *= m_dRatioPlus;
      }

      if(m_inFormat.nChannels == 2 && m_outFormat.nChannels == 1)
         outSmplData.l = outSmplData.l + outSmplData.r;

      if(outSmplData.l < m_dMinData)
         outSmplData.l = m_dMinData;

      if(outSmplData.l > m_dMaxData)
         outSmplData.l = m_dMaxData;

      if(m_outFormat.wBitsPerSample == 8)
         outSmplData.l += 128.0;

      if(m_outFormat.wFormatTag == WAVE_FORMAT_PCM)
      {
         if(outSmplData.l > 0.0) nData = int(outSmplData.l + 0.5);
         else            nData = int(outSmplData.l - 0.5);

         for(i=0;i<m_outFormat.wBitsPerSample;i+=8)
         {
            pOutData[*pOutLength] = nData >> i;
            (*pOutLength)++;
         }
      }
      else  
      {
         if(m_outFormat.wBitsPerSample == 32)
         {
            fData = (float)outSmplData.l;
            ::CopyMemory(&pOutData[*pOutLength], &fData, 4);
            *pOutLength += 4;
         }
         else 
         {
            ::CopyMemory(&pOutData[*pOutLength], &outSmplData.l, 8);
            *pOutLength += 8;
         }

      }

      if(m_outFormat.nChannels == 2)
      {
         if(m_inFormat.nChannels == 1)
         {
            outSmplData.r = outSmplData.l; 
         }
         else
         {
            if(outSmplData.r < m_dMinData)
               outSmplData.r = m_dMinData;

            if(outSmplData.r > m_dMaxData)
               outSmplData.r = m_dMaxData;

            if(m_outFormat.wBitsPerSample == 8)
               outSmplData.r += 128.0;
         }

         if(m_outFormat.wFormatTag == WAVE_FORMAT_PCM)
         {
            if(outSmplData.r > 0.0) nData = int(outSmplData.r + 0.5);
            else                    nData = int(outSmplData.r - 0.5);

            for(i=0;i<m_outFormat.wBitsPerSample;i+=8)
            {
               pOutData[*pOutLength] = nData >> i;
               (*pOutLength)++;
            }
         }
         else  
         {
            if(m_outFormat.wBitsPerSample == 32)
            {
               fData = (float)outSmplData.r;
               ::CopyMemory(&pOutData[*pOutLength], &fData, 4);
               *pOutLength += 4;
            }
            else 
            {
               ::CopyMemory(&pOutData[*pOutLength], &outSmplData.r, 8);
               *pOutLength += 8;
            }
         }
      }

      sptr += ismd;
      sptrr += ismr;
      if(sptrr>0)
      {
         sptrr -= isrm;
         sptr++;
      }

      eptr += ismd;
      eptrr += ismr;
      if(eptrr>=isrm)
      {
         eptrr-=isrm;
         eptr++;
      }
   }
}
void CConvertSamples::ConvertEnd(struct SmplData *smpld, int nSmplSize, BYTE *pOutData, int *pOutLength)
{  

   float fData;
   int i, j, l, nData, optr;
   double dLeft, dRight;
   int nEnd = int (double(eptr - sptr) / 2.0 / m_inFormat.nSamplesPerSec * m_outFormat.nSamplesPerSec + 0.5);

   *pOutLength = 0;
   optr = 0;

   while(true)
   {  

      if( optr >= nEnd )
         return;

      dLeft = 0.0;
      dRight = 0.0;

      j = -firodrv - sptrr;   

      if(m_inFormat.nChannels == 1)
      {
         for(i=sptr; i<=eptr; i++, j+=isrm)
         {
            l=j;
            if(j<0)   l=-j;   

            dLeft = dLeft + m_hfir[l] * smpld[i&0x1ffff].l;
         }

         if(dLeft < 0.0)
            dLeft *= m_dRatioMinus;
         else
            dLeft *= m_dRatioPlus;
      }
      else 
      {
         for(i=sptr; i<=eptr; i++, j+=isrm)
         {
            l=j;
            if(j<0)   l=-j;   

            dLeft = dLeft + m_hfir[l] * smpld[i&0x1ffff].l;
            dRight = dRight + m_hfir[l] * smpld[i&0x1ffff].r;
         }

         if(dLeft < 0.0)
            dLeft *= m_dRatioMinus;
         else
            dLeft *= m_dRatioPlus;

         if(dRight < 0.0)
            dRight *= m_dRatioMinus;
         else
            dRight *= m_dRatioPlus;
      }

      if(m_inFormat.nChannels == 2 && m_outFormat.nChannels == 1)
         dLeft = dLeft + dRight;

      if(dLeft < m_dMinData)
         dLeft = m_dMinData;

      if(dLeft > m_dMaxData)
         dLeft = m_dMaxData;

      if(m_outFormat.wBitsPerSample == 8)
         dLeft += 128.0;

      if(m_outFormat.wFormatTag == WAVE_FORMAT_PCM)
      {
         if(dLeft > 0.0) nData = int(dLeft + 0.5);
         else            nData = int(dLeft - 0.5);

         for(i=0;i<m_outFormat.wBitsPerSample;i+=8)
         {
            pOutData[*pOutLength] = nData >> i;
            (*pOutLength)++;
         }
      }
      else  
      {
         if(m_outFormat.wBitsPerSample == 32)
         {
            fData = (float)dLeft;
            ::CopyMemory(&pOutData[*pOutLength], &fData, 4);
            *pOutLength += 4;
         }
         else 
         {
            ::CopyMemory(&pOutData[*pOutLength], &dLeft, 8);
            *pOutLength += 8;
         }

      }

      if(m_outFormat.nChannels == 2)
      {
         if(m_inFormat.nChannels == 1)
         {
            dRight = dLeft;
         }
         else
         {
            if(dRight < m_dMinData)
               dRight = m_dMinData;

            if(dRight > m_dMaxData)
               dRight = m_dMaxData;

            if(m_outFormat.wBitsPerSample == 8)
               dRight += 128.0;
         }

         if(m_outFormat.wFormatTag == WAVE_FORMAT_PCM)
         {
            if(dRight > 0.0) nData = int(dRight + 0.5);
            else             nData = int(dRight - 0.5);

            for(i=0;i<m_outFormat.wBitsPerSample;i+=8)
            {
               pOutData[*pOutLength] = nData >> i;
               (*pOutLength)++;
            }
         }
         else  
         {
            if(m_outFormat.wBitsPerSample == 32)
            {
               fData = (float)dLeft;
               ::CopyMemory(&pOutData[*pOutLength], &fData, 4);
               *pOutLength += 4;
            }
            else 
            {
               ::CopyMemory(&pOutData[*pOutLength], &dLeft, 8);
               *pOutLength += 8;
            }
         }
      }

      optr++;

      sptr += ismd;
      sptrr += ismr;
      if(sptrr>0)
      {
         sptrr -= isrm;
         sptr++;
      }

      eptr += ismd;
      eptrr += ismr;
      if(eptrr>=isrm)
      {
         eptrr-=isrm;
         eptr++;
      }
   }
}
double CConvertSamples::bessel0(double x)
{
   double part, ret, step;

   ret = 1.0;
   step = 1.0;

   part = (x/2.0) / step;

   while(part > 1.0E-10)
   {
      ret += part * part;
      step += 1.0;
      part = part * (x / 2.0) / step;
   }

   return(ret);
}
