/***************************************************/
/*! \class MyHRTF
    \STK HRTF Spatializer

    This private class spatializes input samples according to
	the specified azimuth and elevation in degrees.
	It requires an HRTF profile to make calculations
	based on individualized coefficients.

    Class structure taken from MyJCRev: John Chowning Reverberator
    Rego Sen, 220c Project, Spring Quarter 2003-2004
*/
/***************************************************/


#include "MyHRTF.h"
#include "Filter.h"
#include "RevRev.h"
#include <stdio.h>
#include <math.h>
#include "Stk.h"
#include <iostream>
//#include <complex>


using namespace std;

MyHRTF :: MyHRTF(MY_FLOAT thetaDeg, MY_FLOAT phiDeg, char* filename)
{

  FILE * rx;
  rx=fopen(filename,"r");


  int i;
  double pi = 3.14159265;

  int fs = (int)Stk::sampleRate();

  theta = deg2Rad(thetaDeg);
  phi = deg2Rad(phiDeg);
  leftEarPos = deg2Rad(-100.0);
  rightEarPos = deg2Rad(100.0);
 MY_FLOAT clA,clB,clD,crA,crB,crD,clP,crP;

  /*

 MY_FLOAT cleftA[6] = {0,1,5,5,5,5};
 MY_FLOAT cleftB[6] = {0,2,4,7,11,13};
 MY_FLOAT cleftD[6] = {0,1,0.5,0.5,0.5,0.5};
 MY_FLOAT crightA[6] = {0,1,5,5,5,5};
 MY_FLOAT crightB[6] = {0,2,4,7,11,13};
 MY_FLOAT crightD[6] = {0,1,0.5,0.5,0.5,0.5};
 MY_FLOAT cleftPinnaP[6] = {0,0.5,-1,0.5,-0.25,0.25};
 MY_FLOAT crightPinnaP[6] = {0,0.5,-1,0.5,-0.25,0.25};

 headR=0.07;
 fprintf(rx,"%lf\n",headR);

  for (i=0;i<6;i++) {
 fprintf(rx,"%lf\n",cleftA[i]);

 fprintf(rx,"%lf\n",cleftB[i]);
 fprintf(rx,"%lf\n",cleftD[i]);
 fprintf(rx,"%lf\n",crightA[i]);
 fprintf(rx,"%lf\n",crightB[i]);
 fprintf(rx,"%lf\n",crightD[i]);
 fprintf(rx,"%lf\n",cleftPinnaP[i]);
 fprintf(rx,"%lf\n",crightPinnaP[i]);
  }

  rewind(rx);
  */


  fscanf(rx,"%lf\n",&headR);

  for (i=0;i<6;i++) {
 fscanf(rx,"%lf\n",&clA);

 fscanf(rx,"%lf\n",&clB);
 fscanf(rx,"%lf\n",&clD);
 fscanf(rx,"%lf\n",&crA);
 fscanf(rx,"%lf\n",&crB);
 fscanf(rx,"%lf\n",&crD);
 fscanf(rx,"%lf\n",&clP);
 fscanf(rx,"%lf\n",&crP);
 leftA[i]=clA;
 leftB[i]=clB;
 leftD[i]=clD;
 rightA[i]=crA;
 rightB[i]=crB;
 rightD[i]=crD;
 leftPinnaP[i]=clP;
 rightPinnaP[i]=crP;
  }


  // TRANSFER FUNCTION FOR HEAD SHADOW
  MY_FLOAT numL[2];   
  MY_FLOAT numR[2];
  MY_FLOAT denL[2];   
  MY_FLOAT denR[2];

  // COEFFICIENT w_0
  //headR = 0.07;  // average radius of human head is 7 cm.
  MY_FLOAT c = 343.0;  // speed of sound = 343 m/sec
  MY_FLOAT w_0 = c/headR;
  // w_0 /= (MY_FLOAT)1000;

  numL[0]=1.0;
  denL[0]=1.0;
  numL[1]=alpha(theta-leftEarPos)/(2.0*w_0);
  denL[1]=1.0/(2.0*w_0);

  numR[0]=1.0;
  denR[0]=1.0;
  numR[1]=alpha(theta-rightEarPos)/(2.0*w_0);
  denR[1]=1.0/(2.0*w_0);
 
  //cout << "ALPHA{L,R} = {" << alpha(theta-leftEarPos) << ", " << alpha(theta-rightEarPos) << "}" << endl;

  leftHS = new Filter(2,numL,2,denL);
  rightHS = new Filter(2,numR,2,denR);


  // DELAY FOR HEAD SHADOW
  // cout << "Azimuth = " << theta << " radians" << endl;
  // cout << "Elevation = " << phi << " radians" << endl;

  if (abs(theta) < pi/2) {
    delHSL = (int)(fs*(-(headR/c)*cos(theta-leftEarPos)));
    delHSR = (int)(fs*(-(headR/c)*cos(theta-rightEarPos)));
  } else {
    delHSL = (int)(fs*((headR/c)*cos(abs(theta-leftEarPos)-(pi/2.0))));
    delHSR = (int)(fs*((headR/c)*cos(abs(theta-rightEarPos)-(pi/2.0))));
  }
  delHSL += (int)(fs*(headR/c));
  delHSR += (int)(fs*(headR/c));
  //delHSL *= fs/1000;
  //delHSR *= fs/1000;
  delHSL *= 1;
  delHSR *= 1;

  //cout << "Left HS Delay = " << delHSL << endl;
  //cout << "Right HS Delay = " << delHSR << endl;
  leftHeadT = new Delay(delHSL,delHSL);
  rightHeadT = new Delay(delHSR,delHSR);

  // DELAYS FOR PINNA
  int lP[6], rP[6];
  for (i=0;i<6;i++)
    {
    lP[i]=(int)(1.0*(leftA[i]*cos((theta)/2.0)*sin(leftD[i]*((pi/2.0)-phi)) + leftB[i]));
    rP[i]=(int)(1.0*(rightA[i]*cos((theta)/2.0)*sin(rightD[i]*((pi/2.0)-phi)) + rightB[i]));

    // This assumes 44.1k sampling rate -- enter code to adjust for other rates:

    //lP[i] *= fs/1000;
    //rP[i] *= fs/1000;

    //cout << "Left Pinna[" << i << "] Delay = " << lP[i] << endl;
    //cout << "Right Pinna[" << i << "] Delay = " << rP[i] << endl;
    leftPinnaT[i] = new Delay(lP[i],lP[i]);
    rightPinnaT[i] = new Delay(rP[i],rP[i]);

    }
  //this->clear();

}

