/*
    Copyright 2016 Emanuele Vespa, Imperial College London
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:

    1. Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.

    2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.

    3. Neither the name of the copyright holder nor the names of its contributors
    may be used to endorse or promote products derived from this software without
    specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
    FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
    DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
    OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
    OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/
#ifndef OCTANT_OPS_HPP
#define OCTANT_OPS_HPP
#include "utils/morton_utils.hpp"
#include "utils/math_utils.h"
#include "octree_defines.h"
#include <iostream>
#include <bitset>
#include <Eigen/Dense>

namespace se {
  namespace keyops {

    inline se::key_t code(const se::key_t octant_key) {
      return octant_key & ~SCALE_MASK;
    }

    inline int depth(const se::key_t octant_key) {
      return octant_key & SCALE_MASK;
}

    inline se::key_t encode(const int x, const int y, const int z,
        const int depth, const int voxel_depth) {
      const int offset = MAX_BITS - voxel_depth + depth - 1;
      return (compute_morton(x, y, z) & MASK[offset] & ~SCALE_MASK) | depth;
    }

    inline Eigen::Vector3i decode(const se::key_t octant_key) {
      return unpack_morton(octant_key & ~SCALE_MASK);
    }
  }

/*
 * Algorithm 5 of p4est paper: https://epubs.siam.org/doi/abs/10.1137/100791634
 */
inline Eigen::Vector3i face_neighbour(const se::key_t octant_key,
    const unsigned int face, const unsigned int l,
    const unsigned int voxel_depth) {
  Eigen::Vector3i octant_coord = se::keyops::decode(octant_key);
  const unsigned int octant_size = 1 << (voxel_depth - l);
  octant_coord.x() = octant_coord.x() + ((face == 0) ? -octant_size : (face == 1) ? octant_size : 0);
  octant_coord.y() = octant_coord.y() + ((face == 2) ? -octant_size : (face == 3) ? octant_size : 0);
  octant_coord.z() = octant_coord.z() + ((face == 4) ? -octant_size : (face == 5) ? octant_size : 0);
  return {octant_coord.x(), octant_coord.y(), octant_coord.z()};
}

/*
 * \brief Return true if node is a descendant of ancestor
 * \param[in] octant_key
 * \param[in] ancestor_key
 * \param[in] voxel_depth  Max/voxel depth of the tree the keys refer to.
 * \return True if octant_key is a descendant of the ancestor_key or if
 *         they are equal.
 */
inline bool descendant(const se::key_t octant_key,
                       const se::key_t ancestor_key,
                       const int       voxel_depth) {
  const int depth = se::keyops::depth(octant_key);
  const int ancestor_depth = se::keyops::depth(ancestor_key);
  // MAX_BITS is used for the maximum voxel depth given the size of the se::key_t
  const int idx = MAX_BITS - voxel_depth + ancestor_depth - 1;
  const se::key_t ancestor_key_code = se::keyops::code(ancestor_key);
  const se::key_t octant_key_code = se::keyops::code(octant_key) & MASK[idx];
  return (depth >= ancestor_depth) && (ancestor_key_code ^ octant_key_code) == 0;
}

/*
 * \brief Computes the parent's morton code of a given octant
 * \param octant_key
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline se::key_t parent(const se::key_t& octant_key, const int voxel_depth) {
  const int depth = se::keyops::depth(octant_key) - 1;
  const int idx = MAX_BITS - voxel_depth + depth - 1;
  return (octant_key & MASK[idx]) | depth;
}

/*
 * \brief Computes the octants's id in its local brotherhood
 * \param octant_key
 * \param depth of octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline int child_idx(se::key_t octant_key, const int depth,
    const int voxel_depth) {
  int shift = voxel_depth - depth;
  octant_key = se::keyops::code(octant_key) >> shift*3;
  int idx = (octant_key & 0x01) | (octant_key & 0x02) | (octant_key & 0x04);
  return idx;
}

/*
 * \brief Computes the octants's id in its local brotherhood
 * \param octant_key
 * \param depth of octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline int child_idx(se::key_t octant_key, const int voxel_depth) {
  int shift = voxel_depth - se::keyops::depth(octant_key);
  octant_key = se::keyops::code(octant_key) >> shift*3;
  int idx = (octant_key & 0x01) | (octant_key & 0x02) | (octant_key & 0x04);
  return idx;
}

/*
 * \brief Computes the octants's corner which is not shared with its siblings
 * \param octant_key
 * \param depth of octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline Eigen::Vector3i far_corner(const se::key_t octant_key, const int depth,
    const int voxel_depth) {
  const unsigned int octant_size = 1 << (voxel_depth - depth);
  const int child_idx = se::child_idx(octant_key, depth, voxel_depth);
  const Eigen::Vector3i octant_coord = se::keyops::decode(octant_key);
  return Eigen::Vector3i(octant_coord.x() + ( child_idx & 1)       * octant_size,
                         octant_coord.y() + ((child_idx & 2) >> 1) * octant_size,
                         octant_coord.z() + ((child_idx & 4) >> 2) * octant_size);
}

/*
 * \brief Computes the non-sibling neighbourhood around an octants. In the
 * special case in which the octant lies on an edge, neighbour are duplicated
 * as movement outside the enclosing cube is forbidden.
 * \param result 7-vector containing the neighbours
 * \param octant_key
 * \param depth of octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline void exterior_neighbours(se::key_t result[7],
    const se::key_t octant_key, const int depth, const int voxel_depth) {

  const int child_idx = se::child_idx(octant_key, depth, voxel_depth);
  Eigen::Vector3i dir = Eigen::Vector3i((child_idx & 1) ? 1 : -1,
                                        (child_idx & 2) ? 1 : -1,
                                        (child_idx & 4) ? 1 : -1);
  Eigen::Vector3i base_coord = far_corner(octant_key, depth, voxel_depth);
  dir.x() = se::math::in(base_coord.x() + dir.x() , 0, (1 << voxel_depth) - 1) ? dir.x() : 0;
  dir.y() = se::math::in(base_coord.y() + dir.y() , 0, (1 << voxel_depth) - 1) ? dir.y() : 0;
  dir.z() = se::math::in(base_coord.z() + dir.z() , 0, (1 << voxel_depth) - 1) ? dir.z() : 0;

 result[0] = se::keyops::encode(base_coord.x() + dir.x(), base_coord.y() + 0, base_coord.z() + 0,
     depth, voxel_depth);
 result[1] = se::keyops::encode(base_coord.x() + 0, base_coord.y() + dir.y(), base_coord.z() + 0,
     depth, voxel_depth);
 result[2] = se::keyops::encode(base_coord.x() + dir.x(), base_coord.y() + dir.y(), base_coord.z() + 0,
     depth, voxel_depth);
 result[3] = se::keyops::encode(base_coord.x() + 0, base_coord.y() + 0, base_coord.z() + dir.z(),
     depth, voxel_depth);
 result[4] = se::keyops::encode(base_coord.x() + dir.x(), base_coord.y() + 0, base_coord.z() + dir.z(),
     depth, voxel_depth);
 result[5] = se::keyops::encode(base_coord.x() + 0, base_coord.y() + dir.y(), base_coord.z() + dir.z(),
     depth, voxel_depth);
 result[6] = se::keyops::encode(base_coord.x() + dir.x(), base_coord.y() + dir.y(),
     base_coord.z() + dir.z(), depth, voxel_depth);
}

/*
 * \brief Computes the six face neighbours of an octant. These are stored in an
 * 4x6 matrix in which each column represents the homogeneous coordinates of a
 * neighbouring octant. The neighbours along the x axis come first, followed by
 * neighbours along the y axis and finally along the z axis. All coordinates are
 * clamped to be in the range between [0, max_size] where max size is given
 * by pow(2, voxel_depth).
 * \param res 4x6 matrix containing the neighbours
 * \param octant_coord octant coordinates
 * \param depth depth of the octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */

static inline void one_neighbourhood(Eigen::Ref<Eigen::Matrix<int, 4, 6>> res,
    const Eigen::Vector3i& octant_coord, const int depth, const int voxel_depth) {
  const Eigen::Vector3i base_coord = octant_coord;
  const int size = 1 << voxel_depth;
  const int step = 1 << (voxel_depth - depth);
  Eigen::Matrix<int, 4, 6> cross;
  res <<
    -step, step,     0,    0,     0,    0,
        0,    0, -step, step,     0,    0,
        0,    0,     0,    0, -step, step,
        0,    0,     0,    0,     0,    0;
    res.colwise() += base_coord.homogeneous();
    res = res.unaryExpr([size](const int a) {
        return std::max(std::min(a, size-1), 0);
        });
}

/*
 * \brief Computes the six face neighbours of an octant. These are stored in an
 * 4x6 matrix in which each column represents the homogeneous coordinates of a
 * neighbouring octant. The neighbours along the x axis come first, followed by
 * neighbours along the y axis and finally along the z axis. All coordinates are
 * clamped to be in the range between [0, max_size] where max size is given
 * by pow(2, voxel_depth).
 * \param res 4x6 matrix containing the neighbours
 * \param octant octant key
 * \param voxel_depth max depth of the tree on which the octant lives
 */

static inline void one_neighbourhood(Eigen::Ref<Eigen::Matrix<int, 4, 6>> res,
    const se::key_t octant_key, const int voxel_depth) {
  one_neighbourhood(res, se::keyops::decode(octant_key), se::keyops::depth(octant_key),
      voxel_depth);
}

/*
 * \brief Computes the morton number of all siblings around an octant,
 * including itself.
 * \param result 8-vector containing the neighbours
 * \param octant
 * \param voxel_depth max depth of the tree on which the octant lives
 */
inline void siblings(se::key_t result[8],
    const se::key_t octant_key, const int voxel_depth) {
  const int depth = (octant_key & SCALE_MASK);
  const int shift = 3 * (voxel_depth - depth);
  const se::key_t parent_key = parent(octant_key, voxel_depth) + 1; // set-up next depth
  for(int i = 0; i < 8; ++i) {
    result[i] = parent_key | (i << shift);
  }
}
}
#endif


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver 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.
 *
 *     PCMSolver 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 PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef GREENSFUNCTION_HPP
#define GREENSFUNCTION_HPP

#include <iosfwd>

#include "Config.hpp"

#include <Eigen/Core>

#include "DerivativeTypes.hpp"
#include "IGreensFunction.hpp"
#include "utils/Stencils.hpp"

