#ifndef   math_Transformations_H__
#define   math_Transformations_H__

#include "Transformation.h"
#include "Matrix.h"
#include "utility.h"
#include "../Self.h"

#include <cstdlib>

namespace meow {

/*!
 * @brief A ball projection is to project the given vector to a hyper-sphere
 *
 * Assume:
 *   - The dimension of a ball projection is \f$ N \f$
 *   - The radius of the hyper-sphere is \f$ R \f$
 *   .
 * Then the transformation is like below: \n
 * \f[
 *   \left[
 *     \begin{array}{c}
 *        x_1 \\
 *        x_2 \\
 *        x_3 \\
 *        .   \\
 *        .   \\
 *        .   \\
 *        x_N \\
 *     \end{array}
 *   \right]
 * \stackrel{transformate}{\rightarrow}
 *   \left[
 *     \begin{array}{c}
 *        \frac{x_1 \times R}{L} \\
 *        \frac{x_2 \times R}{L} \\
 *        \frac{x_3 \times R}{L} \\
 *        .   \\
 *        .   \\
 *        .   \\
 *        \frac{x_N \times R}{L} \\
 *     \end{array}
 *   \right] \\
 * \f]
 * where \f$ L=\sqrt{x_1^2 + x_2^2 + x_3^2 + ... + x_N^2 } \f$
 * @author cat_leopard
 */
template<class Scalar>
class BallProjection: public Transformation<Scalar> {
private:
  struct Myself {
    Scalar    radius_;
    size_t dimension_;

    Myself() {
    }
    ~Myself() {
    }
    Myself& copyFrom(Myself const& b) {
      radius_    = b.radius_;
      dimension_ = b.dimension_;
      return *this;
    }
  };
  
  Self<Myself> const self;
public:
  /*!
   * Constructor, copy settings from given BallProjection
   * @param [in] b another ball projection class
   */
  BallProjection(BallProjection const& b):
  Transformation<Scalar>(b), self(false) {
    copyFrom(b);
  }
  
  /*!
   * Constructor and setup, radius = 1
   * @param [in] d Dimension of the input/output vector
   */
  BallProjection(size_t d): self(true),
  Transformation<Scalar>(d, 1, d, 1, 1) {
    self()->dimension_ = d;
    radius(1);
  }
  
  /*!
   * Constructor and setup
   * @param [in] d Dimension of the input/output vector
   * @param [in] r Radius of the hyper-sphere
   */
  BallProjection(size_t d, Scalar const& r):
  Transformation<Scalar>(d, 1, d, 1, 1), self(true) {
    self()->dimension_ = d;
    radius(r);
  }
  
  /*!
   * @brief Copy settings from another one
   * @param [in] b Another one
   * @return \c *this
   */
  BallProjection& copyFrom(BallProjection const& b) {
    Transformation<Scalar>::copyFrom(b);
    copyFrom(b);
    return *this;
  }
  
  /*!
   * @brief Reference settings from another one
   * @param [in] b Another one
   * @return \c *this
   */
  BallProjection& referenceFrom(BallProjection const& b) {
    Transformation<Scalar>::referenceFrom(b);
    referenceFrom(b);
    return *this;
  }

  /*!
   * @brief same as \c radius()
   */
  Scalar parameter(size_t i) const {
    return radius();
  }

  /*!
   * @brief same as \c radius(s)
   */
  Scalar parameter(size_t i, Scalar const& s) {
    return radius(s);
  }

  /*!
   * @brief Return the value of the radius
   */
  Scalar radius() const {
    return self->radius_;
  }
  
  /*!
   * @brief Setup the radius
   *
   * @param [in] r New value of the radius
   * @return New radius
   */
  Scalar radius(Scalar const& r) {
    self()->radius_ = r;
    return radius();
  }
  
