/*=========================================================================
  Program:   vv                     http://www.creatis.insa-lyon.fr/rio/vv

  Authors belong to:
  - University of LYON              http://www.universite-lyon.fr/
  - Léon Bérard cancer center       http://www.centreleonberard.fr
  - CREATIS CNRS laboratory         http://www.creatis.insa-lyon.fr

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the copyright notices for more information.

  It is distributed under dual licence

  - BSD        See included LICENSE.txt file
  - CeCILL-B   http://www.cecill.info/licences/Licence_CeCILL-B_V1-en.html
===========================================================================**/

#ifndef clitkElastix_h
#define clitkElastix_h

#include <itkEuler3DTransform.h>
#include <iterator>

//--------------------------------------------------------------------
namespace clitk {

//-------------------------------------------------------------------
bool
GetElastixValueFromTag(std::ifstream & is,
                       std::string tag,
                       std::string & value)
{
  std::string line;
  is.seekg (0, is.beg);
  while(std::getline(is, line))   {
    unsigned pos = line.find(tag);
    if (pos<line.size()) {
      value=line.substr(pos+tag.size(),line.size()-2);// remove last ')'
      value.erase (std::remove (value.begin(), value.end(), '"'), value.end());
      value.erase (std::remove (value.begin(), value.end(), ')'), value.end());
      return true;
    }
  }
  return false;
}
//-------------------------------------------------------------------


//-------------------------------------------------------------------
void
GetValuesFromValue(const std::string & s,
                   std::vector<std::string> & values)
{
  std::stringstream strstr(s);
  std::istream_iterator<std::string> it(strstr);
  std::istream_iterator<std::string> end;
  std::vector<std::string> results(it, end);
  values.clear();
  values.resize(results.size());
  for(uint i=0; i<results.size(); i++) values[i] = results[i];
}
//-------------------------------------------------------------------


//-------------------------------------------------------------------
template<unsigned int Dimension>
typename itk::Matrix<double, Dimension+1, Dimension+1>
createMatrixFromElastixFile(std::string& filename, bool verbose=true) {
  if (Dimension != 3) {
    FATAL("Only 3D yet" << std::endl);
  }
  typename itk::Matrix<double, Dimension+1, Dimension+1> matrix, init;

  itk::Euler3DTransform<double>::Pointer mat = itk::Euler3DTransform<double>::New();
  itk::Euler3DTransform<double>::Pointer previous;

  // Open file
  if (verbose) std::cout << "Read elastix parameters in " << filename << std::endl;
  std::ifstream is;
  clitk::openFileForReading(is, filename);

  // Check Transform
  std::string s;
  bool b = GetElastixValueFromTag(is, "Transform ", s);
  if (!b) {
    FATAL("Error must read 'Transform' in " << filename << std::endl);
  }
  if (s != "EulerTransform") {
    FATAL("Sorry only 'EulerTransform'" << std::endl);
  }

  // Get previous
  b = GetElastixValueFromTag(is, "InitialTransformParametersFileName ", s);
  if(s == "NoInitialTransform")
    init.SetIdentity();
  else
    init = createMatrixFromElastixFile<Dimension>(s, verbose);

  // Get CenterOfRotationPoint
  GetElastixValueFromTag(is, "CenterOfRotationPoint ", s); // space is needed
  if (!b) {
    FATAL("Error must read 'CenterOfRotationPoint' in " << filename << std::endl);
  }
  std::vector<std::string> cor;
  GetValuesFromValue(s, cor);
  itk::Euler3DTransform<double>::CenterType c;
  for(uint i=0; i<3; i++)
    c[i] = atof(cor[i].c_str());
  mat->SetCenter(c);

  // Get Transformparameters
  GetElastixValueFromTag(is, "ComputeZYX ", s); // space is needed
  mat->SetComputeZYX( s==std::string("true") );

  // Get Transformparameters
  GetElastixValueFromTag(is, "TransformParameters ", s); // space is needed
  if (!b) {
    FATAL("Error must read 'TransformParameters' in " << filename << std::endl);
  }
  std::vector<std::string> results;
  GetValuesFromValue(s, results);

  // construct a stream from the string
  itk::Euler3DTransform<double>::ParametersType p;
  p.SetSize(6);
  for(uint i=0; i<3; i++)
    p[i] = atof(results[i].c_str()); // Rotation
  for(uint i=0; i<3; i++)
    p[i+3] = atof(results[i+3].c_str()); // Translation
  mat->SetParameters(p);

  if (verbose) {
    std::cout << "Rotation      (deg) : " << rad2deg(p[0]) << " " << rad2deg(p[1]) << " " << rad2deg(p[2]) << std::endl;
    std::cout << "Center of rot (phy) : " << c[0] << " " << c[1] << " " << c[2] << std::endl;
    std::cout << "Translation   (phy) : " << p[3] << " " << p[4] << " " << p[5] << std::endl;
  }

  previous = itk::Euler3DTransform<double>::New();
  previous->SetParameters(mat->GetParameters());
  previous->SetCenter(c);
  previous->SetComputeZYX(mat->GetComputeZYX());

  mat = previous;
  for(uint i=0; i<3; i++)
    for(uint j=0; j<3; j++)
      matrix[i][j] = mat->GetMatrix()[i][j];
  // Offset is -Rc + t + c
  matrix[0][3] = mat->GetOffset()[0];
  matrix[1][3] = mat->GetOffset()[1];
  matrix[2][3] = mat->GetOffset()[2];
  matrix[3][3] = 1;

  return matrix*init;
}
}
//-------------------------------------------------------------------

#endif