/*! \file GreensFunction.hpp
 *  \class GreensFunction
 *  \brief Templated interface for Green's functions
 *  \author Luca Frediani and Roberto Di Remigio
 *  \date 2012-2014
 *  \tparam DerivativeTraits evaluation strategy for the function and its derivatives
 *  \tparam IntegratorPolicy policy for the calculation of the matrix represenation of S and D
 *  \tparam ProfilePolicy    dielectric profile type
 */

template <typename DerivativeTraits,
          typename IntegratorPolicy,
          typename ProfilePolicy,
          typename Derived>
class GreensFunction: public IGreensFunction
{
public:
    GreensFunction() : delta_(1.0e-04), integrator_(IntegratorPolicy()) {}
    GreensFunction(double f) : delta_(1.0e-04), integrator_(IntegratorPolicy(f)) {}
    virtual ~GreensFunction() {}
    /*! Returns value of the directional derivative of the
     *  Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_1}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the source point.
     *  \param[in] normal_p1 the normal vector to p1
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double derivativeSource(const Eigen::Vector3d & normal_p1,
                            const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const
    {
        DerivativeTraits t1[3], t2[3];
        t1[0] = p1(0); t1[1] = p1(1); t1[2] = p1(2);
        t1[0][1] = normal_p1(0); t1[1][1] = normal_p1(1); t1[2][1] = normal_p1(2);
        t2[0] = p2(0); t2[1] = p2(1); t2[2] = p2(2);
        return this->operator()(t1, t2)[1];
    }
    /*! Returns value of the directional derivative of the
     *  Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the probe point.
     *  \param[in] normal_p2 the normal vector to p2
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double derivativeProbe(const Eigen::Vector3d & normal_p2,
                                   const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __final
    {
        DerivativeTraits t1[3], t2[3];
        t1[0] = p1(0); t1[1] = p1(1); t1[2] = p1(2);
        t2[0] = p2(0); t2[1] = p2(1); t2[2] = p2(2);
        t2[0][1] = normal_p2(0); t2[1][1] = normal_p2(1); t2[2][1] = normal_p2(2);
        return this->operator()(t1, t2)[1];
    }
    /*! Returns full gradient of Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  Notice that this method returns the gradient with respect to the source point.
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    Eigen::Vector3d gradientSource(const Eigen::Vector3d & p1,
                                           const Eigen::Vector3d & p2) const
    {
        return (Eigen::Vector3d() << derivativeSource(Eigen::Vector3d::UnitX(), p1, p2),
                derivativeSource(Eigen::Vector3d::UnitY(), p1, p2),
                derivativeSource(Eigen::Vector3d::UnitZ(), p1, p2)).finished();
    }
    /*! Returns full gradient of Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  Notice that this method returns the gradient with respect to the probe point.
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    Eigen::Vector3d gradientProbe(const Eigen::Vector3d & p1,
                                          const Eigen::Vector3d & p2) const
    {
        return (Eigen::Vector3d() << derivativeProbe(Eigen::Vector3d::UnitX(), p1, p2),
                derivativeProbe(Eigen::Vector3d::UnitY(), p1, p2),
                derivativeProbe(Eigen::Vector3d::UnitZ(), p1, p2)).finished();
    }

    /*! Whether the Green's function describes a uniform environment */
    virtual bool uniform() const __final __override { return profiles::uniform(this->profile_); }
    /*! Returns a dielectric permittivity profile */
    virtual Permittivity permittivity() const __final __override { return this->profile_; }

    friend std::ostream & operator<<(std::ostream & os, GreensFunction & gf) {
        return gf.printObject(os);
    }
protected:
    /*! Evaluates the Green's function given a pair of points
     *  \param[in] source the source point
     *  \param[in]  probe the probe point
     */
    virtual DerivativeTraits operator()(DerivativeTraits * source, DerivativeTraits * probe) const = 0;
    /*! Returns value of the kernel of the \f$\mathcal{S}\f$ integral operator, i.e. the value of the
     *  Greens's function for the pair of points p1, p2: \f$ G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     *  \note Relies on the implementation of operator() in the subclasses and that is all subclasses
     *  need to implement. Thus this method is marked __final.
     */
    virtual double kernelS_impl(const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __final __override
    {
        DerivativeTraits sp[3], pp[3];
        sp[0] = p1(0); sp[1] = p1(1); sp[2] = p1(2);
        pp[0] = p2(0); pp[1] = p2(1); pp[2] = p2(2);
        return this->operator()(sp, pp)[0];
    }
    virtual std::ostream & printObject(std::ostream & os) __override
    {
        os << "Green's Function" << std::endl;
        return os;
    }
    double delta_;
    IntegratorPolicy integrator_;
    ProfilePolicy profile_;
};

template <typename IntegratorPolicy,
          typename ProfilePolicy,
          typename Derived>
class GreensFunction<Numerical, IntegratorPolicy, ProfilePolicy, Derived>: public IGreensFunction
{
public:
    GreensFunction() : delta_(1.0e-04), integrator_(IntegratorPolicy()) {}
    GreensFunction(double f) : delta_(1.0e-04), integrator_(IntegratorPolicy(f)) {}
    virtual ~GreensFunction() {}
    /*! Returns value of the directional derivative of the
     *  Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_1}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the source point.
     *  \param[in] normal_p1 the normal vector to p1
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double derivativeSource(const Eigen::Vector3d & normal_p1,
                            const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const
    {
        return threePointStencil(pcm::bind(&GreensFunction<Numerical, IntegratorPolicy, ProfilePolicy, Derived>::kernelS, this, pcm::_1, pcm::_2),
                                p1, p2, normal_p1, this->delta_);
    }
    /*! Returns value of the directional derivative of the
     *  Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the probe point.
     *  \param[in] normal_p2 the normal vector to p2
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double derivativeProbe(const Eigen::Vector3d & normal_p2,
                                   const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __final
    {
        return threePointStencil(pcm::bind(&GreensFunction<Numerical, IntegratorPolicy, ProfilePolicy, Derived>::kernelS, this, pcm::_1, pcm::_2),
                                p2, p1, normal_p2, this->delta_);
    }
    /*! Returns full gradient of Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_1}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  Notice that this method returns the gradient with respect to the source point.
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    Eigen::Vector3d gradientSource(const Eigen::Vector3d & p1,
                                           const Eigen::Vector3d & p2) const
    {
        return (Eigen::Vector3d() << derivativeSource(Eigen::Vector3d::UnitX(), p1, p2),
                derivativeSource(Eigen::Vector3d::UnitY(), p1, p2),
                derivativeSource(Eigen::Vector3d::UnitZ(), p1, p2)).finished();
    }
    /*! Returns full gradient of Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  Notice that this method returns the gradient with respect to the probe point.
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     */
    Eigen::Vector3d gradientProbe(const Eigen::Vector3d & p1,
                                          const Eigen::Vector3d & p2) const
    {
        return (Eigen::Vector3d() << derivativeProbe(Eigen::Vector3d::UnitX(), p1, p2),
                derivativeProbe(Eigen::Vector3d::UnitY(), p1, p2),
                derivativeProbe(Eigen::Vector3d::UnitZ(), p1, p2)).finished();
    }

    /*! Whether the Green's function describes a uniform environment */
    virtual bool uniform() const __final __override { return profiles::uniform(this->profile_); }
    /*! Returns a dielectric permittivity profile */
    virtual Permittivity permittivity() const __final __override { return this->profile_; }

    friend std::ostream & operator<<(std::ostream & os, GreensFunction & gf) {
        return gf.printObject(os);
    }
protected:
    /*! Evaluates the Green's function given a pair of points
     *  \param[in] source the source point
     *  \param[in]  probe the probe point
     */
    virtual Numerical operator()(Numerical * source, Numerical * probe) const = 0;
    /*! Returns value of the kernel of the \f$\mathcal{S}\f$ integral operator, i.e. the value of the
     *  Greens's function for the pair of points p1, p2: \f$ G(\mathbf{p}_1, \mathbf{p}_2)\f$
     *  \param[in] p1 first point
     *  \param[in] p2 second point
     *  \note Relies on the implementation of operator() in the subclasses and that is all subclasses
     *  need to implement. Thus this method is marked __final.
     */
    virtual double kernelS_impl(const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __final __override
    {
        return this->operator()(const_cast<Numerical *>(p1.data()), const_cast<Numerical *>(p2.data()));
    }
    virtual std::ostream & printObject(std::ostream & os) __override
    {
        os << "Green's Function" << std::endl;
        return os;
    }
    double delta_;
    IntegratorPolicy integrator_;
    ProfilePolicy profile_;
};

#endif // GREENSFUNCTION_HPP


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Filename    : sphdec.hpp                                                                            *
 * Project     : Planewalker - Schnorr-Euchner sphere decoder simulation for space-time lattice codes  *
 * Authors     : Pasi Pyrrö, Oliver Gnilke                                                             *
 * Version     : 1.0                                                                                *
 * Copyright   : Aalto University ~ School of Science ~ Department of Mathematics and Systems Analysis *
 * Date        : 17.8.2017                                                                             *
 * Language    : C++ (2011 or newer standard)                                                          *
 * Description : Function prototypes for sphdec.cpp                                                    *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef SPHDEC_HPP
#define SPHDEC_HPP

#include <armadillo>
#include <vector>
#include <complex>
#include <limits>

std::vector<int> sphdec(const arma::vec &y, const arma::mat &R, const std::vector<int> &S,
						int &counter, double radius=std::numeric_limits<double>::max());
std::vector<int> sphdec_spherical_shaping(const arma::vec &y, const arma::mat &HR, const arma::mat &R, const std::vector<int> &S,
										  int &counter, double P, double radius=std::numeric_limits<double>::max());
std::vector<int> sphdec_wrapper(const std::vector<arma::cx_mat> &bases, const arma::mat Rorig,
								const arma::cx_mat &H, const arma::cx_mat &X, const arma::cx_mat &N,
								const std::vector<int> &symbset, int &visited_nodes, double radius=std::numeric_limits<double>::max());

#endif /* SPHDEC_HPP */


#ifndef VIENNACL_LINALG_LANCZOS_HPP_
#define VIENNACL_LINALG_LANCZOS_HPP_

/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** @file viennacl/linalg/lanczos.hpp
*   @brief Generic interface for the Lanczos algorithm.
*
*   Contributed by Guenther Mader and Astrid Rupp.
*/

#include <cmath>
#include <vector>
#include "viennacl/vector.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/io/matrix_market.hpp"
#include "viennacl/linalg/bisect.hpp"
#include <boost/random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublas/io.hpp>

namespace viennacl
{
namespace linalg
{

/** @brief A tag for the lanczos algorithm.
*/
class lanczos_tag
{
public:

