/*//////////////////////////////////////////////////////////////////////////////
// CMT Cosmic Muon Tomography project //////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

  Copyright (c) 2014, Universita' degli Studi di Padova, INFN sez. di Padova
  All rights reserved

  Authors: Andrea Rigoni Garola < andrea.rigoni@pd.infn.it >

  ------------------------------------------------------------------
  This library is free software;  you  can  redistribute  it  and/or
  modify it  under the  terms  of  the  GNU  Lesser  General  Public
  License as published  by  the  Free  Software  Foundation;  either
  version 3.0 of the License, or (at your option) any later version.

  This library is  distributed in  the hope that it will  be useful,
  but  WITHOUT ANY WARRANTY;  without  even  the implied warranty of
  MERCHANTABILITY  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of  the GNU Lesser General  Public
  License along with this library.

//////////////////////////////////////////////////////////////////////////////*/

/*
 * Copyright (C) 2012  Andrea Rigoni Garola <andrea.rigoni@pd.infn.it>
 *
 * This library is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation; either version 2.1 of the
 * License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301 USA
 *
 */

#ifndef U_TRANSFORM_H
#define U_TRANSFORM_H

#include "Math/Dense.h"
#include "Math/Units.h"
#include <Eigen/Geometry>

namespace uLib {

using Eigen::Isometry3d;
using Eigen::Isometry3f;

using Eigen::Affine3d;
using Eigen::Affine3f;

using Eigen::Projective3d;
using Eigen::Projective3f;

////////////////////////////////////////////////////////////////////////////////
/////////  AFFINE TRANSFORM  WRAPPER  //////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

class AffineTransform : virtual public Object {
public:
uLibTypeMacro(AffineTransform, Object) protected :

    Affine3f m_T;
  AffineTransform *m_Parent;

public:
  AffineTransform() : m_T(Matrix4f::Identity()), m_Parent(NULL) {}

  AffineTransform(AffineTransform *parent)
      : m_T(Matrix4f::Identity()), m_Parent(parent) {}

  AffineTransform(const AffineTransform &copy)
      : m_T(copy.m_T), m_Parent(copy.m_Parent) {}

  Affine3f &GetTransform() { return m_T; }

  AffineTransform *GetParent() const { return this->m_Parent; }

  void SetParent(AffineTransform *name) { this->m_Parent = name; }

  void SetMatrix(const Matrix4f &mat) { m_T.matrix() = mat; }
  Matrix4f &GetMatrix() { return m_T.matrix(); }
  const Matrix4f &GetMatrix() const { return m_T.matrix(); }

  Matrix4f GetWorldMatrix() const {
    if (!m_Parent)
      return m_T.matrix();
    else
      return m_Parent->GetWorldMatrix() * m_T.matrix(); // T = B * A //
  }

  void SetWorldMatrix(const Matrix4f &mat) {
    if (!m_Parent)
      m_T.matrix() = mat;
    else
      m_T.matrix() = m_Parent->GetWorldMatrix().inverse() * mat;
  }

  void SetPosition(const Vector3f &v) { this->m_T.translation() = v; }

  Vector3f GetPosition() const { return this->m_T.translation(); }

  void SetRotation(const Matrix3f &m) { this->m_T.linear() = m; }

  Matrix3f GetRotation() const { return this->m_T.rotation(); }

  void Translate(const Vector3f &v) { this->m_T.translate(v); }

  void Scale(const Vector3f &v) { this->m_T.scale(v); }

  Vector3f GetScale() const {
    return Vector3f(this->m_T.linear().col(0).norm(),
                    this->m_T.linear().col(1).norm(),
                    this->m_T.linear().col(2).norm());
  }

  void Rotate(const Matrix3f &m) { this->m_T.rotate(m); }

  void Rotate(const float angle, Vector3f axis) {
    axis.normalize(); // prehaps not necessary ( see eigens )
    Eigen::AngleAxisf ax(angle, axis);
    this->m_T.rotate(Eigen::Quaternion<float>(ax));
  }

  void Rotate(const Vector3f euler_axis) {
    float angle = euler_axis.norm();
    Rotate(angle, euler_axis);
  }