  /*!
   * @brief Get the dimension of this projection
   */
  size_t dimension() const {
    return self->dimension_;
  }

  
  /*!
   * @brief Project the input vector(s) onto the hyper-sphere and return it.
   *
   * If the number of columns of the input matrix is larger than 1, this
   * method will think that you want to transform multiple vector once
   * and the number of columns of the output matrix will be the same of
   * the number of columns of the input one.
   * 
   * @param [in] x The input matrix.
   * @return The output matrix.
   * @note Take into account that too much safty checking will lead to
   *       inefficient, this method will not checking whether the dimension
   *       of the input vector/matrix is right. So be sure the data is valid
   *       before you call this method.
   */
  Matrix<Scalar> transformate(Matrix<Scalar> const& x) const {
    Matrix<Scalar> ret(x);
    for (size_t c = 0, C = ret.cols(); c < C; c++) {
      Scalar sum(0);
      for (size_t i = 0; i < self->dimension_; i++) {
        sum = sum + squ(ret(i, c));
      }
      Scalar len(sqrt(double(sum)));
      for (size_t i = 0; i < self->dimension_; i++) {
        ret(i, c, ret(i, c) * radius() / len);
      }
    }
    return ret;
  }
  
  /*!
   * @brief Return the jacobian matrix (derivate by the input vector)
   *        of this projection.
   *
   * This method only allow a vector-like matrix be input.
   * Assume:
   *  - The dimension of a ball projection is \f$ N \f$
   *  - The length of the input vector is \f$ L=\sqrt{x_1^2+x_2^2+...+x_N^2} \f$
   *  - The radius of the hyper-sphere is \f$ R \f$
   *  .
   * Then the jacobian matrix is like below: \n
   * \f[
   *   \frac{R}{L^3} \times \left[
   *     \begin{array}{ccccc}
   *       L^2-x_1^2 & -x_1x_2   & -x_1x_3   & ... & -x_1x_N   \\
   *       -x_2x_1   & L^2-x_2^2 & -x_2x_3   & ... & -x_2x_N   \\
   *       -x_3x_1   & -x_3x_2   & L^2-x_3^2 & ... & -x_3x_N   \\
   *       .         & .         & .         &     & .         \\
   *       .         & .         & .         &     & .         \\
   *       .         & .         & .         &     & .         \\
   *       -x_Nx_1   & -x_Nx_2   & -x_Nx_3   & ... & L^2-x_N^2 \\
   *     \end{array}
   *   \right]
   * \f]
   *
   * @param [in] x The input matrix.
   * @return The output matrix.
   */
  Matrix<Scalar> jacobian(Matrix<Scalar> const& x) const {
    Scalar sum(0);
    for(size_t i = 0, I = dimension(); i < I; ++i)
      sum = sum + squ(x(i, 0));
    Scalar len(sqrt(double(sum)));
    Matrix<Scalar> ret(dimension(), dimension(), Scalar(0.0));
    for(size_t i = 0, I = dimension(); i < I; ++i)
      for(size_t j = 0; j < I; ++j)
        if (i == j) {
          ret(i, j, radius() * (squ(len) - squ(x(i, 0))) / cub(len));
        }
        else {
          ret(i, j, radius() * (-x(i, 0) * x(j, 0) / cub(len)));
        }
    return ret;
  }
  
  /*!
   * @brief Return the jacobian matrix (derivate by radius) of this projection.
   *
   * This method only allow a vector-like matrix be input.
   * Assume:
   *  - The dimension of a ball projection is \f$ N \f$
   *  - The length of the input vector is \f$ L=\sqrt{x_1^2+x_2^2+...+x_N^2} \f$
   *  - The radius of the hyper-sphere is \f$ R \f$
   *  .
   * Then the jacobian matrix is like below: \n
   * \f[
   *   R \times \left[
   *     \begin{array}{c}
   *        \frac{x_1}{L} \\
   *        \frac{x_2}{L} \\
   *        \frac{x_3}{L} \\
   *        .   \\
   *        .   \\
   *        .   \\
   *        \frac{x_N}{L} \\
   *     \end{array}
   *   \right]
   * \f]
   *
   * @param [in] x The input matrix.
   * @param [in] i Useless parameter
   * @return The output matrix.
   */
  Matrix<Scalar> jacobian(Matrix<Scalar> const& x, size_t i) const {
    Matrix<Scalar> ret(dimension(), 1, Scalar(0.0));
    Scalar sum(0);
    for(size_t i = 0, I = dimension(); i < I; i++) {
      sum = sum + squ(x(i, 0));
    }
    return ret / Scalar(sqrt(double(sum)));
  }
  