  enum
  {
    partial_reorthogonalization = 0,
    full_reorthogonalization,
    no_reorthogonalization
  };

  /** @brief The constructor
  *
  * @param factor                 Exponent of epsilon - tolerance for batches of Reorthogonalization
  * @param numeig                 Number of eigenvalues to be returned
  * @param met                    Method for Lanczos-Algorithm: 0 for partial Reorthogonalization, 1 for full Reorthogonalization and 2 for Lanczos without Reorthogonalization
  * @param krylov                 Maximum krylov-space size
  */

  lanczos_tag(double factor = 0.75,
              vcl_size_t numeig = 10,
              int met = 0,
              vcl_size_t krylov = 100) : factor_(factor), num_eigenvalues_(numeig), method_(met), krylov_size_(krylov) {}

  /** @brief Sets the number of eigenvalues */
  void num_eigenvalues(vcl_size_t numeig){ num_eigenvalues_ = numeig; }

    /** @brief Returns the number of eigenvalues */
  vcl_size_t num_eigenvalues() const { return num_eigenvalues_; }

    /** @brief Sets the exponent of epsilon */
  void factor(double fct) { factor_ = fct; }

  /** @brief Returns the exponent */
  double factor() const { return factor_; }

  /** @brief Sets the size of the kylov space */
  void krylov_size(vcl_size_t max) { krylov_size_ = max; }

  /** @brief Returns the size of the kylov space */
  vcl_size_t  krylov_size() const { return krylov_size_; }

  /** @brief Sets the reorthogonalization method */
  void method(int met){ method_ = met; }

  /** @brief Returns the reorthogonalization method */
  int method() const { return method_; }

private:
  double factor_;
  vcl_size_t num_eigenvalues_;
  int method_; // see enum defined above for possible values
  vcl_size_t krylov_size_;
};

namespace detail
{
  /**
  *   @brief Implementation of the Lanczos PRO algorithm
  *
  *   @param A            The system matrix
  *   @param r            Random start vector
  *   @param size         Size of krylov-space
  *   @param tag          Lanczos_tag with several options for the algorithm
  *   @return             Returns the eigenvalues (number of eigenvalues equals size of krylov-space)
  */

  template< typename MatrixT, typename VectorT >
  std::vector<
          typename viennacl::result_of::cpu_value_type<typename MatrixT::value_type>::type
          >
  lanczosPRO (MatrixT const& A, VectorT & r, vcl_size_t size, lanczos_tag const & tag)
  {
    typedef typename viennacl::result_of::value_type<MatrixT>::type        ScalarType;
    typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type    CPU_ScalarType;

    // generation of some random numbers, used for lanczos PRO algorithm
    boost::mt11213b mt;
    boost::normal_distribution<CPU_ScalarType> N(0, 1);
    boost::bernoulli_distribution<CPU_ScalarType> B(0.5);
    boost::triangle_distribution<CPU_ScalarType> T(-1, 0, 1);

    boost::variate_generator<boost::mt11213b&, boost::normal_distribution<CPU_ScalarType> >     get_N(mt, N);
    boost::variate_generator<boost::mt11213b&, boost::bernoulli_distribution<CPU_ScalarType> >  get_B(mt, B);
    boost::variate_generator<boost::mt11213b&, boost::triangle_distribution<CPU_ScalarType> >   get_T(mt, T);

    long i, k, retry, reorths;
    std::vector<long> l_bound(size/2), u_bound(size/2);
    bool second_step;
    CPU_ScalarType squ_eps, eta, temp, eps, retry_th;
    vcl_size_t n = r.size();
    std::vector< std::vector<CPU_ScalarType> > w(2, std::vector<CPU_ScalarType>(size));
    CPU_ScalarType cpu_beta;

    boost::numeric::ublas::vector<CPU_ScalarType> s(n);

    VectorT t(n);
    CPU_ScalarType inner_rt;
    ScalarType vcl_beta;
    ScalarType vcl_alpha;
    std::vector<CPU_ScalarType> alphas, betas;
    boost::numeric::ublas::matrix<CPU_ScalarType> Q(n, size);

    second_step = false;
    eps = std::numeric_limits<CPU_ScalarType>::epsilon();
    squ_eps = std::sqrt(eps);
    retry_th = 1e-2;
    eta = std::exp(std::log(eps) * tag.factor());
    reorths = 0;
    retry = 0;

    vcl_beta = viennacl::linalg::norm_2(r);

    r /= vcl_beta;

    detail::copy_vec_to_vec(r,s);
    boost::numeric::ublas::column(Q, 0) = s;

    VectorT u = viennacl::linalg::prod(A, r);
    vcl_alpha = viennacl::linalg::inner_prod(u, r);
    alphas.push_back(vcl_alpha);
    w[0][0] = 1;
    betas.push_back(vcl_beta);

    long batches = 0;
    for (i = 1;i < static_cast<long>(size); i++)
    {
      r = u - vcl_alpha * r;
      vcl_beta = viennacl::linalg::norm_2(r);

      betas.push_back(vcl_beta);
      r = r / vcl_beta;

      vcl_size_t index = vcl_size_t(i % 2);
      w[index][vcl_size_t(i)] = 1;
      k = (i + 1) % 2;
      w[index][0] = (betas[1] * w[vcl_size_t(k)][1] + (alphas[0] - vcl_alpha) * w[vcl_size_t(k)][0] - betas[vcl_size_t(i) - 1] * w[index][0]) / vcl_beta + eps * 0.3 * get_N() * (betas[1] + vcl_beta);

      for (vcl_size_t j = 1; j < vcl_size_t(i - 1); j++)
      {
              w[index][j] = (betas[j + 1] * w[vcl_size_t(k)][j + 1] + (alphas[j] - vcl_alpha) * w[vcl_size_t(k)][j] + betas[j] * w[vcl_size_t(k)][j - 1] - betas[vcl_size_t(i) - 1] * w[index][j]) / vcl_beta + eps * 0.3 * get_N() * (betas[j + 1] + vcl_beta);
      }
      w[index][vcl_size_t(i) - 1] = 0.6 * eps * CPU_ScalarType(n) * get_N() * betas[1] / vcl_beta;

      if (second_step)
      {
        for (vcl_size_t j = 0; j < vcl_size_t(batches); j++)
        {
          l_bound[vcl_size_t(j)]++;
          u_bound[vcl_size_t(j)]--;

          for (k = l_bound[j];k < u_bound[j];k++)
          {
            detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, vcl_size_t(k)), t);
            inner_rt = viennacl::linalg::inner_prod(r,t);
            r = r - inner_rt * t;
            w[index][vcl_size_t(k)] = 1.5 * eps * get_N();
            reorths++;
          }
        }
        temp = viennacl::linalg::norm_2(r);
        r = r / temp;
        vcl_beta = vcl_beta * temp;
        second_step = false;
      }
      batches = 0;

      for (vcl_size_t j = 0; j < vcl_size_t(i); j++)
      {
        if (std::fabs(w[index][j]) >= squ_eps)
        {
          detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, j), t);
          inner_rt = viennacl::linalg::inner_prod(r,t);
          r = r - inner_rt * t;
          w[index][j] = 1.5 * eps * get_N();
          k = long(j) - 1;
          reorths++;
          while (k >= 0 && std::fabs(w[index][vcl_size_t(k)]) > eta)
          {
            detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, vcl_size_t(k)), t);
            inner_rt = viennacl::linalg::inner_prod(r,t);
            r = r - inner_rt * t;
            w[index][vcl_size_t(k)] = 1.5 * eps * get_N();
            k--;
            reorths++;
          }
          l_bound[vcl_size_t(batches)] = k + 1;
          k = long(j) + 1;