MY_FLOAT MyHRTF :: alpha(MY_FLOAT th)
{
 double pi = 3.14159265;
  MY_FLOAT a_min = 0.1;
  MY_FLOAT t_min = deg2Rad(150.0);
  return (1.0+a_min/2.0)+(1.0-a_min/2.0)*cos(pi*th/t_min);
}

MY_FLOAT MyHRTF :: deg2Rad(MY_FLOAT deg)
{
  return deg*2.0*3.14159265/360.0;
}


MyHRTF :: ~MyHRTF()
{

  delete leftHS;
  delete rightHS;
  delete leftHeadT;
  delete rightHeadT;
  for (int i=0;i<6;i++) {
  delete leftPinnaT[i];
  delete rightPinnaT[i];
  }

}

void MyHRTF :: clear()
{
  leftHS->clear();
  rightHS->clear();
  leftHeadT->clear();
  rightHeadT->clear();
  leftPinnaT[0]->clear();
  leftPinnaT[1]->clear();
  leftPinnaT[2]->clear();
  leftPinnaT[3]->clear();
  leftPinnaT[4]->clear();
  leftPinnaT[5]->clear();
  rightPinnaT[0]->clear();
  rightPinnaT[1]->clear();
  rightPinnaT[2]->clear();
  rightPinnaT[3]->clear();
  rightPinnaT[4]->clear();
  rightPinnaT[5]->clear();

  theta = 0.0;
  phi = 0.0;
  headR = 0.0;
  leftEarPos = 0.0;
  rightEarPos = 0.0;

  leftHSOut = 0.0;
  leftHeadOut = 0.0;
  rightHSOut = 0.0;
  rightHeadOut = 0.0;

  delHSL = 0;
  delHSR = 0;

  for (int i=1;i<6;i++) {
  leftPinnaOut[i] = 0.0;
  rightPinnaOut[i] = 0.0;
  leftA[i] = 0.0;
  leftB[i] = 0.0;
  leftD[i] = 0.0;
  rightA[i] = 0.0;
  rightB[i] = 0.0;
  rightD[i] = 0.0;
  leftPinnaP[i] = 0.0;
  rightPinnaP[i] = 0.0;
  }
  output[0] = 0.0;
  output[1] = 0.0;
}


MY_FLOAT MyHRTF :: lastOutLeft()
{
  return output[0];
}

MY_FLOAT MyHRTF :: lastOutRight()
{
  return output[1];
}

void MyHRTF :: tick(MY_FLOAT sample, MY_FLOAT* out)
{

   leftHSOut=leftHS->tick(sample);
   leftHeadOut=leftHeadT->tick(leftHSOut);

   rightHSOut=rightHS->tick(sample);
   rightHeadOut=rightHeadT->tick(rightHSOut);

  for (int i=1;i<6;i++)
    {
    leftPinnaOut[i]=leftPinnaT[i]->tick(leftPinnaP[i]*leftHeadOut);
    rightPinnaOut[i]=rightPinnaT[i]->tick(rightPinnaP[i]*rightHeadOut);
    }

  output[0] = leftHeadOut+leftPinnaOut[1]+leftPinnaOut[2]+leftPinnaOut[3]+leftPinnaOut[4]+leftPinnaOut[5];
  output[1] = rightHeadOut+rightPinnaOut[1]+rightPinnaOut[2]+rightPinnaOut[3]+rightPinnaOut[4]+rightPinnaOut[5];
    out[0] = output[0];
    out[1] = output[1];

  return;
}