/*  Multimodal Image Segmentation Tool (MIST)  */
/*  Eelke Visser  */

/*  Copyright (c) 2016 University of Oxford  */

/*  CCOPYRIGHT  */

#include <string>

#include "transformation.h"
#include "miscmaths/miscmaths.h"
#include "warpfns/fnirt_file_reader.h"


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

Transformation::Transformation()
{
}

ColumnVector IdentityTransformation::InverseTransformPoint(const ColumnVector &p) const
{
    return p;
}

AffineTransformation::AffineTransformation(const string &filename)
{
    BOOST_LOG_TRIVIAL(debug) << "AffineTransformation: Loading matrix " << filename;

    Matrix matrix = MISCMATHS::read_ascii_matrix(4, 4, filename);

    if (matrix.IsZero())
        throw TransformationException("Cannot load affine matrix");

    m_inverse = matrix.i();
}

AffineTransformation::AffineTransformation(const Matrix &matrix)
    : m_inverse(matrix.i())
{

}

AffineTransformation AffineTransformation::Concatenate(const AffineTransformation &second) const
{
    BOOST_LOG_TRIVIAL(warning) << "AffineTransformation::Concatenate(AffineTransformation&) is untested!";

    return AffineTransformation(m_inverse * second.m_inverse);
}

NonlinearTransformation AffineTransformation::Concatenate(const NonlinearTransformation &second) const
{
    volume4D<float> firstfield;
    affine2warp(m_inverse.i(), firstfield, second.AccessField()[0]);

    volume4D<float> combinedfield;
    concat_warps(firstfield, second.AccessField(), combinedfield);

    return NonlinearTransformation(combinedfield);
}

ColumnVector AffineTransformation::InverseTransformPoint(const ColumnVector &p) const
{
    ColumnVector ap(4);
    ap.Rows(1, 3) = p;
    ap(4) = 1.0;

    return (m_inverse * ap).Rows(1, 3);
}

NonlinearTransformation::NonlinearTransformation(const string &filename, bool includeaffine)
    : m_ready(false),
      m_backingFile(filename),
      m_includeAffineFromBackingFile(includeaffine)
{

}

NonlinearTransformation::NonlinearTransformation(const NEWIMAGE::volume4D<float> &warpfield)
    : m_ready(true),
      m_includeAffineFromBackingFile(false),
      m_field(warpfield)
{

}

void NonlinearTransformation::Load() const
{
    BOOST_LOG_TRIVIAL(debug) << "NonlinearTransformation: Loading warp field " << m_backingFile
                             << " (includeaffine = " << m_includeAffineFromBackingFile << ")";

    FnirtFileReader ffr(m_backingFile);
    m_field = ffr.FieldAsNewimageVolume4D(m_includeAffineFromBackingFile);
    convertwarp_rel2abs(m_field);

    m_ready = true;
}

volume4D<float> &NonlinearTransformation::AccessField() const
{
    if (!m_ready)
        Load();

    return m_field;
}

void NonlinearTransformation::ReleaseMemory() const
{
    if (!m_backingFile.empty())
    {
        BOOST_LOG_TRIVIAL(debug) << "NonlinearTransformation: Releasing warp field " << m_backingFile;

        m_ready = false;
        m_field = volume4D<float>();
    }
}

NonlinearTransformation NonlinearTransformation::Concatenate(const NonlinearTransformation &second) const
{
    BOOST_LOG_TRIVIAL(warning) << "NonlinearTransformation::Concatenate(NonlinearTransformation&) is untested!";

    volume4D<float> combinedfield;
    concat_warps(AccessField(), second.AccessField(), combinedfield);

    return NonlinearTransformation(combinedfield);
}

ColumnVector NonlinearTransformation::InverseTransformPoint(const ColumnVector &p) const
{
    if (!m_ready)
        Load();

    ColumnVector d(3);
    // Trilinear by default
    for (int i = 0; i < 3; i++)
        d(i + 1) = m_field[i].interpolate(p(1) / m_field.xdim(), p(2) / m_field.ydim(), p(3) / m_field.zdim());

    return d;
}