          while (k < i && std::fabs(w[index][vcl_size_t(k)]) > eta)
          {
            detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, vcl_size_t(k)), t);
            inner_rt = viennacl::linalg::inner_prod(r,t);
            r = r - inner_rt * t;
            w[index][vcl_size_t(k)] = 1.5 * eps * get_N();
            k++;
            reorths++;
          }
          u_bound[vcl_size_t(batches)] = k - 1;
          batches++;
          j = vcl_size_t(k);
        }
      }

      if (batches > 0)
      {
        temp = viennacl::linalg::norm_2(r);
        r = r / temp;
        vcl_beta = vcl_beta * temp;
        second_step = true;

        while (temp < retry_th)
        {
          for (vcl_size_t j = 0; j < vcl_size_t(i); j++)
          {
            detail::copy_vec_to_vec(boost::numeric::ublas::column(Q, vcl_size_t(k)), t);
            inner_rt = viennacl::linalg::inner_prod(r,t);
            r = r - inner_rt * t;
            reorths++;
          }
          retry++;
          temp = viennacl::linalg::norm_2(r);
          r = r / temp;
          vcl_beta = vcl_beta * temp;
        }
      }

      detail::copy_vec_to_vec(r,s);
      boost::numeric::ublas::column(Q, vcl_size_t(i)) = s;

      cpu_beta = vcl_beta;
      s = - cpu_beta * boost::numeric::ublas::column(Q, vcl_size_t(i - 1));
      detail::copy_vec_to_vec(s, u);
      u += viennacl::linalg::prod(A, r);
      vcl_alpha = viennacl::linalg::inner_prod(u, r);
      alphas.push_back(vcl_alpha);
    }

    return bisect(alphas, betas);
  }

  /**
  *   @brief Implementation of the lanczos algorithm without reorthogonalization
  *
  *   @param A            The system matrix
  *   @param r            Random start vector
  *   @param size         Size of krylov-space
  *   @return             Returns the eigenvalues (number of eigenvalues equals size of krylov-space)
  */
  template<typename MatrixT, typename VectorT>
  std::vector<
          typename viennacl::result_of::cpu_value_type<typename MatrixT::value_type>::type
          >
  lanczos (MatrixT const& A, VectorT & r, vcl_size_t size, lanczos_tag)
  {
    typedef typename viennacl::result_of::value_type<MatrixT>::type        ScalarType;
    typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type    CPU_ScalarType;

    ScalarType vcl_beta;
    ScalarType vcl_alpha;
    std::vector<CPU_ScalarType> alphas, betas;
    CPU_ScalarType norm;
    vcl_size_t n = r.size();
    VectorT u(n), t(n);
    boost::numeric::ublas::vector<CPU_ScalarType> s(r.size()), u_zero(n), q(n);
    boost::numeric::ublas::matrix<CPU_ScalarType> Q(n, size);

    u_zero = boost::numeric::ublas::zero_vector<CPU_ScalarType>(n);
    detail::copy_vec_to_vec(u_zero, u);
    norm = norm_2(r);

    for (vcl_size_t i = 0;i < size; i++)
    {
      r /= norm;
      vcl_beta = norm;

      detail::copy_vec_to_vec(r,s);
      boost::numeric::ublas::column(Q, i) = s;

      u += prod(A, r);
      vcl_alpha = inner_prod(u, r);
      r = u - vcl_alpha * r;
      norm = norm_2(r);

      q = boost::numeric::ublas::column(Q, i);
      detail::copy_vec_to_vec(q, t);

      u = - norm * t;
      alphas.push_back(vcl_alpha);
      betas.push_back(vcl_beta);
      s.clear();
    }

    return bisect(alphas, betas);
  }

  /**
  *   @brief Implementation of the Lanczos FRO algorithm
  *
  *   @param A            The system matrix
  *   @param r            Random start vector
  *   @param size         Size of krylov-space
  *   @return             Returns the eigenvalues (number of eigenvalues equals size of krylov-space)
  */
  template< typename MatrixT, typename VectorT >
  std::vector<
          typename viennacl::result_of::cpu_value_type<typename MatrixT::value_type>::type
          >
  lanczosFRO (MatrixT const& A, VectorT & r, vcl_size_t size, lanczos_tag)
  {
    typedef typename viennacl::result_of::value_type<MatrixT>::type            NumericType;
    typedef typename viennacl::result_of::cpu_value_type<NumericType>::type    CPU_NumericType;

    CPU_NumericType temp;
    CPU_NumericType norm;
    NumericType vcl_beta;
    NumericType vcl_alpha;
    std::vector<CPU_NumericType> alphas, betas;
    vcl_size_t n = r.size();
    VectorT u(n), t(n);
    NumericType inner_rt;
    boost::numeric::ublas::vector<CPU_NumericType> u_zero(n), s(r.size()), q(n);
    boost::numeric::ublas::matrix<CPU_NumericType> Q(n, size);

    long reorths = 0;
    norm = norm_2(r);

    for (vcl_size_t i = 0; i < size; i++)
    {
      r /= norm;

      for (vcl_size_t j = 0; j < i; j++)
      {
        q = boost::numeric::ublas::column(Q, j);
        detail::copy_vec_to_vec(q, t);
        inner_rt = viennacl::linalg::inner_prod(r,t);
        r = r - inner_rt * t;
        reorths++;
      }
      temp = viennacl::linalg::norm_2(r);
      r = r / temp;
      vcl_beta = temp * norm;
      detail::copy_vec_to_vec(r,s);
      boost::numeric::ublas::column(Q, i) = s;

      u += viennacl::linalg::prod(A, r);
      vcl_alpha = viennacl::linalg::inner_prod(u, r);
      r = u - vcl_alpha * r;
      norm = viennacl::linalg::norm_2(r);
      q = boost::numeric::ublas::column(Q, i);
      detail::copy_vec_to_vec(q, t);
      u = - norm * t;
      alphas.push_back(vcl_alpha);
      betas.push_back(vcl_beta);
    }

    return bisect(alphas, betas);
  }

} // end namespace detail

/**
*   @brief Implementation of the calculation of eigenvalues using lanczos
*
*   @param matrix        The system matrix
*   @param tag           Tag with several options for the lanczos algorithm
*   @return              Returns the n largest eigenvalues (n defined in the lanczos_tag)
*/
template<typename MatrixT>
std::vector< typename viennacl::result_of::cpu_value_type<typename MatrixT::value_type>::type >
eig(MatrixT const & matrix, lanczos_tag const & tag)
{
  typedef typename viennacl::result_of::value_type<MatrixT>::type           NumericType;
  typedef typename viennacl::result_of::cpu_value_type<NumericType>::type   CPU_NumericType;
  typedef typename viennacl::result_of::vector_for_matrix<MatrixT>::type    VectorT;

  boost::mt11213b mt;
  boost::normal_distribution<CPU_NumericType>    N(0, 1);
  boost::bernoulli_distribution<CPU_NumericType> B(0.5);
  boost::triangle_distribution<CPU_NumericType>  T(-1, 0, 1);

  boost::variate_generator<boost::mt11213b&, boost::normal_distribution<CPU_NumericType> >     get_N(mt, N);
  boost::variate_generator<boost::mt11213b&, boost::bernoulli_distribution<CPU_NumericType> >  get_B(mt, B);
  boost::variate_generator<boost::mt11213b&, boost::triangle_distribution<CPU_NumericType> >   get_T(mt, T);

  std::vector<CPU_NumericType> eigenvalues;
  vcl_size_t matrix_size = matrix.size1();
  VectorT r(matrix_size);
  std::vector<CPU_NumericType> s(matrix_size);

  for (vcl_size_t i=0; i<s.size(); ++i)
    s[i] = 3.0 * get_B() + get_T() - 1.5;

  detail::copy_vec_to_vec(s,r);

  vcl_size_t size_krylov = (matrix_size < tag.krylov_size()) ? matrix_size
                                                              : tag.krylov_size();

  switch (tag.method())
  {
    case lanczos_tag::partial_reorthogonalization:
      eigenvalues = detail::lanczosPRO(matrix, r, size_krylov, tag);
      break;
    case lanczos_tag::full_reorthogonalization:
      eigenvalues = detail::lanczosFRO(matrix, r, size_krylov, tag);
      break;
    case lanczos_tag::no_reorthogonalization:
      eigenvalues = detail::lanczos(matrix, r, size_krylov, tag);
      break;
  }

  std::vector<CPU_NumericType> largest_eigenvalues;

  for (vcl_size_t i = 1; i<=tag.num_eigenvalues(); i++)
    largest_eigenvalues.push_back(eigenvalues[size_krylov-i]);

  return largest_eigenvalues;
}

} // end namespace linalg
} // end namespace viennacl
#endif


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include "SiconosConfig.h"

#pragma GCC diagnostic ignored "-Wunused-local-typedefs"

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

//#define BIND_FORTRAN_LOWERCASE_UNDERSCORE
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/lapack.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

namespace lapack = boost::numeric::bindings::lapack;

#include "SiconosVector.hpp"
#include "cholesky.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"

using namespace Siconos;

void SimpleMatrix::PLUFactorizationInPlace()
{
  if (_isPLUFactorized)
  {
    std::cout << "SimpleMatrix::PLUFactorizationInPlace warning: this matrix is already PLUFactorized. " << std::endl;
    return;
  }
  if (_num == 1)
  {
    if (!_ipiv)
      _ipiv.reset(new VInt(size(0)));
    else
      _ipiv->resize(size(0));
    int info = lapack::getrf(*mat.Dense, *_ipiv);
    if (info != 0)
    {
      _isPLUFactorized = false;
      SiconosMatrixException::selfThrow("SimpleMatrix::PLUFactorizationInPlace failed: the matrix is singular.");
    }
    else _isPLUFactorized = true;
  }
  else
  {
    int info = cholesky_decompose(*sparse());
    // \warning: VA 24/11/2010: work only for symmetric matrices. Should be replaced by efficient implementatation (e.g. mumps )
    if (info != 0)
    {
      display();
      _isPLUFactorized = false;
      std::cout << "Problem in Cholesky Decomposition for the row number" << info   << std::endl;
      SiconosMatrixException::selfThrow("SimpleMatrix::PLUFactorizationInPlace failed. ");
    }
    else _isPLUFactorized = true;
  }

}

void SimpleMatrix::PLUInverseInPlace()
{
  if (!_isPLUFactorized)
    PLUFactorizationInPlace();
  if (_num != 1)
    SiconosMatrixException::selfThrow(" SimpleMatrix::PLUInverseInPlace: only implemented for dense matrices.");

#if defined(HAVE_ATLAS) && defined(OUTSIDE_FRAMEWORK_BLAS)
  int info = lapack::getri(*mat.Dense, *_ipiv);   // solve from factorization

  if (info != 0)
    SiconosMatrixException::selfThrow("SimpleMatrix::PLUInverseInPlace failed, the matrix is singular.");

  _isPLUInversed = true;
#else
  SiconosMatrixException::selfThrow("SimpleMatrix::PLUInverseInPlace not implemented with lapack.");
#endif
}

void SimpleMatrix::PLUForwardBackwardInPlace(SiconosMatrix &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix PLUForwardBackwardInPlace(B) failed at solving Ax = B. Not yet implemented for a BlockMatrix B.");
  int info = 0;

  if (_num == 1)
  {
    if (!_isPLUFactorized) // call gesv => LU-factorize+solve
    {
      // solve system:
      if (!_ipiv)
        _ipiv.reset(new VInt(size(0)));
      else
        _ipiv->resize(size(0));
      info = lapack::gesv(*mat.Dense, *_ipiv, *(B.dense()));
      _isPLUFactorized = true;

      /*
        ublas::vector<double> S(std::max(size(0),size(1)));
        ublas::matrix<double, ublas::column_major> U(size(0),size(1));
        ublas::matrix<double, ublas::column_major> VT(size(0),size(1));

        int ierr = lapack::gesdd(*mat.Dense, S, U, VT);
        printf("info = %d, ierr = %d, emax = %f, emin = %f , cond = %f\n",info,ierr,S(0),S(2),S(0)/S(2));
      */
      // B now contains solution:
    }
    else // call getrs: only solve using previous lu-factorization
      if (B.num() == 1)
        info = lapack::getrs(*mat.Dense, *_ipiv, *(B.dense()));
      else
        SiconosMatrixException::selfThrow(" SimpleMatrix::PLUInverseInPlace: only implemented for dense matrices in RHS.");
  }
  else
  {
    if (!_isPLUFactorized) // call first PLUFactorizationInPlace
    {
      PLUFactorizationInPlace();
    }
    // and then solve
    if (B.num() == 1)
    {
      inplace_solve(*sparse(), *(B.dense()), ublas::lower_tag());
      inplace_solve(ublas::trans(*sparse()), *(B.dense()), ublas::upper_tag());
    }
    else if (B.num() == 4)
    {
      inplace_solve(*sparse(), *(B.sparse()), ublas::lower_tag());
      inplace_solve(ublas::trans(*sparse()), *(B.sparse()), ublas::upper_tag());
    }
    else
      SiconosMatrixException::selfThrow(" SimpleMatrix::PLUInverseInPlace: only implemented for dense ans sparse matrices in RHS.");
    info = 0 ;
  }
  //  SiconosMatrixException::selfThrow(" SimpleMatrix::PLUInverseInPlace: only implemented for dense matrices.");

  if (info != 0)
    SiconosMatrixException::selfThrow("SimpleMatrix::PLUForwardBackwardInPlace failed.");
}

