// Definitions of PointList classes used by FNIRT
//
// point_list.cpp
//
// Jesper Andersson, FMRIB Image Analysis Group
//
// Copyright (C) 2008 University of Oxford 
//

#include <string>
#include <fstream>
#include <iostream>
#include "newmat.h"
#include "newmatio.h"

#ifndef EXPOSE_TREACHEROUS
#define I_DEFINED_ET
#define EXPOSE_TREACHEROUS
#endif

#include "newimage/newimageall.h"
#include "miscmaths/miscmaths.h"
#include "meshclass/meshclass.h"
#include "point_list.h"

using namespace std;
using namespace NEWMAT;
using namespace NEWIMAGE;
using namespace mesh;

PointList::PointList(const std::string& pfname, const std::string& ifname) : _pfname(pfname), _ifname(ifname) 
{
  _affine = IdentityMatrix(4);

  // First let us read image file
  volume<float>  vol;
  try { read_volume_hdr_only(vol,ifname); }
  catch (...) { throw PointListException(string("PointList: Can not read file: ")+ifname); }
  _vox2mm = vol.sampling_mat();
  if (vol.sform_code() != NIFTI_XFORM_UNKNOWN) _SQform = vol.sform_mat();
  else if (vol.qform_code() != NIFTI_XFORM_UNKNOWN) _SQform = vol.qform_mat();
  else _SQform = IdentityMatrix(4);

  if (is_maybe_vtk(pfname)) {
    if (PointList::read_first_file(pfname)<0) {
      throw PointListException(string("PointList: Can not read vtk-file: ")+pfname);
    }
  }
  else { // Assume text file
    if (PointList::read_ascii_file(pfname)<0) {
      throw PointListException(string("PointList: Can not read text-file: ")+pfname);
    }
  }  
}

NEWMAT::ReturnMatrix PointList::Point(unsigned int indx, bool one_ext) const
{
  if (indx >= NPoints()) throw PointListException("Point: Index out of range");
  ColumnVector vec(4);
  vec.Rows(1,3) = _points.Column(indx+1); vec(4) = 1.0;
  vec = _affine * vec;
  if (one_ext) {
    vec.Release();
    return(vec);
  }
  ColumnVector rvec = vec.Rows(1,3);
  rvec.Release();
  return(rvec);
}

NEWMAT::ReturnMatrix PointList::RawPoint(unsigned int indx, bool one_ext) const
{
  if (indx >= NPoints()) throw PointListException("Point: Index out of range");
  if (one_ext) {
    ColumnVector vec(4);
    vec.Rows(1,3) = _points.Column(indx+1); vec(4) = 1.0;
    vec.Release();
    return(vec);
  }
  ColumnVector vec(3);
  vec = _points.Column(indx+1);
  vec.Release();
  return(vec);
}

NEWMAT::ReturnMatrix PointList::Vox2mm() const
{
  Matrix rmat = _vox2mm;
  rmat.Release();
  return(rmat);
}

NEWMAT::ReturnMatrix PointList::mm2Vox() const
{
  Matrix rmat = _vox2mm.i();
  rmat.Release();
  return(rmat);
}

void PointList::SetAffine(const NEWMAT::Matrix& aff)
{
  if (aff.Nrows() != 4 || aff.Ncols() != 4) throw PointListException("SetAffine: Matrix must be 4x4");
  _affine = aff;
}

// This routine reads a list of points from a text-file that 
// the user has (presumably) entered manually. It will be assumed
// that the user has used fslview to deduce these points, and that
// he/she has entered the coordinates from the second column of the
// coordinate box. In this box will be "FSL mm-coordinates" if no
// sform/qform has been set or "world coordinates" if there is
// a valid sform/qform.
// The PointList is expected to return "FSL mm-coordinates" so
// if it is in "world coordinates" we will need to undo that.
int PointList::read_ascii_file(const std::string& fname)
{
  Matrix pl = MISCMATHS::read_ascii_matrix(fname);
  if (pl.Nrows() == 0 || pl.Ncols() == 0 || (pl.Nrows() != 3 && pl.Ncols() != 3)) return(-1);
  if (pl.Ncols() == 3) pl = pl.t();

  if ((_SQform-IdentityMatrix(4)).MaximumAbsoluteValue() > 1e-6) { // If q/s form is set.
    _points.ReSize(pl.Nrows(),pl.Ncols());
    Matrix M2mm = _vox2mm * _SQform.i();
    for (int i=1; i<=pl.Ncols(); i++) {
      ColumnVector pt(4);
      pt.Rows(1,3) = pl.Column(i); pt(4) = 1.0;
      pt = M2mm * pt;
      _points.Column(i) = pt.Rows(1,3);
    }
  }
  else _points = pl;  
  return(1);
}

// This routines reads a list of points generated by first
// or other "first-related" routines that uses the same
// output format. They are expected to already be in
// "FSL mm-coordinates".
int PointList::read_first_file(const std::string& fname)
{
  // Wrapping this in a try since the meshclass reader sometimes
  // returns -1 and sometimes crashes when encountering invalid
  // .vtk files. And sometimes it crashes without there being a
  // proper exception, so it is not possible to catch. Phew!
  try {
    Mesh pl;
    if (pl.load(fname)<0) return(-1);
    _points.ReSize(3,pl.nvertices());
    for (vector<Mpoint *>::iterator i=pl._points.begin(); i!=pl._points.end(); i++) {
      int indx = (*i)->get_no();
      Pt p = pl.get_point(indx)->get_coord();
      _points(1,indx+1) = p.X; _points(2,indx+1) = p.Y; _points(3,indx+1) = p.Z; 
    }
    return(1);
  }
  catch (const std::exception& error) {
    cerr << "PointList::read_first_file: Exception thrown with message: " << error.what() << endl; 
    return(-1);
  }
  return(-1);
}

bool PointList::is_maybe_vtk(const std::string& fname) const
{
  ifstream fs;
  fs.open(fname.c_str());
  if (fs.is_open()) {
    string tag;
    getline(fs,tag);
    fs.clear(); fs.close();
    if (tag.find("# vtk DataFile") == string::npos) return(false);
    else return(true);
  }
  return(false);
}

#ifdef I_DEFINED_ET
#undef I_DEFINED_ET
#undef EXPOSE_TREACHEROUS   // Avoid exporting dodgy routines
#endif
