/*
 * 
 * g++ -Wall -O3 -g umeyama.cc -lgsl -lgslcblas -lblitz -o umeyama -lhdf5
 * 
 */

#include "common.h"


/*======================================================================*/
/*! 
 *   computes the norm of vector 
 *
 *   \param a  vector
 *
 *   \exception none
 *
 *   \return norm of vector
 */
/*======================================================================*/
template< typename T, int I>
inline
T mynorm(const blitz::TinyVector<T,I>& a)
{
  return sqrt(blitz::dot(a,a));
}


int main( int argc, char** argv ){

  std::string fixedFileName="zebrafish0.h5";
  std::string movingFileName="zebrafish1.h5";
  std::string resultFileName="result.h5";
    
  //read element size
  std::cout<<"read element size"<<std::endl;
  blitz::TinyVector<float,3> element_size_fixed_um,element_size_moving_um;
  readBlitzTinyVectorFromHDF5Attribute(element_size_fixed_um,"element_size_um","/channel0",fixedFileName);
  readBlitzTinyVectorFromHDF5Attribute(element_size_moving_um,"element_size_um","/channel0",movingFileName);
  std::cout<<element_size_fixed_um<<std::endl;
  
  //read images
  blitz::Array<float, 3> fixedImage, movingImage;
  std::cout<<"read images"<<std::endl;
  readHDF5toBlitz(fixedFileName,"/channel0",fixedImage);
  readHDF5toBlitz(movingFileName,"/channel0",movingImage);
  
  //read labels
  std::cout<<"read landmarks"<<std::endl;
  blitz::Array<blitz::TinyVector<double,3>,1> fixedPos_um, movingPos_um;
  readHDF5toBlitz(fixedFileName,"landmarks_um",fixedPos_um);
  readHDF5toBlitz(movingFileName,"landmarks_um",movingPos_um);
  blitz::Array<blitz::TinyVector<float,4>,1> fixedLandmark(fixedPos_um.extent(0));
  blitz::Array<blitz::TinyVector<float,4>,1> movingLandmark(movingPos_um.extent(0));
  
  //mark landmarks
  markLandmark( fixedImage, fixedPos_um, element_size_fixed_um);
  markLandmark( movingImage, movingPos_um, element_size_moving_um);
  blitz::Array<blitz::TinyVector<float,3>,3 > rgbImage=overlay(fixedImage, movingImage);
  
  //write overlay original
  std::cout<<"overlay"<<std::endl;
  std::cout<<" size "<<rgbImage.shape()<<std::endl;   
  writeBlitzToHDF5(rgbImage,"original",resultFileName);
  writeBlitzTinyVectorToHDF5Attribute(element_size_fixed_um,"element_size_um","/original",resultFileName);
  
  //point correspondences before umeyama in homogenous coordinates
  std::cout<<"Points before umeyama"<<std::endl;
  float distance=0.0;
  for(int i=0;i<fixedPos_um.extent(0);++i){
    blitz::TinyVector<float,4> tempFixed, tempMoving;
    blitz::TinyVector<double,3> fixedPoint=fixedPos_um(i);
    blitz::TinyVector<double,3> movingPoint=movingPos_um(i);
    tempFixed=(float)fixedPoint(0),(float)fixedPoint(1),(float)fixedPoint(2),(float)1;
    tempMoving=movingPoint(0),movingPoint(1),movingPoint(2),1;
    fixedLandmark(i)=tempFixed;
    movingLandmark(i)=tempMoving;
    std::cout<<fixedLandmark(i)<<" -> "<<movingLandmark(i)<<std::endl;
    distance+=mynorm(blitz::TinyVector<float,4>(movingLandmark(i)-fixedLandmark(i)));
  }
  
  std::cout<<"Distance before umeyama "<<distance<<std::endl;
  
  //umeyama
  blitz::TinyMatrix<float,4,4> estimatedMat;
  estimatedMat = umeyama( fixedLandmark, movingLandmark);
  //scaling matrices
  blitz::TinyMatrix<float,4,4> voxel_to_mu, mu_to_voxel, transMatrix;
  voxel_to_mu=0.0;
  mu_to_voxel=0.0;
  voxel_to_mu(0,0)=element_size_moving_um(0);
  voxel_to_mu(1,1)=element_size_moving_um(1);
  voxel_to_mu(2,2)=element_size_moving_um(2);
  voxel_to_mu(3,3)=1.0;

  mu_to_voxel(0,0)=1.0/element_size_fixed_um(0);
  mu_to_voxel(1,1)=1.0/element_size_fixed_um(1);
  mu_to_voxel(2,2)=1.0/element_size_fixed_um(2);
  mu_to_voxel(3,3)=1.0;

  transMatrix=voxel_to_mu;
  transMatrix=myproduct(estimatedMat, transMatrix);
  transMatrix=myproduct(mu_to_voxel, transMatrix);

  //point correspondences after umeyama
  distance=0.0;
  for(int i=0;i<fixedPos_um.extent(0);++i){
    blitz::TinyVector<float,4> transformedLandmark;
    transformedLandmark=myproduct(estimatedMat, fixedLandmark(i));
    std::cout<<fixedLandmark(i)<<" || "<<movingLandmark(i)<<" -> "<<transformedLandmark<<std::endl;
    distance+=mynorm(blitz::TinyVector<float,4>(movingLandmark(i)-transformedLandmark));
  }
  std::cout<<"Distance after umeyama "<<distance<<std::endl;

  //transform image
  blitz::Array<float,3> transformedImage(fixedImage.shape());
  transformArray( movingImage, transMatrix, transformedImage);
  
  //overlay
  blitz::Array<blitz::TinyVector<float,3>,3 > rgbImage2=overlay(fixedImage, transformedImage);
  
  //write result
  writeBlitzToHDF5(rgbImage2,"umeyama",resultFileName);
  writeBlitzTinyVectorToHDF5Attribute(element_size_fixed_um,"element_size_um","/umeyama",resultFileName);
  
  std::cout<<"End"<<std::endl;
}