void SimpleMatrix::PLUForwardBackwardInPlace(SiconosVector &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix PLUForwardBackwardInPlace(V) failed. Not yet implemented for V being a BlockVector.");

  DenseMat tmpB(B.size(), 1);
  ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
  int info;

  if (_num == 1)
  {
    if (!_isPLUFactorized) // call gesv => LU-factorize+solve
    {
      // solve system:
      if (!_ipiv)
        _ipiv.reset(new VInt(size(0)));
      else
        _ipiv->resize(size(0));

      info = lapack::gesv(*mat.Dense, *_ipiv, tmpB);
      _isPLUFactorized = true;

      /*
        ublas::matrix<double> COPY(*mat.Dense);
        ublas::vector<double> S(std::max(size(0),size(1)));
        ublas::matrix<double, ublas::column_major> U(size(0),size(1));
        ublas::matrix<double, ublas::column_major> VT(size(0),size(1));

        int ierr = lapack::gesdd(COPY, S, U, VT);
        printf("info = %d, ierr = %d, emax = %f, emin = %f , cond = %f\n",info,ierr,S(0),S(2),S(0)/S(2));
      */
      // B now contains solution:
    }
    else // call getrs: only solve using previous lu-factorization
      info = lapack::getrs(*mat.Dense, *_ipiv, tmpB);
  }
  else
  {
    if (!_isPLUFactorized) // call first PLUFactorizationInPlace
    {
      PLUFactorizationInPlace();
    }
    // and then solve
    inplace_solve(*sparse(), tmpB, ublas::lower_tag());
    inplace_solve(ublas::trans(*sparse()), tmpB, ublas::upper_tag());
    info = 0;
  }
  if (info != 0)
    SiconosMatrixException::selfThrow("SimpleMatrix::PLUForwardBackwardInPlace failed.");
  else
  {
    noalias(*(B.dense())) = ublas::column(tmpB, 0);
  }
}

void SimpleMatrix::resetLU()
{
  if (_ipiv) _ipiv->clear();
  _isPLUFactorized = false;
  _isPLUInversed = false;
}

void SimpleMatrix::resetQR()
{
  _isQRFactorized = false;

}
// const SimpleMatrix operator * (const SimpleMatrix & A, const SimpleMatrix& B )
// {
//   return (DenseMat)prod(*A.dense() , *B.dense());
//   //  return A;
// }

void SimpleMatrix::SolveByLeastSquares(SiconosMatrix &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares(Siconos Matrix &B) failed. Not yet implemented for M being a BlockMatrix.");
  int info = 0;
#ifdef USE_OPTIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, *(B.dense()), lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, *(B.dense()), lapack::minimal_workspace());
#endif
  if (info != 0)
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares failed.");
}