  /*!
   * @brief Same as \c copyFrom(b)
   */
  BallProjection& operator=(BallProjection const& b) {
    return copyFrom(b);
  }
  
  /*!
   * @brief Same as \c transformate(v)
   */
  Matrix<Scalar> operator()(Matrix<Scalar> const& v) const {
    return transformate(v);
  }
};


/*!
 * @brief A \b photo \b projection is a kind of transformation that project
 *        point/vector to a flat \b photo
 *
 * Assume:
 *   - The dimension of a photo projection is \f$ N \f$
 *   - The length of the input vector is \f$ L \f$
 *   - The focal length is \f$ f \f$
 *   .
 * Then transformation is like below: \n
 * \f[
 *   \left[
 *     \begin{array}{c}
 *        x_1 \\
 *        x_2 \\
 *        x_3 \\
 *        .   \\
 *        .   \\
 *        .   \\
 *        x_N \\
 *     \end{array}
 *   \right]
 * \stackrel{transformate}{\rightarrow}
 *   \left[
 *     \begin{array}{c}
 *        \frac{-x_1 \times f}{x_N} \\
 *        \frac{-x_2 \times f}{x_N} \\
 *        \frac{-x_3 \times f}{x_N} \\
 *        .   \\
 *        .   \\
 *        .   \\
 *        -f \\
 *     \end{array}
 *   \right] \\
 * \f]
 * i.e. projecte the vector onto the plane \f$ x_N = -f \f$.
 *
 * @author cat_leopard
 */
template<class Scalar>
class PhotoProjection: public Transformation<Scalar> {
private:
  struct Myself {
    Scalar     focal_;
    size_t dimension_;
    
    Myself() {
    }
    ~Myself() {
    }
    Myself& copyFrom(Myself const& b) {
      focal_     = b.focal_;
      dimension_ = b.dimension_;
      return *this;
    }
  };
  
  Self<Myself> const& self;
public:
  /*!
   * Constructor, focal = 1
   */
  PhotoProjection(size_t dimension): 
  Transformation<Scalar>(dimension, 1, dimension, 1, 1), self(true) {
    self()->dimension_ = dimension;
    focal(1);
  }
  
  /*!
   * Constructor
   */
  PhotoProjection(size_t dimension, Scalar const& f):
  Transformation<Scalar>(dimension, 1, dimension, 1, 1), self(true) {
    self()->dimension_ = dimension;
    focal(f);
  }
  
  /*!
   * Constructor, copy settings from another PhotoProjection.
   */
  PhotoProjection(PhotoProjection const& p):
  Transformation<Scalar>(p), self(false) {
    self().copyFrom(p.self);
  }
  
  /*!
   * Copy settings from another one
   * @param [in] b another one
   * @return \c *this
   */
  PhotoProjection& copyFrom(PhotoProjection const& b) {
    Transformation<Scalar>::copyFrom(b);
    self().copyFrom(b.self);
    return *this;
  }
  
  /*!
   * Reference settings from another one
   * @param [in] b another one
   * @return \c *this
   */
  PhotoProjection& referenceFrom(PhotoProjection const& b) {
    Transformation<Scalar>::referenceFrom(b);
    self().referenceFrom(b.self);
    return *this;
  }

  /*!
   * @brief Same as \c focal()
   */
  Scalar parameter(size_t i) const {
    return focal();
  }

  /*!
   * @brief Same as \c focal(s)
   */
  Scalar parameter(size_t i, Scalar const& s){
    return focal(s);
  }

  /*!
   * @brief Get the focal length
   * @return Focal length
   */
  Scalar focal() const {
    return self->focal_;
  }

  /*!
   * @brief Set the focal length
   *
   * @param [in] f  New focal length
   * @return New focal length
   */
  Scalar focal(Scalar const& f){
    self()->focal_ = f;
    return focal();
  }
  
  /*!
   * @brief Get the dimension of this projection
   */
  size_t dimension() const {
    return self->dimension_;
  }

