#include "scalefunctions.h"

template<class T> void myNormalizeP(T *x, T *y)
{
  //cout <<"Pointer version of myNormalize called\n";
  
  T length;
  length = *x * *x + *y * *y;
  length = (T)sqrt(length);

  *x /= length;
  *y /= length;
}

template<class T> void myNormalize(T &x, T &y)
{
  //cout <<"Reference version of myNormalize called\n";
  T length;
  length = x * x + y * y;
  length = (T)sqrt(length);

  x /= length;
  y /= length;
}

template<class T> void myNormalizeP(T *x, T *y, T *z)
{
  //cout <<"Pointer version of myNormalize called\n";
  T length;
  length = *x * *x + *y * *y + *z * *z;
  length = (T)sqrt(length);

  *x /= length;
  *y /= length;
  *z /= length;
}

template<class T> void myNormalize(T &x, T &y, T &z)
{
  cout <<"Reference version of myNormalize called \n";
  T length;
  length = x * x + y * y + z * z;
  length = (T)sqrt(length);

  x /= length;
  y /= length;
  z /= length;
}

//  template<class T> void myNormalize(T x, T y, T z)
//  {
//    cout <<"Common version of myNormalize called\n";
//  }



template<class T> void scaleArray(T *vector, const int length,
                                  const T min, const T max)
{
  T oldmin, oldmax;//, scale;
  double scale = 1.0;
  minmax(vector, length, oldmin, oldmax);
  //oldrange = oldmax - oldmin;
  //newrange = max - min;
  //scale    = newrange/oldrange;
  if(oldmax==oldmin) { // there is only one value in the array
    cout <<"WARNING:In scalefunctions.h scaleArray(): oldmin==oldmax=="<<oldmax
         <<", so nothing to be done. \n\tReturning array with all elements set to zero\n";
    memset(vector, 0, length*sizeof(T));
    return;
  }
  
  scale      = double(max - min)/double(oldmax - oldmin);
  for(int i=0;i<length;i++){
    vector[i] = T(double(vector[i]-oldmin)*scale + min);
    
    if((vector[i]<min && (vector[i]-min)>0.01) ||
       (vector[i]>max && (max-vector[i])>0.01)){  // out of range values, 1% of tolerance
      cout <<"Values in scaleArray are out of range, even with 0.01 tolerance\n";
      //printf("\tPosition %d has value %f whereas min is %f and max is %f\n", i, vector[i], min, max);
      //printf("\toldmin is %f, oldmax is %f, scale is %f\n", oldmin, oldmax, scale);
    }
    
    //assert(vector[i]>=min);
    //assert(vector[i]<=max);
  }
}


template<class T> void fitVectorToVolume(T *vector, const int length,
                                         const T xmin, const T xmax,
                                         const T ymin, const T ymax,
                                         const T zmin = 0, const T zmax = 0)
{
    cout <<"ERROR:\tFunction fitVectorToVolume NOT YET FINISHED\n";
    // function is not working properly!
    
    int Dimension = 3;
    if(zmin == zmax && zmin == 0){
        Dimension = 2;
    }
    
    T _xmin, _xmax, _ymin, _ymax, _zmin, _zmax;
    getObjectBoundaries(vector, length, _xmin, _xmax, _ymin, _ymax, _zmin, _zmax);

    T _Xmove  = 0.0;
    T _Ymove  = 0.0;
    T _Zmove  = 0.0;
    
    T _Xscale = 1.0;
    T _Yscale = 1.0;
    T _Zscale = 1.0;

        
    _Xscale = (xmax - xmin)/(_xmax - _xmin);
    _Yscale = (ymax - ymin)/(_ymax - _ymin);
    _Zscale = (zmax - zmin)/(_zmax - _zmin);
    
    _Xmove  = _Xscale*(_xmax - _xmin)/2.0;
    _Ymove  = _Yscale*(_ymax - _ymin)/2.0;
    _Zmove  = _Zscale*(_zmax - _zmin)/2.0;
    cout <<"Some values: "<<vector[Dimension*0]
         <<", "<<vector[Dimension*0 +1]
         <<", "<<vector[Dimension*0 +2]<<endl;
    
#ifdef INFO
    cout <<"INFO:\tMinimum values found: ("<<_xmin<<", "<<_ymin<<", "<<_zmin<<")\n";
    cout <<"INFO:\tMaximum values found: ("<<_xmax<<", "<<_ymax<<", "<<_zmax<<")\n";
    cout <<"INFO:\tMove values found:    ("<<_Xmove<<", "<<_Ymove<<", "<<_Zmove<<")\n";
    cout <<"INFO:\tScale values found:   ("<<_Xscale<<", "<<_Yscale<<", "<<_Zscale<<")\n";
#endif
    for(int i=0; i<length/Dimension; i++){
        vector[Dimension*i]   = _Xscale*(vector[Dimension*i]   - _Xmove);
        vector[Dimension*i+1] = _Yscale*(vector[Dimension*i+1] - _Ymove);
        vector[Dimension*i+2] = _Zscale*(vector[Dimension*i+2] - _Ymove);
    }
}