void SimpleMatrix::SolveByLeastSquares(SiconosVector &B)
{
  if (B.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares(SiconosVector &B) failed. Not yet implemented for V being a BlockVector.");

  DenseMat tmpB(B.size(), 1);
  ublas::column(tmpB, 0) = *(B.dense()); // Conversion of vector to matrix. Temporary solution.
  int info = 0;

#ifdef USE_OPTIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, tmpB, lapack::optimal_workspace());
#endif
#ifdef USE_MINIMAL_WORKSPACE
  info += lapack::gels(*mat.Dense, tmpB, lapack::minimal_workspace());
#endif
  if (info != 0)
  {
    std::cout << "info = " << info << std::endl;
    SiconosMatrixException::selfThrow("SimpleMatrix::SolveByLeastSquares failed.");
  }
  else
  {
    noalias(*(B.dense())) = ublas::column(tmpB, 0);
  }

}

/*
void polePlacement(const SiconosMatrix& A, const SiconosVector& B, SiconosVector& P, bool transpose)
{
  unsigned int n = A.size(0);
  DenseMat AA(n, n);
  DenseMat Q(n, n);
  DenseVect tau(n);
  DenseVect BB(n);
  noalias(AA) = (*A.dense());
  lapack::gehrd(1, n, AA, tau);
  lapack::orghr(n, 1, n, Q, tau);
  noalias(BB) = prod(Q, *B.dense());
}
*/


/*-------------------------------------------------------------
Copyright 2019 Wenxin Liu, Kartik Mohta, Giuseppe Loianno

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
--------------------------------------------------------------*/

#include "sdd_vio/vo_stereo.h"

#include <iostream>
#include "sdd_vio/utils/math_utils.h"
#include <algorithm>
#include <math.h>
#include <ros/ros.h>
#include <cmath>
#include <assert.h>
#include <boost/thread.hpp>
#include "sdd_vio/utils/ros_params_helper.h"
#include "sdd_vio/utils/timer.hpp"
#include <ctime>
#include <opencv2/core/eigen.hpp>

namespace sdd_vio {

/*
 * Optimization using Forward Compositional Approach
 *
 * Input:
 *      img_curr: current frame image
 *      ilay: the index of tracking layer in the image pyramid. 0 is the largest layer for tracking.
 * Return:
 *      T_curr: current transformation matrix from keyframe to current frame
 *      T_wb: transformation between current robot frame to world frame
 *      T_: transformation between current camera frame to initial camera frame
 *      v_: velocity of current body frame
 * Use of private class variables:
 *      feat_3D_[ilay]: the vector of 3D points in keyframe
 *      intensities_[ilay]: the vector of pixel intensities in keyframe
 *      mask_[ilay]: indicator of whether points are in view
 * LMA params as private variables:
 *      lambda_
 *      up_factor_
 *      down_factor_
 *
 */
    void VoStereo::optimize_fca(const cv::Mat& img_curr, Eigen::Isometry3f& T_curr, Eigen::Isometry3f& T_wb, Eigen::Isometry3f& T, Eigen::Vector3f& v, int ilay) {

        /* obtain gradient image of the current frame */
        cv::Mat Gx_mat, Gy_mat;
        cv::Sobel(img_curr, Gx_mat, CV_32F, 1, 0, 3);
        cv::Sobel(img_curr, Gy_mat, CV_32F, 0, 1, 3);

        /* masked Jacobian matrix and error vector - remove points outside projection */
        Eigen::MatrixXf J_masked;  // every time J_masked is re-calculated for every pixel inside the view
        Eigen::VectorXf error_masked;
        Eigen::VectorXf W;  // diagonal weight matrix for Huber estimation

        /* Jacobian and error vector of IMU */
        Eigen::MatrixXf J_imu;
        Eigen::VectorXf r_imu;
        Eigen::MatrixXf Z_imu;  // inverse covariance of imu errors
        J_imu.resize(9,9);
        initJacobian_imu_fca(J_imu);
        r_imu.resize(9);
//        Eigen::VectorXf inv_cov_imu(9);
//        inv_cov_imu << weight_R_,weight_R_,weight_R_, weight_v_,weight_v_,weight_v_,weight_p_,weight_p_,weight_p_;
//        Z_imu = inv_cov_imu.asDiagonal();
        Z_imu = Cov_meas_.inverse();
//        std::cout<<"Z_imu:\n"<<Z_imu<<"\n";

        /* Levenberg Marquardt */
        float lambda = lambda_;  // initial lambda is equal to the initial one set
        float up = up_factor_;
        float down = down_factor_;
        bool if_break = false;

        int npts_in;  // number of points remained in view
        perc_ = 1.;  // percentage of points remained in view
        float err_init, err_last = 0;

        /* begins optimization iteration */
        for (int iter=0; iter<max_num_iter_; ++iter)
        {
            /* first iteration */
            if (iter == 0) {

                /* calculate error and Jacobian given initial transformation */
                getError_fca(img_curr, Gx_mat, Gy_mat, T_curr, T_wb, J_masked, error_masked, W, npts_in, ilay);
                getError_imu_fca(T_wb, v, r_imu, J_imu);

                perc_ = 100*npts_in/num_feat_pts_[ilay];
                float err_mean = get_error_mean(error_masked, r_imu);
                err_init = err_mean;
                err_last = err_mean;

            }

            using Mat9 = Eigen::Matrix<float, 9, 9>;
            using Vec9 = Eigen::Matrix<float, 9, 1>;

            if (use_lma_ == false)
            {
                /* obtain H and b */
                const Mat9 H = beta_*J_masked.transpose() * W.asDiagonal() * J_masked + alpha_ * (J_imu.transpose() * Z_imu * J_imu);  // H = J^t * W * J
                const Vec9 b = beta_*J_masked.transpose() * (W.asDiagonal() * error_masked) + alpha_ * (J_imu.transpose() * (Z_imu * r_imu));  // b = J^t * w * e

                /* get delta increment */
                const Vec9 delta = -H.ldlt().solve(b);
//                std::cout<<"delta: \n"<<delta<<"\n";

                update_optimization_step(delta, T_wb, v, T, T_curr);

                /* calculate error vector and Jacobian given transformation */
                getError_fca(img_curr, Gx_mat, Gy_mat, T_curr, T_wb, J_masked, error_masked, W, npts_in, ilay);
                getError_imu_fca(T_wb, v, r_imu, J_imu);
//                std::cout << "r_imu: \n"<<r_imu<<"\n";

                perc_ = 100*npts_in/num_feat_pts_[ilay];
                float err_mean = get_error_mean(error_masked, r_imu);
                float err_diff = (err_mean - err_last)/err_last;
                err_last = err_mean;

                /* terminal output */
                if (verbose_) {
                    std::cout << "iteration = "<<iter<<", err = "<< err_mean<<", derr = "<<err_diff<<std::endl;
                }

                /* Stop condition - if error is going up or decreased below target or reach max iterations */
                if (err_diff>0 || -err_diff < target_derr_ || iter == max_num_iter_-1)
                    if_break = true;

                /* end of optimization */
                if (if_break) {
                    ROS_INFO_STREAM("iteration "<<iter<<"--------");
                    ROS_INFO("err/initial err: %g/%g", err_mean, err_init);
                    if (err_mean > err_init)
                      ROS_WARN_STREAM("bad convergence - err/initial err: "<<err_mean<<"/"<<err_init<<", iteration"<<iter);
                    break;
                }

            }
            else  // use lma
            {

                /* temporary T_curr, is error goes up in subloop can switch back */
                Eigen::Isometry3f T_curr_temp;
                Eigen::Isometry3f T_wb_temp;
                Eigen::Isometry3f T_temp;
                Eigen::Vector3f v_temp;

                /* LMA */
                float mult = lambda;
                bool ill = true;  // if ill conditioned (error increase)
                int lmit = 0;  // number of iterations while ill=true

                /* obtain H and b */
                const Mat9 H = beta_* J_masked.transpose() * W.asDiagonal() * J_masked + alpha_ * (J_imu.transpose() * Z_imu * J_imu);  // H = J^t * W * J
                const Vec9 b = beta_* J_masked.transpose() * (W.asDiagonal() * error_masked) + alpha_ * (J_imu.transpose() * (Z_imu * r_imu));  // b = J^t * w * e

                float err_mean = 0, err_diff = 0;
                /* LMA wrapper of subloop */
                while ((ill==true) && (iter<max_num_iter_) && (lmit<5))
                {

                    /* lma update on H */
                    const Mat9 H_curr = H + Mat9(mult * H.diagonal().asDiagonal());

                    /* get delta increment */
                    const Vec9 delta = -H_curr.ldlt().solve(b);
//                    std::cout<<"delta: \n"<<delta<<"\n";

                    T_curr_temp = T_curr;
                    T_wb_temp = T_wb;
                    T_temp = T;
                    v_temp = v;
                    update_optimization_step(delta, T_wb_temp, v_temp, T_temp, T_curr_temp);

                    /* calculate error vector and Jacobian given transformation */
                    getError_fca(img_curr, Gx_mat, Gy_mat, T_curr_temp, T_wb_temp, J_masked, error_masked, W, npts_in, ilay);
                    getError_imu_fca(T_wb, v_temp, r_imu, J_imu);
//                    std::cout<<"J_imu: \n"<<J_imu<<"\n";
//                    std::cout << "r_imu: \n"<<r_imu<<"\n";

                    perc_ = 100*npts_in/num_feat_pts_[ilay];
                    err_mean = get_error_mean(error_masked, r_imu);
                    err_diff = (err_mean - err_last)/err_last;

                    ill = (err_diff > 0);  // if error larger than before, set ill = true, try increase lambda.

                    /* terminal output */
                    if (verbose_) {
                        std::cout << "iteration = "<<iter<<", lambda = "<<lambda<<", err = "<<
                            err_mean<<", derr = "<<err_diff<<std::endl;
                    }

                    /* if ill=true, higher lambda by up factor, count as one iteration */
                    if (ill) {
                          lambda *= up;
                        mult = lambda;
                        iter++;
                    }
                    lmit++;
                }

                err_last  = err_mean;

                /* update T with the one that makes error smaller or the last try */
                T_curr = T_curr_temp;
                T = T_temp;
                T_wb = T_wb_temp;
                v = v_temp;

                /* if error doesn't get higher for quite some time, this term will bring lambda down eventually */
                if (lambda > 1e-8)  // make sure doesn't overflow
                    lambda *= (1/down);

                /* if LM iterations didn't decrease the error, stop */
                if (ill) {
                    //ROS_WARN_STREAM("bad convergence!");
                    if_break = true;
                }

                /* if error is decreased below target or reach max iterations */
                if (-err_diff < target_derr_ || iter == max_num_iter_-1)
                    if_break = true;

                /* end of optimization */
                if (if_break) {
                    ROS_INFO_STREAM("iteration "<<iter<<"--------");
                    ROS_INFO("err/initial err: %g/%g", err_mean, err_init);
                    if (err_mean > err_init)
                      ROS_WARN_STREAM("bad convergence - err/initial err: "<<err_mean<<"/"<<err_init<<", iteration"<<iter);
                    break;
                }

            }

        } // for (int iter=0;

    }

/*
 * Update optimization targets given delta
 *
 */
    void VoStereo::update_optimization_step(const Eigen::Matrix<float, 9, 1>& delta, Eigen::Isometry3f &T_wb, Eigen::Vector3f &v, Eigen::Isometry3f &T, Eigen::Isometry3f &T_curr)
    {
        /* update current transformation */
        const Eigen::Vector3f delta_phi = delta.segment<3>(0);
        const Eigen::Matrix3f R_delta = exp_SO3(delta_phi);
        const Eigen::Vector3f t_delta = delta.segment<3>(3);
        const Eigen::Vector3f v_delta = delta.segment<3>(6);

        // update T_wb and v
        T_wb.linear() = T_wb.rotation() * R_delta;
        T_wb.translation() += t_delta;
        v += v_delta;

        // update T and T_curr
        T = T_bc_inv_ * T_wb * T_bc_;
        T_curr = T.inverse() * T_kf_;
    }

/*
 * Obtain err_mean for convergence evaluation
 *
 */
    float VoStereo::get_error_mean(const Eigen::VectorXf& error_masked, const Eigen::VectorXf& r_imu)
    {
        int npts_in = error_masked.size();
        float err_photo = error_masked.transpose()*error_masked; // sum of square errors
        // these are raw square errors - not weighed by huber weights
        // photometric part is the mean pixel intensity error, plus the IMU observation square error weighted by optimization weight alpha_
        float err_mean = beta_ * sqrt(err_photo/npts_in) / error_scale_factor_ + alpha_*(r_imu.transpose()*r_imu).value();
        return err_mean;
    }

 /*
  * initialize imu jacobian with the parts that do not change in iterations
  *
  */
    void VoStereo::initJacobian_imu_fca(Eigen::MatrixXf &J_imu)
    {
        /* these are the parts that do not change through iterations */
        J_imu.setZero();
        Eigen::Matrix3f R_i = T_wb_last_.rotation();
        J_imu.block<3,3>(6,3) = R_i.transpose();
        J_imu.block<3,3>(3,6) = R_i.transpose();
    }

/*
 * obtain imu error vector and update imu jacobian
 * Input:
 *      T_wb: current robot pose in world frame
 *      v: current robot velocity in world frame
 * Output:
 *      r_imu: 9x1 vector of imu errors
 *      J_imu: 9x9 jacobian of state (phi,p,v of current robot frame in world) with respect to imu errors
 *
 */
    void VoStereo::getError_imu_fca(const Eigen::Isometry3f &T_wb, const Eigen::Vector3f &v, Eigen::VectorXf &r_imu, Eigen::MatrixXf &J_imu)
    {

        Eigen::Matrix3f R_i = T_wb_last_.rotation();  // rotation w.r.t. world frame of last body frame
        Eigen::Matrix3f R_j = T_wb.rotation();  // rotation w.r.t. world frame of current body frame
        Eigen::Vector3f p_i = T_wb_last_.translation();  // position of last robot frame in world
        Eigen::Vector3f p_j = T_wb.translation();  // position of current robot frame in world

        Eigen::Matrix3f R_err = R_meas_.transpose() * R_i.transpose() * R_j;
        Eigen::Vector3f r_R_imu = log_SO3(R_err);
        Eigen::Vector3f r_v_imu = R_i.transpose() * (v - v_last_ - g_*t_meas_) - v_meas_;
        Eigen::Vector3f r_p_imu = R_i.transpose() * (p_j - p_i - v_last_*t_meas_ - 0.5*g_*t_meas_*t_meas_) - p_meas_;

        r_imu.block<3,1>(0,0) = r_R_imu;
        r_imu.block<3,1>(3,0) = r_v_imu;
        r_imu.block<3,1>(6,0) = r_p_imu;

        J_imu.block<3,3>(0,0) = log_Jacobian(r_R_imu);

    }

/*
 * Get error, Jacobian, and huber weights for Forward Compositional Apporach.
 *
 * Input:
 *      img_curr: current frame image, for getting pixel intensities
 *      Gx: current image gradient image on x direction
 *      Gy: current image gradient image on y direction
 *      ilay: the pyramid layer of current image
 *      T_curr: currently optimized transformation
 * Output:
 *      npts_in: the number of points falling in current camera view
 *      J_masked: Jacobian of the points in-view
 *      error_masked: error of the points in-view
 *      r_imu: 9x1 vector of IMU integrated observation error
 *      W: Huber weight for the points in-view
 * Use of class member:
 * Updates:
 *      mask_[ilay]: indicator of points in-view
 * Data-in:
 *      num_feat_pts_[ilay]
 *      feat_3D_[ilay]
 *      intensities_[ilay]
 *
 */
    void VoStereo::getError_fca(const cv::Mat& img_curr,
                                const cv::Mat& Gx_mat,
                                const cv::Mat& Gy_mat,
                                const Eigen::Isometry3f& T_curr,
                                const Eigen::Isometry3f& T_wb,
                                Eigen::MatrixXf& J_masked,
                                Eigen::VectorXf& error_masked,
                                Eigen::VectorXf& W,
                                int& npts_in,
                                const int ilay)
    {

        /* transformation of 3D point in keyframe to current frame and reproject */
        vector_aligned<Eigen::Vector3f> feat_3D_curr;  // 3D points in current camera's frame
        vector_aligned<Eigen::Vector2f> feat_pixels_curr;  // 2D pixel coordinates in current image
        transform3D(feat_3D_[ilay], feat_3D_curr, T_curr, num_feat_pts_[ilay]);
        cam_->get2DPixels(feat_3D_curr, feat_pixels_curr, num_feat_pts_[ilay], ilay);

        /* update mask and obtain gradients of reprojected points in current frame */
        npts_in = 0;
        std::vector<float> Gx, Gy;  // gradients of points in view
        std::vector<int> index;  // vector of size npts_in storing the corresponding index value
        std::vector<float> error_vec;  // error vector

        const float scale = error_scale_factor_; // Arbitrary scaling for better numerical conditioning
        for (int i=0; i<num_feat_pts_[ilay]; ++i) {
            if (inBound(feat_pixels_curr[i](0), feat_pixels_curr[i](1), 0, ilay)) {
                Gx.push_back(scale * interpolate_gradient(feat_pixels_curr[i](0), feat_pixels_curr[i](1), Gx_mat));
                Gy.push_back(scale * interpolate_gradient(feat_pixels_curr[i](0), feat_pixels_curr[i](1), Gy_mat));
                error_vec.push_back(scale * (intensities_[ilay][i]-interpolate(feat_pixels_curr[i](0),feat_pixels_curr[i](1),img_curr)));
                index.push_back(i);

//                mask_[ilay][i] = 1;
                npts_in += 1;
            }
//            else
//                mask_[ilay][i] = 0;
        }

        /* obtain photometric error for points in-view */
        error_masked = Eigen::Map<Eigen::VectorXf, Eigen::Unaligned>(error_vec.data(), error_vec.size());

        /* obtain jacobian matrix for photometric error with respect to state */
        getJacobian_fca(Gx, Gy, feat_3D_curr, index, T_curr, T_wb, J_masked, ilay, npts_in);

        if (use_huber_)
        {
            /* find standard deviation of the error vector and compute W */
            float r_mean = error_masked.mean();
            Eigen::VectorXf r_hat = error_masked.array() - r_mean;  // normalize by removing the mean (original order)
            float sigma = sqrt(((r_hat.array()).square()).sum()/npts_in);  // standard deviation
            // std::cout << "sigma: " << sigma << std::endl;

            W.resize(npts_in);
            for (int i=0; i<npts_in; ++i) {
                if (fabs(r_hat[i]) < 1.345*sigma)
                    W[i] = 1;
                else
                    W[i] = (1.345*sigma)/fabs(r_hat[i]);
                if(use_weights_)
                    W[i] = W[i] * c_/(c_+error_masked[i]*error_masked[i]);

            }
        }
        else {

            W.resize(npts_in);
            W = Eigen::VectorXf::Constant(npts_in,1);

            if(use_weights_)
                for (int i=0; i<npts_in; ++i) {
                    W[i]=c_/(c_+error_masked[i]*error_masked[i]);
                    //std::cout<<"W" << W[i];
                }
            //std::cout<<"W ="<<W;
        }

    }

/*
 * Compute the Jacobian matrix for Forward Compositional Approach (FCA)
 * Called every iteration
 *
 * Input:
 *      Gx: gradient vector in x direction of current image
 *      Gy: gradient vector in y direction of current image
 *      feat_3D_curr: 3D feature points represented in current frame, of size num_feat_pts[ilay]
 *      index: vector of index for each point in-view, of size npts_in, ranges within num_feat_pts[ilay]
 *      T_curr: current transformation from keyframe to current frame. p_curr = T_curr * p_kf
 *      T_wb: transformation from current body frame to world frame. p_w = T_wb * p_b
 *      ilay: number of layer we're optimizing on
 *      npts_in: number of points in-view
 * Output:
 *      J: jacobian of size npts_in x 6, photometric error with respect to delta rotation phi in so(3) and delta translation p.
 *
 *
 */
    void VoStereo::getJacobian_fca(const std::vector<float>& Gx, const std::vector<float>& Gy,
                                   const vector_aligned<Eigen::Vector3f>& feat_3D_curr, const std::vector<int> &index,
                                   const Eigen::Isometry3f& T_curr, const Eigen::Isometry3f& T_wb, Eigen::MatrixXf& J, const int ilay, const int npts_in)
    {
//        std::cout<<"+++++calculating jacobian \n";
//        int id_ind = 475;

        Eigen::Matrix<float,3,3> R_cb = T_bc_inv_.rotation();
        Eigen::Matrix<float,3,3> R_wb = T_wb.rotation();

        J.resize(npts_in,9);
        J.setZero();
        Eigen::Matrix<float,1,3> J_i_phi; // Jacobian of one point on rotation
        Eigen::Matrix<float,1,3> J_i_p;  // Jacobian of one point on position
        Eigen::Matrix<float,1,2> J_grad; // gradient jacobian
        Eigen::Matrix<float,2,3> J_proj; // projection jacobian
        Eigen::Matrix<float,3,3> J_SO3;  // exponential jacobian

        float fx = focal_pyr_[ilay];
        float fy = focal_pyr_[ilay];

//        std::cout<<"fx: "<<fx<<"\n";
//        bool jacobianhasnan = false;
//        bool jacobianhasinf = false;
//        bool jacobian_too_large = false;

        for (int i=0; i < npts_in; ++i)
        {
            /* calculate for each point the 1x6 Jacobian */
            Eigen::Vector3f p_curr;  // point in current frame
            p_curr(0) = feat_3D_curr[index[i]](0);
            p_curr(1) = feat_3D_curr[index[i]](1);
            p_curr(2) = feat_3D_curr[index[i]](2);

            Eigen::Vector3f p_kf;  // point in keyframe
            p_kf(0) = feat_3D_[ilay][index[i]](0);
            p_kf(1) = feat_3D_[ilay][index[i]](1);
            p_kf(2) = feat_3D_[ilay][index[i]](2);

            Eigen::Vector4f p_kf_homo = unproject3d(p_kf); // homogeneous coordinate in keyframe
            Eigen::Vector4f p_hat_homo = T_bc_ * T_curr * p_kf_homo;
            Eigen::Vector3f p_hat = project3d(p_hat_homo);  // point p_i prime in jacobian formulation

            J_grad(0,0) = Gx[i];
            J_grad(0,1) = Gy[i];

            J_proj(0,0) = fx/p_curr(2);
            J_proj(1,0) = 0;
            J_proj(0,1) = 0;
            J_proj(1,1) = fy/p_curr(2);
            J_proj(0,2) = -fx*p_curr(0)/(p_curr(2)*p_curr(2));
            J_proj(1,2) = -fy*p_curr(1)/(p_curr(2)*p_curr(2));

            J_SO3 << 0,-p_hat(2),p_hat(1),
                    p_hat(2),0,-p_hat(0),
                    -p_hat(1),p_hat(0),0;  // [p_hat]x

            J_i_phi = - J_grad * J_proj * R_cb * J_SO3;
            J_i_p = J_grad * J_proj * R_cb * R_wb.transpose();

//            if (i==id_ind)
//            {
//                std::cout<<"J_grad: \n"<<J_grad<<"\n";
//                std::cout<<"J_proj: \n"<<J_proj<<"\n";
//                std::cout<<"J_SO3: \n"<<J_SO3<<"\n";
//                std::cout<<"J_i_p: \n"<<J_i_p<<"\n";
//                std::cout<<"J_i_phi: \n"<<J_i_phi<<"\n";
//            }

            /* assign J_i to J */
            J.block<1,3>(i,0) = J_i_phi;
            J.block<1,3>(i,3) = J_i_p;

            /* for debug - will print out info if nan values found */

//            for (int k=0; k<6; ++k){
//                if (std::isnan(J(i,k)))
//                    jacobianhasnan = true;
//                if (std::isinf(J(i,k)))
//                    jacobianhasinf = true;
//                if(J(i,k) > error_scale_factor_ * 1e8)
//                {
//                    jacobian_too_large = true;
//                    ROS_WARN_STREAM("jacobian too large! i: " << i << ", Index[i]: " << index[i] << " / " << num_feat_pts_[ilay] << "\nJ.row: "<< J.row(i));
//                    std::cout<<"J_grad: \n"<<J_grad<<"\n";
//                    std::cout<<"J_proj: \n"<<J_proj<<"\n";
//                    std::cout<<"J_SO3: \n"<<J_SO3<<"\n";
//                    std::cout<<"J_i_p: \n"<<J_i_p<<"\n";
//                    std::cout<<"J_i_phi: \n"<<J_i_phi<<"\n";
//                }
//            }

            /* for debug */
        }
//        if (jacobianhasnan)
//            std::cout << "Jacobian has nan"<< std::endl;
//        if (jacobianhasinf)
//            std::cout <<"Jacobian has inf"<<std::endl;
    }

}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2006, 2008 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

/*! \file vanillaswap.hpp
    \brief Simple fixed-rate vs Libor swap
*/

#ifndef quantlib_vanilla_swap_hpp
#define quantlib_vanilla_swap_hpp

#include <ql/instruments/swap.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/schedule.hpp>
#include <boost/optional.hpp>

namespace QuantLib {

    class IborIndex;

    //! Plain-vanilla swap: fix vs floating leg
    /*! \ingroup instruments

        If no payment convention is passed, the convention of the
        floating-rate schedule is used.

        \warning if <tt>Settings::includeReferenceDateCashFlows()</tt>
                 is set to <tt>true</tt>, payments occurring at the
                 settlement date of the swap might be included in the
                 NPV and therefore affect the fair-rate and
                 fair-spread calculation. This might not be what you
                 want.

        \test
        - the correctness of the returned value is tested by checking
          that the price of a swap paying the fair fixed rate is null.
        - the correctness of the returned value is tested by checking
          that the price of a swap receiving the fair floating-rate
          spread is null.
        - the correctness of the returned value is tested by checking
          that the price of a swap decreases with the paid fixed rate.
        - the correctness of the returned value is tested by checking
          that the price of a swap increases with the received
          floating-rate spread.
        - the correctness of the returned value is tested by checking
          it against a known good value.
    */
    class VanillaSwap : public Swap {
      public:
        enum Type { Receiver = -1, Payer = 1 };
        class arguments;
        class results;
        class engine;
        VanillaSwap(
            Type type,
            Real nominal,
            const Schedule& fixedSchedule,
            Rate fixedRate,
            const DayCounter& fixedDayCount,
            const Schedule& floatSchedule,
            const boost::shared_ptr<IborIndex>& iborIndex,
            Spread spread,
            const DayCounter& floatingDayCount,
            boost::optional<BusinessDayConvention> paymentConvention =
                                                                 boost::none);
        //! \name Inspectors
        //@{
        Type type() const;
        Real nominal() const;

        const Schedule& fixedSchedule() const;
        Rate fixedRate() const;
        const DayCounter& fixedDayCount() const;

        const Schedule& floatingSchedule() const;
        const boost::shared_ptr<IborIndex>& iborIndex() const;
        Spread spread() const;
        const DayCounter& floatingDayCount() const;

        BusinessDayConvention paymentConvention() const;

        const Leg& fixedLeg() const;
        const Leg& floatingLeg() const;
        //@}

        //! \name Results
        //@{
        Real fixedLegBPS() const;
        Real fixedLegNPV() const;
        Rate fairRate() const;

        Real floatingLegBPS() const;
        Real floatingLegNPV() const;
        Spread fairSpread() const;
        //@}
        // other
        void setupArguments(PricingEngine::arguments* args) const;
        void fetchResults(const PricingEngine::results*) const;
      private:
        void setupExpired() const;
        Type type_;
        Real nominal_;
        Schedule fixedSchedule_;
        Rate fixedRate_;
        DayCounter fixedDayCount_;
        Schedule floatingSchedule_;
        boost::shared_ptr<IborIndex> iborIndex_;
        Spread spread_;
        DayCounter floatingDayCount_;
        BusinessDayConvention paymentConvention_;
        // results
        mutable Rate fairRate_;
        mutable Spread fairSpread_;
    };

    //! %Arguments for simple swap calculation
    class VanillaSwap::arguments : public Swap::arguments {
      public:
        arguments() : type(Receiver),
                      nominal(Null<Real>()) {}
        Type type;
        Real nominal;

        std::vector<Date> fixedResetDates;
        std::vector<Date> fixedPayDates;
        std::vector<Time> floatingAccrualTimes;
        std::vector<Date> floatingResetDates;
        std::vector<Date> floatingFixingDates;
        std::vector<Date> floatingPayDates;

        std::vector<Real> fixedCoupons;
        std::vector<Spread> floatingSpreads;
        std::vector<Real> floatingCoupons;
        void validate() const;
    };

    //! %Results from simple swap calculation
    class VanillaSwap::results : public Swap::results {
      public:
        Rate fairRate;
        Spread fairSpread;
        void reset();
    };

    class VanillaSwap::engine : public GenericEngine<VanillaSwap::arguments,
                                                     VanillaSwap::results> {};

    // inline definitions

    inline VanillaSwap::Type VanillaSwap::type() const {
        return type_;
    }

    inline Real VanillaSwap::nominal() const {
        return nominal_;
    }

    inline const Schedule& VanillaSwap::fixedSchedule() const {
        return fixedSchedule_;
    }

    inline Rate VanillaSwap::fixedRate() const {
        return fixedRate_;
    }

    inline const DayCounter& VanillaSwap::fixedDayCount() const {
        return fixedDayCount_;
    }

    inline const Schedule& VanillaSwap::floatingSchedule() const {
        return floatingSchedule_;
    }

    inline const boost::shared_ptr<IborIndex>& VanillaSwap::iborIndex() const {
        return iborIndex_;
    }

    inline Spread VanillaSwap::spread() const {
        return spread_;
    }

    inline const DayCounter& VanillaSwap::floatingDayCount() const {
        return floatingDayCount_;
    }

    inline BusinessDayConvention VanillaSwap::paymentConvention() const {
        return paymentConvention_;
    }

    inline const Leg& VanillaSwap::fixedLeg() const {
        return legs_[0];
    }

    inline const Leg& VanillaSwap::floatingLeg() const {
        return legs_[1];
    }

    std::ostream& operator<<(std::ostream& out,
                             VanillaSwap::Type t);

}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2007 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/iborcoupon.hpp>
#include <ql/cashflows/cashflowvectors.hpp>
#include <ql/cashflows/cashflows.hpp>
#include <ql/cashflows/couponpricer.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>

namespace QuantLib {

    inline VanillaSwap::VanillaSwap(
                     Type type,
                     Real nominal,
                     const Schedule& fixedSchedule,
                     Rate fixedRate,
                     const DayCounter& fixedDayCount,
                     const Schedule& floatSchedule,
                     const boost::shared_ptr<IborIndex>& iborIndex,
                     Spread spread,
                     const DayCounter& floatingDayCount,
                     boost::optional<BusinessDayConvention> paymentConvention)
    : Swap(2), type_(type), nominal_(nominal),
      fixedSchedule_(fixedSchedule), fixedRate_(fixedRate),
      fixedDayCount_(fixedDayCount),
      floatingSchedule_(floatSchedule), iborIndex_(iborIndex), spread_(spread),
      floatingDayCount_(floatingDayCount) {

        if (paymentConvention)
            paymentConvention_ = *paymentConvention;
        else
            paymentConvention_ = floatingSchedule_.businessDayConvention();

        legs_[0] = FixedRateLeg(fixedSchedule_)
            .withNotionals(nominal_)
            .withCouponRates(fixedRate_, fixedDayCount_)
            .withPaymentAdjustment(paymentConvention_);

        legs_[1] = IborLeg(floatingSchedule_, iborIndex_)
            .withNotionals(nominal_)
            .withPaymentDayCounter(floatingDayCount_)
            .withPaymentAdjustment(paymentConvention_)
            .withSpreads(spread_);
        for (Leg::const_iterator i = legs_[1].begin(); i < legs_[1].end(); ++i)
            registerWith(*i);

        switch (type_) {
          case Payer:
            payer_[0] = -1.0;
            payer_[1] = +1.0;
            break;
          case Receiver:
            payer_[0] = +1.0;
            payer_[1] = -1.0;
            break;
          default:
            QL_FAIL("Unknown vanilla-swap type");
        }
    }

    inline void VanillaSwap::setupArguments(PricingEngine::arguments* args) const {

        Swap::setupArguments(args);

        VanillaSwap::arguments* arguments =
            dynamic_cast<VanillaSwap::arguments*>(args);

        if (!arguments)  // it's a swap engine...
            return;

        arguments->type = type_;
        arguments->nominal = nominal_;

        const Leg& fixedCoupons = fixedLeg();

        arguments->fixedResetDates = arguments->fixedPayDates =
            std::vector<Date>(fixedCoupons.size());
        arguments->fixedCoupons = std::vector<Real>(fixedCoupons.size());

        for (Size i=0; i<fixedCoupons.size(); ++i) {
            boost::shared_ptr<FixedRateCoupon> coupon =
                boost::dynamic_pointer_cast<FixedRateCoupon>(fixedCoupons[i]);

            arguments->fixedPayDates[i] = coupon->date();
            arguments->fixedResetDates[i] = coupon->accrualStartDate();
            arguments->fixedCoupons[i] = coupon->amount();
        }

        const Leg& floatingCoupons = floatingLeg();

        arguments->floatingResetDates = arguments->floatingPayDates =
            arguments->floatingFixingDates =
            std::vector<Date>(floatingCoupons.size());
        arguments->floatingAccrualTimes =
            std::vector<Time>(floatingCoupons.size());
        arguments->floatingSpreads =
            std::vector<Spread>(floatingCoupons.size());
        arguments->floatingCoupons = std::vector<Real>(floatingCoupons.size());
        for (Size i=0; i<floatingCoupons.size(); ++i) {
            boost::shared_ptr<IborCoupon> coupon =
                boost::dynamic_pointer_cast<IborCoupon>(floatingCoupons[i]);

            arguments->floatingResetDates[i] = coupon->accrualStartDate();
            arguments->floatingPayDates[i] = coupon->date();

            arguments->floatingFixingDates[i] = coupon->fixingDate();
            arguments->floatingAccrualTimes[i] = coupon->accrualPeriod();
            arguments->floatingSpreads[i] = coupon->spread();
            try {
                arguments->floatingCoupons[i] = coupon->amount();
            } catch (Error&) {
                arguments->floatingCoupons[i] = Null<Real>();
            }
        }
    }

    inline Rate VanillaSwap::fairRate() const {
        calculate();
        QL_REQUIRE(fairRate_ != Null<Rate>(), "result not available");
        return fairRate_;
    }

    inline Spread VanillaSwap::fairSpread() const {
        calculate();
        QL_REQUIRE(fairSpread_ != Null<Spread>(), "result not available");
        return fairSpread_;
    }

    inline Real VanillaSwap::fixedLegBPS() const {
        calculate();
        QL_REQUIRE(legBPS_[0] != Null<Real>(), "result not available");
        return legBPS_[0];
    }

    inline Real VanillaSwap::floatingLegBPS() const {
        calculate();
        QL_REQUIRE(legBPS_[1] != Null<Real>(), "result not available");
        return legBPS_[1];
    }

    inline Real VanillaSwap::fixedLegNPV() const {
        calculate();
        QL_REQUIRE(legNPV_[0] != Null<Real>(), "result not available");
        return legNPV_[0];
    }

    inline Real VanillaSwap::floatingLegNPV() const {
        calculate();
        QL_REQUIRE(legNPV_[1] != Null<Real>(), "result not available");
        return legNPV_[1];
    }

    inline void VanillaSwap::setupExpired() const {
        Swap::setupExpired();
        legBPS_[0] = legBPS_[1] = 0.0;
        fairRate_ = Null<Rate>();
        fairSpread_ = Null<Spread>();
    }

    inline void VanillaSwap::fetchResults(const PricingEngine::results* r) const {
        static const Spread basisPoint = 1.0e-4;

        Swap::fetchResults(r);

        const VanillaSwap::results* results =
            dynamic_cast<const VanillaSwap::results*>(r);
        if (results) { // might be a swap engine, so no error is thrown
            fairRate_ = results->fairRate;
            fairSpread_ = results->fairSpread;
        } else {
            fairRate_ = Null<Rate>();
            fairSpread_ = Null<Spread>();
        }

        if (fairRate_ == Null<Rate>()) {
            // calculate it from other results
            if (legBPS_[0] != Null<Real>())
                fairRate_ = fixedRate_ - NPV_/(legBPS_[0]/basisPoint);
        }
        if (fairSpread_ == Null<Spread>()) {
            // ditto
            if (legBPS_[1] != Null<Real>())
                fairSpread_ = spread_ - NPV_/(legBPS_[1]/basisPoint);
        }
    }

    inline void VanillaSwap::arguments::validate() const {
        Swap::arguments::validate();
        QL_REQUIRE(nominal != Null<Real>(), "nominal null or not set");
        QL_REQUIRE(fixedResetDates.size() == fixedPayDates.size(),
                   "number of fixed start dates different from "
                   "number of fixed payment dates");
        QL_REQUIRE(fixedPayDates.size() == fixedCoupons.size(),
                   "number of fixed payment dates different from "
                   "number of fixed coupon amounts");
        QL_REQUIRE(floatingResetDates.size() == floatingPayDates.size(),
                   "number of floating start dates different from "
                   "number of floating payment dates");
        QL_REQUIRE(floatingFixingDates.size() == floatingPayDates.size(),
                   "number of floating fixing dates different from "
                   "number of floating payment dates");
        QL_REQUIRE(floatingAccrualTimes.size() == floatingPayDates.size(),
                   "number of floating accrual Times different from "
                   "number of floating payment dates");
        QL_REQUIRE(floatingSpreads.size() == floatingPayDates.size(),
                   "number of floating spreads different from "
                   "number of floating payment dates");
        QL_REQUIRE(floatingPayDates.size() == floatingCoupons.size(),
                   "number of floating payment dates different from "
                   "number of floating coupon amounts");
    }

    inline void VanillaSwap::results::reset() {
        Swap::results::reset();
        fairRate = Null<Rate>();
        fairSpread = Null<Spread>();
    }

    inline std::ostream& operator<<(std::ostream& out,
                             VanillaSwap::Type t) {
        switch (t) {
          case VanillaSwap::Payer:
            return out << "Payer";
          case VanillaSwap::Receiver:
            return out << "Receiver";
          default:
            QL_FAIL("unknown VanillaSwap::Type(" << Integer(t) << ")");
        }
    }

}

#endif