  /*!
   * @brief Project the input vector(s) onto the plane
   *
   * The equation of the plane is \f$ x_N = -f \f$, where the \f$ N \f$ 
   * is the dimension of this projection and f is the focal length. \n
   * If the number of columns of the input matrix is larger than 1, this
   * method will think that you want to transform multiple vector once
   * and the number of columns of the output matrix will be the same of
   * the number of columns of the input one.
   * 
   * @param [in] x The input matrix.
   * @return The output matrix.
   * @note Take into account that too much safty checking will lead to
   *       inefficient, this method will not checking whether the dimension
   *       of the input vector/matrix is right. So be sure the data is valid
   *       before you call this method.
   */
  Matrix<Scalar> transformate(Matrix<Scalar> const& x) const {
    Matrix<Scalar> ret(x);
    for (size_t c = 0, C = ret.cols(); c < C; c++) {
      for (size_t i = 0, I = dimension(); i < I; ++i) {
        ret(i, c, -ret(i, c) * focal() / ret(I - 1, c));
      }
    }
    return ret;
  }
  
  /*!
   * @brief Return the jacobian matrix (derivate by the input vector) 
   *        of this projection.
   *
   * This method only allow a vector-like matrix be input.
   * Assume:
   *  - The dimension of this projection is \f$ N \f$
   *  - The length of the input vector is \f$ L=\sqrt{x_1^2+x_2^2+...+x_N^2} \f$
   *  - The focal length of this projection is \f$ f \f$
   *  .
   * Then the jacobian matrix is like below: \n
   * \f[
   *   f \times
   *   \left[
   *     \begin{array}{ccccc}
   * \frac{-1}{x_N} & 0              & 0              & ... & \frac{1}{x_N^2} \\
   *  0             & \frac{-1}{x_N} & 0              & ... & \frac{1}{x_N^2} \\
   *  0             & 0              & \frac{-1}{x_N} & ... & \frac{1}{x_N^2} \\
   *  .             & .              & .              &     & . \\
   *  .             & .              & .              &     & . \\
   *  .             & .              & .              &     & . \\
   *  0             & 0              & 0              & ... & 0 \\
   *     \end{array}
   *   \right]
   * \f]
   *
   * @param [in] x The input matrix.
   * @return The output matrix.
   */
  Matrix<Scalar> jacobian(Matrix<Scalar> const& x) const{
    Matrix<Scalar> ret(dimension(), dimension(), Scalar(0.0));
    for(ssize_t i = 0, I = (ssize_t)dimension() - 1; i < I; i++){
      ret(i,               i, -focal() /     x(I, 0) );
      ret(i, dimension() - 1,  focal() / squ(x(I, 0)));
    }
    return ret;
  }
  
  /*!
   * @brief Return the jacobian matrix (derivate by the focus length)
   *        of this projection.
   *
   * This method only allow a vector-like matrix be input.
   * Assume:
   *  - The dimension of this projection is \f$ N \f$
   *  - The length of the input vector is \f$ L=\sqrt{x_1^2+x_2^2+...+x_N^2} \f$
   *  - The focal length of this projection is \f$ f \f$
   *  .
   * Then the jacobian matrix is like below: \n
   * \f[
   *   f \times
   *   \left[
   *     \begin{array}{c}
   *       \frac{-x_1}{x_N} \\
   *       \frac{-x_2}{x_N} \\
   *       \frac{-x_3}{x_N} \\
   *       .                \\
   *       .                \\
   *       .                \\
   *       -1               \\
   *     \end{array}
   *   \right]
   * \f]
   *
   * @param [in] x The input matrix.
   * @param [in] i Useless parameter
   * @return The output matrix.
   */
  Matrix<Scalar> jacobian(Matrix<Scalar> const& x, size_t i) const{
    Matrix<Scalar> ret(dimension(), 1, Scalar(0.0));
    for(size_t i = 0, I = dimension(); i < I; ++i) {
      ret(i, 0, -x(i, 0) / x(I - 1, 0));
    }
    return ret;
  }
  
  /*!
   * @brief Same as \c copyFrom(b)
   */
  PhotoProjection& operator=(PhotoProjection const& b) {
    return copyFrom(b);
  }
  
  /*!
   * @brief Same as \c transformate(v)
   */
  Matrix<Scalar> operator()(Matrix<Scalar> const& v) const {
    return transformate(v);
  }
};

}

#endif // Transformations_H__