template<class T> void fitVectorToArea(const T xmin, const T xmax,
                                       const T ymin, const T ymax,
                                       T *vector, const int length)
{
}

template<class T> void centerAndScaleObject(T *vector, const int length,
                                            const T range, const int Dimension=3)
{
    T _xmin, _xmax, _ymin, _ymax, _zmin, _zmax;
    getObjectBoundaries(vector, length, _xmin, _xmax, _ymin, _ymax, _zmin, _zmax, Dimension);
    centerObject(vector, length, _xmin, _xmax, _ymin, _ymax, _zmin, _zmax, Dimension);
    T _scale = scaleObject(vector, length, range, _xmin, _xmax, _ymin, _ymax, _zmin, _zmax, Dimension);
     
    for(int i=0; i<length/Dimension; i++){
        vector[Dimension*i]   = _scale*vector[Dimension*i];
        vector[Dimension*i+1] = _scale*vector[Dimension*i+1];
        if(Dimension == 3) {
            vector[Dimension*i+2] = _scale*vector[Dimension*i+2];
        }
    }
#ifdef INFO
    cout <<"INFO:\tIn centerAndScaleObject: _scale="<<_scale<<endl;
#endif   
}


template<class T> void scaleObject(T *vector, const int length,
                                   const T range, const int Dimension=3)
{
    T _xmin, _xmax, _ymin, _ymax, _zmin, _zmax;
    getObjectBoundaries(vector, length, _xmin, _xmax, _ymin, _ymax, _zmin, _zmax, Dimension);
    //T _scale = scaleObject(vector, length, range, _xmin, _xmax, _ymin, _ymax, _zmin, _zmax, Dimension);
    cout <<"ERROR:\tscaleObject NOT YET FINISHED\n";
    
}

template<class T> T scaleObject(T *vector, const int length, const T range, 
                                   const T xmin, const T xmax,
                                   const T ymin, const T ymax,
                                   const T zmin, const T zmax,
                                   const int Dimension=3)
{
    T _small  = xmax - xmin;
    T _medium = ymax - ymin;
    T _big    = zmax - zmin;
    sort(_small, _medium, _big);
    return range/_big;
}

template<class T> void centerObject(T *vector, const int length, const int Dimension=3)
{
    T _xmin, _xmax, _ymin, _ymax, _zmin, _zmax;
    getObjectBoundaries(vector, length, _xmin, _xmax, _ymin, _ymax, _zmin, _zmax, Dimension);
    centerObject(vector, length, _xmin, _xmax, _ymin, _ymax, _zmin, _zmax, Dimension);
}

