#include "proc.h"
//--- Procrust ---//
double Procrust::pythag(const double a, const double b)
  {
  double absa,absb;
  double tmp;

  absa=fabs(a);
  absb=fabs(b);
  if (absa > absb)
    {
    tmp=absb/absa;
    return absa*sqrt(1.0+tmp*tmp);
    }
    else
      {
      tmp=absa/absb;
      return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+tmp*tmp));
      }
  }

void Procrust::svdcmp(Matrix &a, Matrix &w, Matrix &v)
  {
  bool flag;
  int i,its,j,jj,k,l,nm;
  //DP anorm,c,f,g,h,s,scale,x,y,z;
  double anorm,c,f,g,h,s,scale,x,y,z;

  int m=a.nrows();
  int n=a.ncols();
  //Vec_DP rv1(n);
  Matrix rv1(n,n);
  g=scale=anorm=0.0;
  for (i=0;i<n;i++)
    {
    l=i+2;
    //rv1[i]=scale*g;
    rv1[i][i]=scale*g;
    g=s=scale=0.0;
    if (i < m)
      {
      for (k=i;k<m;k++) scale += fabs(a[k][i]);
      if (scale != 0.0)
        {
        for (k=i;k<m;k++)
          {
          a[k][i] /= scale;
          s += a[k][i]*a[k][i];
          }
        f=a[i][i];
        g = -SIGN(sqrt(s),f);
        h=f*g-s;
        a[i][i]=f-g;
        for (j=l-1;j<n;j++)
          {
          for (s=0.0,k=i;k<m;k++) s += a[k][i]*a[k][j];
          f=s/h;
          for (k=i;k<m;k++) a[k][j] += f*a[k][i];
          }
        for (k=i;k<m;k++) a[k][i] *= scale;
        }
      }
    //w[i]=scale *g;
    w[i][i]=scale *g;
    g=s=scale=0.0;
    if (i+1 <= m && i != n)
      {
      for (k=l-1;k<n;k++) scale += fabs(a[i][k]);
      if (scale != 0.0)
        {
        for (k=l-1;k<n;k++)
          {
          a[i][k] /= scale;
          s += a[i][k]*a[i][k];
          }
        f=a[i][l-1];
        g = -SIGN(sqrt(s),f);
        h=f*g-s;
        a[i][l-1]=f-g;
        //for (k=l-1;k<n;k++) rv1[k]=a[i][k]/h;
        for (k=l-1;k<n;k++) rv1[k][k]=a[i][k]/h;
        for (j=l-1;j<m;j++)
          {
          for (s=0.0,k=l-1;k<n;k++) s += a[j][k]*a[i][k];
          //for (k=l-1;k<n;k++) a[j][k] += s*rv1[k];
          for (k=l-1;k<n;k++) a[j][k] += s*rv1[k][k];
          }
        for (k=l-1;k<n;k++) a[i][k] *= scale;
        }
      }
    //anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
    anorm=MAX(anorm,(fabs(w[i][i])+fabs(rv1[i][i])));
    }
  for (i=n-1;i>=0;i--)
    {
    if (i < n-1)
      {
      if (g != 0.0)
        {
        for (j=l;j<n;j++) v[j][i]=(a[i][j]/a[i][l])/g;
        for (j=l;j<n;j++)
          {
          for (s=0.0,k=l;k<n;k++) s += a[i][k]*v[k][j];
          for (k=l;k<n;k++) v[k][j] += s*v[k][i];
          }
         }
      for (j=l;j<n;j++) v[i][j]=v[j][i]=0.0;
      }
    v[i][i]=1.0;
    //g=rv1[i];
    g=rv1[i][i];
    l=i;
    }
  for (i=MIN(m,n)-1;i>=0;i--)
    {
    l=i+1;
    //g=w[i];
    g=w[i][i];
    for (j=l;j<n;j++) a[i][j]=0.0;
    if (g != 0.0)
      {
      g=1.0/g;
      for (j=l;j<n;j++)
        {
        for (s=0.0,k=l;k<m;k++) s += a[k][i]*a[k][j];
        f=(s/a[i][i])*g;
        for (k=i;k<m;k++) a[k][j] += f*a[k][i];
        }
      for (j=i;j<m;j++) a[j][i] *= g;
      }
      else
        for (j=i;j<m;j++) a[j][i]=0.0;
    ++a[i][i];
    }
  for (k=n-1;k>=0;k--)
    {
    for (its=0;its<30;its++)
      {
      flag=true;
      for (l=k;l>=0;l--)
        {
        nm=l-1;
        //if (fabs(rv1[l])+anorm == anorm) {
        if (fabs(rv1[l][l])+anorm == anorm)
          {
          flag=false;
          break;
          }
        //if (fabs(w[nm])+anorm == anorm) break;
        if (fabs(w[nm][nm])+anorm == anorm) break;
        }
      if (flag)
        {
        c=0.0;
        s=1.0;
        for (i=l-1;i<k+1;i++)
          {
          //f=s*rv1[i];
          f=s*rv1[i][i];
          //rv1[i]=c*rv1[i];
          rv1[i][i]=c*rv1[i][i];
          if (fabs(f)+anorm == anorm) break;
          //g=w[i];
          g=w[i][i];
          h=pythag(f,g);
          //w[i]=h;
          w[i][i]=h;
          h=1.0/h;
          c=g*h;
          s = -f*h;
          for (j=0;j<m;j++)
            {
            y=a[j][nm];
            z=a[j][i];
            a[j][nm]=y*c+z*s;
            a[j][i]=z*c-y*s;
            }
          }
        }
      //z=w[k];
      z=w[k][k];
      if (l == k)
        {
        if (z < 0.0)
          {
          //w[k] = -z;
          w[k][k] = -z;
          for (j=0;j<n;j++) v[j][k] = -v[j][k];
          }
        break;
        }
      //if (its == 29) nrerror("no convergence in 30 svdcmp iterations");
      if (its == 29) ProcError="no convergence in 30 svdcmp iterations";
      //x=w[l];
      x=w[l][l];
      nm=k-1;
      //y=w[nm];
      y=w[nm][nm];
      //g=rv1[nm];
      g=rv1[nm][nm];
      //h=rv1[k];
      h=rv1[k][k];
      f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
      g=pythag(f,1.0);
      f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
      c=s=1.0;
      for (j=l;j<=nm;j++)
        {
        i=j+1;
        //g=rv1[i];
        g=rv1[i][i];
        //y=w[i];
        y=w[i][i];
        h=s*g;
        g=c*g;
        z=pythag(f,h);
        //rv1[j]=z;
        rv1[j][j]=z;
        c=f/z;
        s=h/z;
        f=x*c+g*s;
        g=g*c-x*s;
        h=y*s;
        y *= c;
        for (jj=0;jj<n;jj++)
          {
          x=v[jj][j];
          z=v[jj][i];
          v[jj][j]=x*c+z*s;
          v[jj][i]=z*c-x*s;
          }
        z=pythag(f,h);
        //w[j]=z;
        w[j][j]=z;
        if (z)
          {
          z=1.0/z;
          c=f*z;
          s=h*z;
          }
        f=c*g+s*y;
        x=c*y-s*g;
        for (jj=0;jj<m;jj++)
          {
          y=a[jj][j];
          z=a[jj][i];
          a[jj][j]=y*c+z*s;
          a[jj][i]=z*c-y*s;
          }
        }
      //rv1[l]=0.0;
      rv1[l][l]=0.0;
      //rv1[k]=f;
      rv1[k][k]=f;
      //w[k]=x;
      w[k][k]=x;
      }
    }
  }

