/***************************************************************************
                          gauss.cpp  -  description
                             -------------------
    begin                : Mit Jul 23 2003
    copyright            : (C) 2003 by klaus frieler
    email                : kgf@omniversum.de
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   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 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/
#include "Types.h"
#include "gauss.h"

#include <math.h>
#include "VectorManip.cpp"

static double *dGauss = 0;


bool initGauss()
{
   double left = -3.71;
   double right = 3.71;
   double step  = 1e-6;
   uint count = 7420000 + 1;
   uint i = 0;
   dGauss = new double[count];
   if(!dGauss) return false;
   cerr<<"Init Gauss function lookup table..."<<endl;
   for(double t = left; t<=right; t+= step, ++i){
      dGauss[i] = gauss(t);
   }
   cerr<<"Done."<<endl;
   return true;
}

void exitGauss()
{
   if(dGauss){
      delete[] dGauss;
      dGauss = 0;
   }
}
inline
double gauss(double t)
{
   if( t >= 3.71 || t <= -3.71) return 0;
   return exp(-t*t);
}

inline
double fastGauss(double t)
{
   if( t >= 3.71 || t <= -3.71) return 0;
   return dGauss[ (uint)floor((t +3.71)* 1000000 +0.5)];
}

data  gaussFourier(const data& vec, const data& koeffs, double step, double sigma)
{
  return gaussFourier(vec, koeffs, vec.size(), step, sigma);
}

data  gaussFourier(const data& vec, const data& koeffs, uint max, double step, double sigma)
{
   data fourier, phase;
   double sum = 0.0;
   uint size = vec.size();
   if(max<size) size = max;
   if(!sigma) return fourier;
   const double end =  20;
   double im = 0.0, re = 0.0;
   for(double f = 0; f<end; f+=step){
      double w = 2 * 3.14159236 * f * 1e-3;
      double phi = 0.;
      im = 0.0;
      re = 0.0;
      sum = 0.0;
      for(uint i = 0; i<size; i++){
         re += koeffs[i] * cos(w*vec[i]);
         im += koeffs[i] * sin(w*vec[i]);
         sum += koeffs[i] * koeffs[i];
         for(uint j = i+1; j<size; ++j){
            sum += koeffs[i]*koeffs[j]* cos( w * (vec[i]-vec[j]));
         }
      }
      sum *= fastGauss(w*sigma);
      fourier.push_back(sum);
      phi = atan2(im,re);
      phase.push_back(phi);
   }


   return fourier;
}

data getGaussSuperposition(const double dT, const double phase, const uint count, const data &koeffs, double step, double sigma)
{
   data tmp;
   for(uint i = 0; i<count; ++i){
      tmp.push_back(i*dT+phase);
   }
   return getGaussSuperposition(tmp, koeffs, step, sigma);
}

data getGaussSuperposition(const data& vec, const data &koeffs, double step, double sigma)
{
   const double factor = 0.5/ sigma;
   double start = vec[0];
   double end = *(vec.end()-1);
   double sum = 0;
   data tmp;

   for(double t = start-2*sigma; t<end+ 2*sigma; t+= step){
      sum = 0.0;
      for(uint i = 0; i<vec.size(); ++i){
         double b = (t- vec[i]) * factor;
         sum += koeffs[i]*fastGauss(b);
      }
      tmp.push_back(sum);
   }
   return tmp;
}

data gaussAutoCorrelation(const data &vec, double cut, double step, double sigma)
{
   data coeff;
   coeff.assign(vec.size(), 1.0);
   return gaussAutoCorrelation(vec, coeff, cut, step, sigma);
}

data gaussAutoCorrelation(const data &vec, const data &coeff, double cut, double step, double sigma)
{
   const double factor = 0.5/ sigma;
   data tmp;
   uint  size = vec.size();
   double sum = 0;
   double end   = *(vec.end()-1)- *(vec.begin());

   if(end>cut) end = cut;
   if(step>= end/2) return tmp;
   if(coeff.size()!= size) return tmp;

   for (double t = 0; t < end + 2*sigma; t += step){
      sum = 0.0;
      for( uint i1 = 0; i1 <size; ++i1){
         sum += fastGauss( factor*t);
         for( uint i2 = i1 + 1;  i2 <size; ++i2){
            double b1 = vec[i1] - vec[i2] + t;
            double b2 = vec[i2] - vec[i1] + t;
            sum +=  coeff[i1]*coeff[i2]*(fastGauss( factor * b1 ) + fastGauss(factor * b2));
         }
      }
      tmp.push_back(sum);
   }
   scale(tmp, 1.0/tmp[0]);
   
   return tmp;
}

double gaussNorm(const data &vec, const data &coeff, double sigma)
{
   double sum = .0;
   const double factor = 1.0/(2 * sigma);
   for(uint i = 0; i< vec.size(); ++i){
      sum += 1;
      for(uint j = i + 1; j < vec.size(); ++j){
         double b = vec[i] - vec[j];
         if(coeff[i] == 0 || coeff[j] == 0 ) continue;
         sum +=  2 * coeff[i]*coeff[j]*fastGauss( factor * b );
      }
   }
   return sqrt( factor * sum/ sqrt(3.141592) );
}

double gaussNorm(const data &vec, double sigma)
{
   double sum = .0;
   const double factor = 1.0/(2 * sigma);

   for(uint i = 0; i< vec.size(); ++i){
      sum += 1;
      for(uint j = i + 1; j < vec.size(); ++j){
         double b = vec[i] - vec[j];
         sum +=  2 * fastGauss( factor * b );
      }
   }
   return sqrt( factor * sum/ sqrt(3.141592) );
}

double gaussNorm(const double dT, const data &coeff, double sigma)
{
   double sum = .0;
   const double factor = 1.0/(2 * sigma);
   for(uint i = 0; i< coeff.size(); ++i){
      sum += 1;
      for(uint j = i + 1; j< coeff.size(); ++j){
         double b = dT*(j-i);
         if(coeff[i] == 0 || coeff[j] == 0 ) continue;
         sum +=  2 * coeff[i]*coeff[j]*fastGauss( factor* b );
      }
   }
   return sqrt( factor * sum/sqrt(3.141592) );
}

double gaussMatch(const data &vec, const data &accents, const double dT, double phase, const data& accentPattern, double sigma)
{
   double sum = 0.0;
   double len = getMaximum<double>(vec) - getMinimum<double>(vec) - phase;
   const double factor = 1.0/(2 * sigma);
   uint maxI = (uint)floor(len/dT) + 1;
   double norm1 = gaussNorm(vec, accents, sigma);
   double norm2 = gaussNorm(dT, accentPattern, sigma);

   if(vec.size()!=accents.size() || sigma<=0.0) return -1;
   for(uint i = 0; i < vec.size(); ++i){
      if(accents[i] == 0) continue;
      for(uint j = 0; j < maxI; ++j){
         double b = (j * dT + phase - vec[i]);
         double accent = accentPattern[j];
         if(accent == 0 ) continue;
         sum += accents[i]*accent*fastGauss(factor * b);
      }
   }
   return factor * sum/(sqrt(3.14159236)*norm1*norm2);
}

double gaussCrossCorrelation(const data& tp1, const data &koeffs1, const data& tp2, const data &koeffs2, 
                             double sigma, double tau)
{
   double sum = 0.0;
   const double factor = 1.0/(2 * sigma);
   double norm1 = gaussNorm(tp1, koeffs1, sigma);
   double norm2 = gaussNorm(tp1, koeffs1, sigma);

   if(tp1.size()!=koeffs1.size() || tp2.size()!=koeffs2.size()|| sigma<=0.0) return -1;
   for(uint i = 0; i < tp1.size(); ++i){
      for(uint j = 0; j<tp2.size();++j){
         sum += koeffs1[i] *koeffs2[j] * fastGauss(factor * (tp1[i]-tp2[j]-tau));
      }
   }
   return factor * sum/(sqrt(3.14159236)*norm1*norm2);
}

double gaussCrossCorrelation(const data& tp1, const data& tp2, double sigma, double tau)
{
   double sum = 0.0;
   const double factor = 1.0/(2 * sigma);
   if(sigma<=0.0) return -1;
   double norm1 = gaussNorm(tp1,sigma);
   double norm2 = gaussNorm(tp2,sigma);
   for(uint i = 0; i < tp1.size(); ++i){
      for(uint j = 0; j<tp2.size();++j){
         sum += fastGauss(factor * (tp1[i]-tp2[j]-tau));
      }
   }
   if(norm1 == 0 || norm2 == 0) return 0.0;
   return factor * sum/(sqrt(3.14159236)*norm1*norm2);
}

double metricSimilarity(uint n1, uint n2, double sigma, double T)
{
   if(n1 <2 || n2 < 2 || fabs(sigma)<1e-6 ) return 1;
   data tp1, tp2;
   for(uint i = 0; i<n1; ++i){
      tp1.push_back(double(i)/n1);
   }
   for(uint i = 0; i<n2; ++i){
      tp2.push_back(double(i)/n2);
   }
   //dumpData("TP1", tp1, cout);
   //dumpData("TP1", tp2, cout);
   double sum1 = 0;
   double factor = 0.5*T/sigma;
   for(uint i = 0; i < tp1.size(); ++i){
      for(uint j = 0; j<tp2.size();++j){
         sum1 += fastGauss(factor * (tp1[i]-tp2[j]));
      }
   }
   double sum2 = 0;
   for(uint i = 0; i < tp1.size(); ++i){
      for(uint j = 0; j<tp1.size();++j){
         sum2 += fastGauss(factor * (tp1[i]-tp1[j]));
      }
   }
   double sum3 = 0;
   for(uint i = 0; i < tp2.size(); ++i){
      for(uint j = 0; j<tp2.size();++j){
         sum3 += fastGauss(factor * (tp2[i]-tp2[j]));
      }
   }
   double denom = sqrt(sum2*sum3);
   double val = sum1/denom; 
   return val;
}