template<class T> void centerObject(T *vector, const int length,
                                    const T xmin, const T xmax,
                                    const T ymin, const T ymax,
                                    const T zmin, const T zmax,
                                    const int Dimension=3)
{

    T _xmove = xmax - (xmax-xmin)/T(2.0);
    T _ymove = ymax - (ymax-ymin)/T(2.0);
    T _zmove = zmax - (zmax-zmin)/T(2.0);
    
    for(int i=0; i<length/Dimension; i++){
        vector[Dimension*i]   = vector[Dimension*i]   - _xmove;
        vector[Dimension*i+1] = vector[Dimension*i+1] - _ymove;
        if(Dimension == 3) {
            vector[Dimension*i+2] = vector[Dimension*i+2] - _zmove;
        }
    }
#ifdef INFO
    cout <<"INFO:\tIn centerObject: _xmove="<<_xmove<<" _ymove="<<_ymove<<" _zmove="<<_zmove<<endl;
#endif
}

template<class T> void getObjectBoundaries(T *vector, const int length,
                                           T &xmin, T &xmax,
                                           T &ymin, T &ymax,
                                           T &zmin, T &zmax,
                                           const int Dimension = 3)
{
#ifdef DEBUG
    if(Dimension == 2){
        cout <<"DEBUG:\tIn function getObjectBoundaries: Dimension is 2\n";
    }
#endif 
    
    xmin   = vector[0];
    xmax   = vector[0];//-(T)FLT_MAX;
    ymin   = vector[1];//(T)FLT_MAX;
    ymax   = vector[1];//-(T)FLT_MAX;
    zmin   = vector[2];//(T)FLT_MAX;
    zmax   = vector[2];//-(T)FLT_MAX;

/*
    cout <<"xmin "<<xmin<<" xmax "<<xmax
         <<"\nymin "<<ymin<<" ymax "<<ymax
         <<"\nzmin "<<zmin<<" zmax "<<zmax<<endl;
    cout <<"FLT_MIN    < 0 is " <<(FLT_MIN<0)<<endl;
    cout <<"-FLT_MAX   < 0 is " <<(float(-FLT_MAX)<0)<<endl;
    cout <<"-FLT_MAX-1 < 0 is " <<(float(-FLT_MAX-1)<0)<<endl;
    cout <<"int(FLT_MAX)  is " <<int(FLT_MAX)<<endl;
    cout <<"int(FLT_MIN)  is " <<int(FLT_MIN)<<endl;
*/
    
    for(int i=0; i<length/Dimension; i++){
        if(vector[Dimension*i]   < xmin) xmin = vector[Dimension*i];
        if(vector[Dimension*i+1] < ymin) ymin = vector[Dimension*i+1];

        if(Dimension == 3 && vector[Dimension*i+2] < zmin) zmin = vector[Dimension*i+2];

        
        if(vector[Dimension*i]   > xmax) xmax = vector[Dimension*i];
        if(vector[Dimension*i+1] > ymax) ymax = vector[Dimension*i+1];
        
        if(Dimension == 3 && vector[Dimension*i+2] > zmax) zmax = vector[Dimension*i+2];
    }
    
#ifdef INFO
    if(Dimension == 3){
        cout <<"INFO:\tMinimum values found: ("<<xmin<<", "<<ymin<<", "<<zmin<<")\n";
        cout <<"INFO:\tMaximum values found: ("<<xmax<<", "<<ymax<<", "<<zmax<<")\n";
    }
    else {
        cout <<"INFO:\tMinimum values found: ("<<xmin<<", "<<ymin<<")\n";
        cout <<"INFO:\tMaximum values found: ("<<xmax<<", "<<ymax<<")\n";
    }   
#endif
}

