#include <math.h>
#include "cmds.h"

#include <stdlib.h>

//--- CMDS ---//

CMDS::CMDS(DataFile &MDfile, DataFile &OUTfile)
  {
  this->MDfile=&MDfile;
  this->OUTfile=&OUTfile;
  this->Dist=false;
  }

CMDS::~CMDS()
  {
  this->MDfile=this->OUTfile=0;
  }

//--- Uruchamia klasyczne skalowanie jesli macierz wejsciowa ma co najmniej dwie kolumny ---//
double CMDS::ClassicalScaling(bool FullPrecisionEigensystem, unsigned int NumberOfIterations, bool Dist)
  {
  if (MDfile->rData().ncols()>=2)
    {
    this->Dist=Dist;
    this->NumberOfObjects=MDfile->rData().nrows();
    Matrix *tmp=new Matrix(NumberOfObjects,2,0,2);
    OUTfile->Combine(*MDfile,*tmp);
    delete tmp;
    OUTfile->ReplaceFeaturesNames();
    OUTfile->AddComment("Classical Scaling");
    this->FullPrecisionEigensystem=FullPrecisionEigensystem;
    this->NumberOfIterations=NumberOfIterations;
    cmds();
    return this->Accuracy();
    }
  return 0;
  }

//--- Dokladnosc jako sredni kwadrat roznicy kwadratow odleglosci ---//
//--- Nie jest to funkcja strat, za duzy naklad pracy a chodzi o to samo ---//
double CMDS::Accuracy()
  {
  double sum=0;
  double tmp;
  for (unsigned int i=0; i < NumberOfObjects; i++)
    {
    for (unsigned int j=i+1; j < NumberOfObjects; j++)
      {
      tmp=Dist2(MDfile->rData(),i,j)-Dist2(OUTfile->rData(),i,j);
      sum+=tmp*tmp;
      }
    }
  sum/=NumberOfObjects*(NumberOfObjects-1)/2;
  return sum;
  }

//--- Oblicza kwadrat roznicy odleglosci dla obiektow i i j ---//
double CMDS::Dist2(const Matrix & m, unsigned int i, unsigned int j)
  {
  double sum=0, tmp;
  double *wsk_i1=m[i];
  double *wsk_i2=wsk_i1+m.ncols();
  double *wsk_j1=m[j];
  for (; wsk_i1 < wsk_i2; wsk_i1++, wsk_j1++)
    {
    tmp=(*wsk_i1)-(*wsk_j1);
    sum+=tmp*tmp;
    }
  return sum;
  }

//--- Funkcja zarzadzajaca klasycznym skalowaniem ---//  
void CMDS::cmds()
  {
  Matrix *B=new Matrix(NumberOfObjects,NumberOfObjects,0,2);

  Matrix *Vnew=new Matrix(NumberOfObjects,1,0,2);

  double w_new=0;

  if (this->Dist == false)
    MDdataToB(*B);
    else
      {
      this->DistToB(*B);
      //Matrix J(Matrix(MDfile->rData().nrows(),MDfile->rData().nrows(),1,1)-Matrix(MDfile->rData().nrows(),MDfile->rData().nrows(),1.0/MDfile->rData().nrows()));
      //*B=J*(MDfile->rData())*J*(-0.5);
      }

  Matrix *Q=new Matrix(NumberOfObjects,2,0,2);
  Matrix *L=new Matrix(2,2,0,0);
  unsigned int k=0;
  unsigned int nw=0;
  do
    {
    EigenSystem(*B,w_new,Vnew);

    for (unsigned int i=0; i < NumberOfObjects; i++)
      for (unsigned int j=i; j < NumberOfObjects; j++)
        (*B)[j][i]=(*B)[i][j]-=w_new*((*Vnew)[i][0]*(*Vnew)[j][0]);

    if (w_new > 0)
      {
      for (unsigned int i=0; i < NumberOfObjects; i++)
        (*Q)[i][nw]=(*Vnew)[i][0];
      (*L)[nw][nw]=sqrt(w_new);
      nw++;
      }

    k++;
    }
    while ((k < NumberOfObjects) && (nw < 2));

  delete B;
  delete Vnew;

  OUTfile->rData()=(*Q)*(*L);
  delete Q,L;
  }