  void PreRotate(const Matrix3f &m) { this->m_T.prerotate(m); }

  void QuaternionRotate(const Vector4f &q) {
    this->m_T.rotate(Eigen::Quaternion<float>(q));
  }

  void EulerYZYRotate(const Vector3f &e) {
    Matrix3f mat;
    mat = Eigen::AngleAxisf(e.x(), Vector3f::UnitY()) *
          Eigen::AngleAxisf(e.y(), Vector3f::UnitZ()) *
          Eigen::AngleAxisf(e.z(), Vector3f::UnitY());
    m_T.rotate(mat);
  }

  void FlipAxes(int first, int second) {
    Matrix3f mat = Matrix3f::Identity();
    mat.col(first).swap(mat.col(second));
    m_T.rotate(mat);
  }
};

////////////////////////////////////////////////////////////////////////////////
/////////  TRS PARAMETERS  /////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

typedef Eigen::Affine3f AffineMatrix;

class TRS : public AffineTransform {

uLibTypeMacro(TRS, AffineTransform) ULIB_SERIALIZE_ACCESS
    // ULIB_DECLARE_PROPERTIES(TRS)

    public :

    Vector3f position = Vector3f::Zero();
  Vector3f rotation = Vector3f::Zero();
  Vector3f scaling = Vector3f::Ones();

  TRS() = default;

  TRS(const class AffineTransform &at) { this->FromMatrix(at.GetMatrix()); }

  TRS(const Matrix4f &mat) { this->FromMatrix(mat); }

  void FromMatrix(const Matrix4f &mat) {
    this->position = mat.block<3, 1>(0, 3);

    Matrix3f linear = mat.block<3, 3>(0, 0);
    this->scaling(0) = linear.col(0).norm();
    this->scaling(1) = linear.col(1).norm();
    this->scaling(2) = linear.col(2).norm();

    Matrix3f rot = linear;
    if (this->scaling(0) > 1e-6)
      rot.col(0) /= this->scaling(0);
    if (this->scaling(1) > 1e-6)
      rot.col(1) /= this->scaling(1);
    if (this->scaling(2) > 1e-6)
      rot.col(2) /= this->scaling(2);

    Vector3f euler = rot.canonicalEulerAngles(2, 1, 0);
    this->rotation = Vector3f(euler(2), euler(1), euler(0));

    this->SetMatrix(mat);
    this->NotifyPropertiesUpdated();
  }

  void SetPosition(const Vector3f &v) {
    position = v;
    this->AffineTransform::SetPosition(v);
  }

  void SetRotation(const Vector3f &v) {
    rotation = v;
    this->SyncMatrix();
  }

  void SetOrientation(const Vector3f &v) { SetRotation(v); }

  void SetScale(const Vector3f &v) {
    scaling = v;
    this->SyncMatrix();
  }

  void SyncMatrix() { this->GetTransform() = GetAffineMatrix(); }

  void Updated() override {
    this->SyncMatrix();
    this->NotifyPropertiesUpdated();
    this->AffineTransform::Updated();
  }

  template <class ArchiveT>
  void serialize(ArchiveT &ar, const unsigned int version) {
    ar &HRPU(position, "mm");
    ar &HRPU(rotation, "rad");
    ar &HRP(scaling);
  }

  AffineMatrix GetAffineMatrix() const {
    AffineMatrix m = AffineMatrix::Identity();
    m.translate(position);
    m.rotate(Eigen::AngleAxisf(rotation.z(), Vector3f::UnitZ()));
    m.rotate(Eigen::AngleAxisf(rotation.y(), Vector3f::UnitY()));
    m.rotate(Eigen::AngleAxisf(rotation.x(), Vector3f::UnitX()));
    m.scale(scaling);
    return m;
  }

  Matrix4f GetMatrix() const { return this->GetAffineMatrix().matrix(); }

};

inline std::ostream &operator<<(std::ostream &os, const TRS &trs) {
  os << trs.position << " " << trs.rotation << " " << trs.scaling;
  return os;
}

inline std::istream &operator>>(std::istream &is, TRS &trs) {
  is >> trs.position >> trs.rotation >> trs.scaling;
  return is;
}

} // namespace uLib

#endif // U_TRANSFORM_H