void dummyScalefunctions()
{

  int    i                 = 0;
  float  f                 = 0;
  double d                 = 0;
  short  s                 = 0;
  unsigned short us        = 0;
  int    *ivector          = new int [1];
  float  *fvector          = new float[1];
  double *dvector          = new double[1];
  short  *svector          = new short[1];
  unsigned short *usvector = new unsigned short[1];
  
  
  int    *ip               = &i;
  float  *fp               = &f;
  double *dp               = &d;
  short  *sp               = &s;
  unsigned short *usp      = &us;

  myNormalizeP(ip, ip);
  myNormalize(i, i);
  myNormalizeP(ip, ip, ip);
  myNormalize(i, i, i);
  scaleArray(ivector, 1, i, i);
  fitVectorToArea(i, i, i, i, ivector, 1);
  centerAndScaleObject(ivector, 1,  i,  3);
  scaleObject(ivector,  1,  i,  3);
  scaleObject(ivector,  1,  i,  i,  i,  i,  i, i,  i, 3);
  centerObject(ivector,  1,  3);
  centerObject(ivector,  1,  i,  i,  i,  i,  i,  i,  3);
  getObjectBoundaries(ivector,  1, i, i,  i, i, i, i, 3);
  
  myNormalizeP(fp, fp);
  myNormalize(f, f);
  myNormalizeP(fp, fp, fp);
  myNormalize(f, f, f);
  scaleArray(fvector, 1, f, f);
  fitVectorToArea(f, f, f, f, fvector, 1);
  centerAndScaleObject(fvector, 1,  f,  3);
  scaleObject(fvector,  1,  f,  3);
  scaleObject(fvector,  1,  f,  f,  f,  f,  f, f,  f, 3);
  centerObject(fvector,  1,  3);
  centerObject(fvector,  1,  f,  f,  f,  f,  f,  f,  3);
  getObjectBoundaries(fvector,  1, f, f,  f, f, f, f, 3);
  
  myNormalizeP(dp, dp);
  myNormalize(d, d);
  myNormalizeP(dp, dp, dp);
  myNormalize(d, d, d);
  scaleArray(dvector, 1, d, d);
  fitVectorToArea(d, d, d, d, dvector, 1);
  centerAndScaleObject(dvector, 1,  d,  3);
  scaleObject(dvector,  1,  d,  3);
  scaleObject(dvector,  1,  d,  d,  d,  d,  d, d,  d, 3);
  centerObject(dvector,  1,  3);
  centerObject(dvector,  1,  d,  d,  d,  d,  d,  d,  3);
  getObjectBoundaries(dvector,  1, d, d,  d, d, d, d, 3);
  
  myNormalizeP(sp, sp);
  myNormalize(s, s);
  myNormalizeP(sp, sp, sp);
  myNormalize(s, s, s);
  scaleArray(svector, 1, s, s);
  fitVectorToArea(s, s, s, s, svector, 1);
  centerAndScaleObject(svector, 1,  s,  3);
  scaleObject(svector,  1,  s,  3);
  scaleObject(svector,  1,  s,  s,  s,  s,  s, s,  s, 3);
  centerObject(svector,  1,  3);
  centerObject(svector,  1,  s,  s,  s,  s,  s,  s,  3);
  getObjectBoundaries(svector,  1, s, s,  s, s, s, s, 3);
  
  myNormalizeP(usp, usp);
  myNormalize(us, us);
  myNormalizeP(usp, usp, usp);
  myNormalize(us, us, us);
  scaleArray(usvector, 1, us, us);
  fitVectorToArea(us, us, us, us, usvector, 1);
  centerAndScaleObject(usvector, 1,  us,  3);
  scaleObject(usvector,  1,  us,  3);
  scaleObject(usvector,  1,  us,  us,  us,  us,  us, us,  us, 3);
  centerObject(usvector,  1,  3);
  centerObject(usvector,  1,  us,  us,  us,  us,  us,  us,  3);
  getObjectBoundaries(usvector,  1, us, us,  us, us, us, us, 3);

  // so the optimizer does not delete this function:
  cout <<i<<s<<f<<d<<us<<ip<<sp<<fp<<dp<<usp<<endl;
  
  
  delete[] ivector;
  delete[] fvector;
  delete[] dvector;
  delete[] svector;
  delete[] usvector;

  delete ip;
  delete fp;
  delete dp;
  delete sp;
  delete usp;
  
}