//--- Szybkie mnozenie macierzy przez wektor ---//
void CMDS::MultiplyMxV(const Matrix & M, const Matrix & V, Matrix & MxV)
  {
  double * *wski1_M=M.pmatrix();
  double *wskj1_M=0;
  double *wskj2_M=0;

  double * *wsk1_V=V.pmatrix();;
  double * *wsk2_V=0;

  double * *wsk1_MxV=MxV.pmatrix();
  double * *wsk2_MxV=wsk1_MxV+NumberOfObjects;

  double sum;
  for (; wsk1_MxV < wsk2_MxV; wsk1_MxV++, wski1_M++)
    {
    sum=0;
    for (wskj1_M=*wski1_M, wskj2_M=wskj1_M+NumberOfObjects, wsk2_V=wsk1_V; wskj1_M < wskj2_M; wskj1_M++, wsk2_V++)
      sum+=*wskj1_M * **wsk2_V;
    **wsk1_MxV=sum;
    }
  }

//--- Tworzy wycentrowana macierz kwadratow odleglosci ze wspolrzednych ---//
void CMDS::MDdataToB(Matrix & B)
  {
  double Jneq=-1.0/NumberOfObjects;

  Matrix *SD=new Matrix(NumberOfObjects,1,0,2);

  double sum;
  double SSD=0;
  double tmp;
  for (unsigned int i=0; i < NumberOfObjects; i++)
    {
    sum=0;
    for (unsigned int k=0; k < NumberOfObjects; k++)
      {
      sum+=Dist2(MDfile->rData(),k,i);
      }
    tmp=sum*Jneq;
    (*SD)[i][0]=tmp;
    SSD+=tmp;
    }
  SSD*=Jneq;

  double **wsk_SD=SD->pmatrix();
  for (unsigned int i=0; i < NumberOfObjects; i++)
    {
    for (unsigned int j=i; j < NumberOfObjects; j++)
      {
      B[j][i]=B[i][j]=-0.5*(SSD+(**(wsk_SD+i))+(**(wsk_SD+j))+Dist2(MDfile->rData(),i,j));
      }
    }
  delete SD;
  }

//--- Tworzy wycentrowana macierz kwadratow odleglosci z odleglosci ---//
void CMDS::DistToB(Matrix & B)
  {
  double Jneq=-1.0/NumberOfObjects;

  Matrix *SD=new Matrix(NumberOfObjects,1,0,2);

  double sum;
  double SSD=0;
  double tmp;
  for (unsigned int i=0; i < NumberOfObjects; i++)
    {
    sum=0;
    for (unsigned int k=0; k < NumberOfObjects; k++)
      {
      sum+=SQR(MDfile->rData()[k][i]);
      }
    tmp=sum*Jneq;
    (*SD)[i][0]=tmp;
    SSD+=tmp;
    }
  SSD*=Jneq;

  double **wsk_SD=SD->pmatrix();
  for (unsigned int i=0; i < NumberOfObjects; i++)
    {
    for (unsigned int j=i; j < NumberOfObjects; j++)
      {
      B[j][i]=B[i][j]=-0.5*(SSD+(**(wsk_SD+i))+(**(wsk_SD+j))+SQR(MDfile->rData()[i][j]));
      }
    }
  delete SD;
  }

//--- Wylicza wektory i wartosci wlasne ---//
double CMDS::EigenSystem(const Matrix & B, double & EVal, Matrix * EVec)
  {
  double **wsk1=EVec->pmatrix();
  double **wsk2=wsk1+NumberOfObjects;
  for (; wsk1 < wsk2; wsk1++)
    **wsk1=rand()%10000;

  Matrix *V=new Matrix(NumberOfObjects,1,0,2);
  Matrix *tmp=0;
  double w=1;
  EVal=0;

  if (FullPrecisionEigensystem)
    for (unsigned int i=0; (i < NumberOfIterations) && ((EVal-w!=0) || ((*EVec)[0][0]-(*V)[0][0]!=0)); i++)
      {
      tmp=V;
      V=EVec;
      EVec=tmp;
      tmp=0;
      MultiplyMxV(B,*V,*EVec);
      w=EVal;
      EVal=(EVec->Transpose()*(*V))[0][0]/(V->Transpose()*(*V))[0][0];
      (*EVec)*=(1/sqrt((EVec->Transpose()*(*EVec))[0][0]));
      }
    else
      for (unsigned int i=0; i < NumberOfIterations; i++)
        {
        tmp=V;
        V=EVec;
        EVec=tmp;
        tmp=0;
        MultiplyMxV(B,*V,*EVec);
        w=EVal;
        EVal=(EVec->Transpose()*(*V))[0][0]/(V->Transpose()*(*V))[0][0];
        (*EVec)*=(1/sqrt((EVec->Transpose()*(*EVec))[0][0]));
        }

  return EVal-w;
  }

DataFile * CMDS::pMDfile()
  {
  return MDfile;
  }

DataFile * CMDS::pOUTfile()
  {
  return OUTfile;
  }
