// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
//
// Eigen 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 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen 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 or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#ifndef EIGEN_ALIGNED_VECTOR3
#define EIGEN_ALIGNED_VECTOR3

#include <Eigen/Geometry>

namespace Eigen {

/** \ingroup Unsupported_modules
  * \defgroup AlignedVector3_Module Aligned vector3 module
  *
  * \code
  * #include <unsupported/Eigen/AlignedVector3>
  * \endcode
  */
  //@{


/** \class AlignedVector3
  *
  * \brief A vectorization friendly 3D vector
  *
  * This class represents a 3D vector internally using a 4D vector
  * such that vectorization can be seamlessly enabled. Of course,
  * the same result can be achieved by directly using a 4D vector.
  * This class makes this process simpler.
  *
  */
// TODO specialize Cwise
template<typename _Scalar> class AlignedVector3;

template<typename _Scalar> struct ei_traits<AlignedVector3<_Scalar> >
  : ei_traits<Matrix<_Scalar,3,1,0,4,1> >
{
};

template<typename _Scalar> class AlignedVector3
  : public MatrixBase<AlignedVector3<_Scalar> >
{
    typedef Matrix<_Scalar,4,1> CoeffType;
    CoeffType m_coeffs;
  public:

    EIGEN_GENERIC_PUBLIC_INTERFACE(AlignedVector3)
    using Base::operator*;

    inline int rows() const { return 3; }
    inline int cols() const { return 1; }

    inline const Scalar& coeff(int row, int col) const
    { return m_coeffs.coeff(row, col); }

    inline Scalar& coeffRef(int row, int col)
    { return m_coeffs.coeffRef(row, col); }

    inline const Scalar& coeff(int index) const
    { return m_coeffs.coeff(index); }

    inline Scalar& coeffRef(int index)
    { return m_coeffs.coeffRef(index);}


    inline AlignedVector3(const Scalar& x, const Scalar& y, const Scalar& z)
      : m_coeffs(x, y, z, Scalar(0))
    {}

    inline AlignedVector3(const AlignedVector3& other)
      : Base(), m_coeffs(other.m_coeffs)
    {}

    template<typename XprType, int Size=XprType::SizeAtCompileTime>
    struct generic_assign_selector {};

    template<typename XprType> struct generic_assign_selector<XprType,4>
    {
      inline static void run(AlignedVector3& dest, const XprType& src)
      {
        dest.m_coeffs = src;
      }
    };

    template<typename XprType> struct generic_assign_selector<XprType,3>
    {
      inline static void run(AlignedVector3& dest, const XprType& src)
      {
        dest.m_coeffs.template head<3>() = src;
        dest.m_coeffs.w() = Scalar(0);
      }
    };

    template<typename Derived>
    inline explicit AlignedVector3(const MatrixBase<Derived>& other)
    {
      generic_assign_selector<Derived>::run(*this,other.derived());
    }

    inline AlignedVector3& operator=(const AlignedVector3& other)
    { m_coeffs = other.m_coeffs; return *this; }


    inline AlignedVector3 operator+(const AlignedVector3& other) const
    { return AlignedVector3(m_coeffs + other.m_coeffs); }

    inline AlignedVector3& operator+=(const AlignedVector3& other)
    { m_coeffs += other.m_coeffs; return *this; }

    inline AlignedVector3 operator-(const AlignedVector3& other) const
    { return AlignedVector3(m_coeffs - other.m_coeffs); }

    inline AlignedVector3 operator-=(const AlignedVector3& other)
    { m_coeffs -= other.m_coeffs; return *this; }

    inline AlignedVector3 operator*(const Scalar& s) const
    { return AlignedVector3(m_coeffs * s); }

    inline friend AlignedVector3 operator*(const Scalar& s,const AlignedVector3& vec)
    { return AlignedVector3(s * vec.m_coeffs); }

    inline AlignedVector3& operator*=(const Scalar& s)
    { m_coeffs *= s; return *this; }

    inline AlignedVector3 operator/(const Scalar& s) const
    { return AlignedVector3(m_coeffs / s); }

    inline AlignedVector3& operator/=(const Scalar& s)
    { m_coeffs /= s; return *this; }

    inline Scalar dot(const AlignedVector3& other) const
    {
      ei_assert(m_coeffs.w()==Scalar(0));
      ei_assert(other.m_coeffs.w()==Scalar(0));
      return m_coeffs.dot(other.m_coeffs);
    }

    inline void normalize()
    {
      m_coeffs /= norm();
    }

    inline AlignedVector3 normalized()
    {
      return AlignedVector3(m_coeffs / norm());
    }

    inline Scalar sum() const
    {
      ei_assert(m_coeffs.w()==Scalar(0));
      return m_coeffs.sum();
    }

    inline Scalar squaredNorm() const
    {
      ei_assert(m_coeffs.w()==Scalar(0));
      return m_coeffs.squaredNorm();
    }

    inline Scalar norm() const
    {
      return ei_sqrt(squaredNorm());
    }

    inline AlignedVector3 cross(const AlignedVector3& other) const
    {
      return AlignedVector3(m_coeffs.cross3(other.m_coeffs));
    }

    template<typename Derived>
    inline bool isApprox(const MatrixBase<Derived>& other, RealScalar eps=dummy_precision<Scalar>()) const
    {
      return m_coeffs.template head<3>().isApprox(other,eps);
    }
};

//@}

}

#endif // EIGEN_ALIGNED_VECTOR3