//--- Symuluje elementy macierzy centrujacej ----//
double Procrust::SimJ(unsigned int n, unsigned int row, unsigned int col)
  {
  if ((row < n) && (col < n))
    if (row == col)
      return 1-1.0/n;
      else
        return -1.0/n;
  return 0;
  }

//--- Centruje macierz bez tworzenia macierzy centrujacej ---//
Matrix Procrust::center_matrix(const Matrix & m)
  {
  unsigned int n=m.nrows();
  unsigned int col=m.ncols();
  Matrix out(n,col,0,3);
  double tmp;
  for (unsigned int i=0; i < n; i++)
    for (unsigned int j=0; j < col; j++)
      {
      tmp=0;
      for (unsigned int k=0; k < n; k++)
        {
        tmp+=SimJ(n,i,k)*m[k][j];
        }
      out[i][j]=tmp;
      }
  return out;
  }

//--- Wylicza przeksztalcenia i transformuje Y do X ---//
void Procrust::proc(const Matrix & Y, const Matrix & X, Matrix & YtoX)
  {
  //Matrix J(Matrix(X.nrows(),Y.nrows(),1,1)-Matrix(X.nrows(),Y.nrows(),1.0/X.nrows()));
  //Matrix C(X.Transpose()*(J*Y));
  Matrix C(X.Transpose()*center_matrix(Y));
  Matrix P(C);
  Matrix Phi(P.ncols(),P.ncols()), Q(P.ncols(),P.ncols());
  svdcmp(P,Phi,Q);
  if (ProcError=="")
    {
    Matrix T(Q*P.Transpose());
    //double s=(X.Transpose()*(J*(Y*T))).Trace()/((Y.Transpose()*(J*Y)).Trace());
    double s=(X.Transpose()*center_matrix(Y*T)).Trace()/((Y.Transpose()*center_matrix(Y)).Trace());
    Matrix t(1.0/X.nrows()*((X-Y*(T*s)).Transpose()*Matrix(X.nrows(),1,1)));

    YtoX=Y*T*s+Matrix(X.nrows(),1,1)*t.Transpose();
    }
  }

void Procrust::proc_accuracy()
  {
  if (ProcError=="OK")
    {
    Matrix *tmp=new Matrix(Destination->rData()-Target->rData());
    this->Accuracy=((tmp->Transpose())*(*tmp)).Trace()/tmp->nrows();
    delete tmp;
    }
    else
      this->Accuracy=0;
  }

Procrust::Procrust(DataFile &Testee, DataFile &Target, DataFile &Destination)
  {
  this->Testee=&Testee;
  this->Target=&Target;
  this->Destination=&Destination;
  ProcError="";
  Accuracy=0;
  }

Procrust::~Procrust()
  {
  this->Testee=this->Target=this->Destination=0;
  }

double Procrust::ProcrustesAnalysis()
  {
  ProcError="";
  this->Accuracy=0;
  if ((this->Testee!=0) && (this->Target!=0) && (this->Destination!=0) && (this->Testee->rData().nrows()==this->Target->rData().nrows()) && (this->Testee->rData().ncols()==this->Target->rData().ncols()))
    {
    Matrix * dest=new Matrix(this->Testee->rData().nrows(),this->Testee->rData().ncols(),0,2);
    Destination->Combine(*Testee,*dest);
    delete dest;
    Destination->AddComment("Procrustes Analysis");

    proc(this->Testee->rData(),this->Target->rData(),this->Destination->rData());
    if (ProcError=="")
      ProcError="OK";
    proc_accuracy();
    }
    else
      {
      //ProcError="Error";
      }
  return this->Accuracy;
  }

DataFile * Procrust::pTestee()
  {
  return Testee;
  }

DataFile * Procrust::pTarget()
  {
  return Target;
  }

DataFile * Procrust::pDestination()
  {
  return Destination;
  }
