/*
 * Copyright 2020 California  Institute  of Technology (“Caltech”)
 *
 * 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 <x/vio/slam_update.h>
#include <x/vio/tools.h>
#include <x/ekf/state.h>
#include <boost/math/distributions.hpp>

using namespace x;
using namespace Eigen;

SlamUpdate::SlamUpdate(const x::TrackList& trks,
                       const x::AttitudeList& quats,
                       const x::TranslationList& poss,
                       const MatrixXd& feature_states,
                       const std::vector<int>& anchor_idxs,
                       const MatrixXd& cov_s,
                       const int n_poses_max,
                       const double sigma_img)
{
  // Number of features
  const size_t n_trks = trks.size();

  // Initialize Kalman update matrices
  const size_t rows = 2 * n_trks;
  const size_t cols = cov_s.cols();
  jac_ = MatrixXd::Zero(rows, cols);
  cov_m_diag_ = VectorXd::Ones(rows);
  res_ = MatrixXd::Zero(rows, 1);

  // For each track, compute residual, Jacobian and covariance block
  const double var_img = sigma_img * sigma_img;
  for (size_t i = 0, row_h = 0; i < n_trks; ++i) {
    processOneTrack(trks[i],
                    quats,
                    poss,
                    feature_states,
                    anchor_idxs,
                    cov_s,
                    n_poses_max,
                    var_img,
                    i,
                    row_h);
  }
}

void SlamUpdate::processOneTrack(const x::Track& track,
                                 const x::AttitudeList& C_q_G,
                                 const x::TranslationList& G_p_C,
                                 const MatrixXd& feature_states,
                                 const std::vector<int>& anchor_idxs,
                                 const MatrixXd& P,
                                 const int n_poses_max,
                                 const double var_img,
                                 const size_t& j,
                                 size_t& row_h)
{
  const size_t cols = P.cols();
  MatrixXd h_j(MatrixXd::Zero(2, cols));
  MatrixXd Hf_j(MatrixXd::Zero(2, cols));
  MatrixXd res_j(MatrixXd::Zero(2, 1));

  //==========================================================================
  // Feature information
  //==========================================================================
  // A-priori inverse-depth parameters in last observation frame
  double alpha = feature_states(j * 3, 0);
  double beta = feature_states(j * 3 + 1, 0);
  double rho = feature_states(j * 3 + 2, 0);

  // Anchor pose
  unsigned int anchor_idx = anchor_idxs[j];
  x::Quaternion Ca_q_G;
  Ca_q_G.x() = C_q_G[anchor_idx].ax;
  Ca_q_G.y() = C_q_G[anchor_idx].ay;
  Ca_q_G.z() = C_q_G[anchor_idx].az;
  Ca_q_G.w() = C_q_G[anchor_idx].aw;

  Vector3d G_p_Ca(G_p_C[anchor_idx].tx, G_p_C[anchor_idx].ty, G_p_C[anchor_idx].tz);

  // Coordinate of feature in global frame
  Vector3d G_p_fj = 1 / (rho)*Ca_q_G.normalized().toRotationMatrix() * Vector3d(alpha, beta, 1) + G_p_Ca;

  // FOR LAST FEATURE OBSERVATION
  x::Translation G_p_Cn(G_p_C.back());
  x::Attitude Cn_q_G(C_q_G.back());

  x::Quatern attitude_to_quaternion;
  Quaterniond Ci_q_G_ = attitude_to_quaternion(Cn_q_G);
  Vector3d G_p_Ci_(G_p_Cn.tx, G_p_Cn.ty, G_p_Cn.tz);

  // Feature position expressed in camera frame.
  Vector3d Ci_p_fj;
  Ci_p_fj << Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);

  // eq. 20(a)
  Vector2d z;
  const size_t track_size(track.size());
  const unsigned int i = track_size - 1;
  z(0) = track[i].getX();
  z(1) = track[i].getY();

  Vector2d z_hat(z);
  assert(Ci_p_fj(2));
  z_hat(0) = Ci_p_fj(0) / Ci_p_fj(2);
  z_hat(1) = Ci_p_fj(1) / Ci_p_fj(2);

  // eq. 20(b)
  res_j(0, 0) = z(0) - z_hat(0);
  res_j(1, 0) = z(1) - z_hat(1);

  //============================
  // Measurement Jacobian matrix
  //============================

  const unsigned int pos = C_q_G.size() - 1;
  if (anchor_idx == pos)  // Handle special case
  {
    // Inverse-depth feature coordinates jacobian
    MatrixXd mat(MatrixXd::Zero(2, 3));
    mat(0, 0) = 1.0;
    mat(1, 1) = 1.0;

    // Update stacked Jacobian matrices associated to the current feature
    unsigned int row = 0;
    unsigned int col = (n_poses_max * 2 + j) * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = mat;
  }
  else
  {
    // Set Jacobian of pose for i'th measurement of feature j (eq.22, 23)
    VisJacBlock J_i(VisJacBlock::Zero());
    // first row
    J_i(0, 0) = 1.0 / Ci_p_fj(2);
    J_i(0, 1) = 0.0;
    J_i(0, 2) = -Ci_p_fj(0) / std::pow((double)Ci_p_fj(2), 2);
    // second row
    J_i(1, 0) = 0.0;
    J_i(1, 1) = 1.0 / Ci_p_fj(2);
    J_i(1, 2) = -Ci_p_fj(1) / std::pow((double)Ci_p_fj(2), 2);

    // Attitude
    Vector3d skew_vector = Ci_q_G_.normalized().toRotationMatrix().transpose() * (G_p_fj - G_p_Ci_);
    VisJacBlock J_attitude = J_i * x::Skew(skew_vector(0), skew_vector(1), skew_vector(2)).matrix;

    // Position
    VisJacBlock J_position = -J_i * Ci_q_G_.normalized().toRotationMatrix().transpose();

    // Anchor attitude
    VisJacBlock J_anchor_att = -1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() *
                                    Ca_q_G.normalized().toRotationMatrix() * x::Skew(alpha, beta, 1).matrix;

    // Anchor position
    VisJacBlock J_anchor_pos = -J_position;

    // Inverse-depth feature coordinates
    MatrixXd mat(MatrixXd::Identity(3, 3));
    mat(0, 2) = -alpha / rho;
    mat(1, 2) = -beta / rho;
    mat(2, 2) = -1 / rho;
    VisJacBlock Hf_j1 = 1 / rho * J_i * Ci_q_G_.normalized().toRotationMatrix().transpose() *
                             Ca_q_G.normalized().toRotationMatrix() * mat;

    // Update stacked Jacobian matrices associated to the current feature
    unsigned int row = 0;
    const unsigned int pos = C_q_G.size() - 1;
    unsigned int col = pos * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_position;

    col += n_poses_max * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_attitude;

    col = anchor_idx * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_anchor_pos;

    col += n_poses_max * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = J_anchor_att;

    col = (n_poses_max * 2 + j) * kJacCols;
    h_j.block<kVisJacRows, kJacCols>(row, kSizeCoreErr + col) = Hf_j1;
  }

  //==========================================================================
  // Outlier rejection
  //==========================================================================
  const VectorXd r_j_diag = var_img * VectorXd::Ones(2);
  MatrixXd r_j = var_img * MatrixXd::Identity(2, 2);
  MatrixXd S_inv = (h_j * P * h_j.transpose() + r_j).inverse();
  MatrixXd gamma = res_j.transpose() * S_inv * res_j;
  boost::math::chi_squared_distribution<> my_chisqr(2 * track_size);
  double chi = quantile(my_chisqr, 0.9);  // 95-th percentile

  if (gamma(0, 0) < chi)  // Inlier
  {
    jac_.block(row_h,          // startRow
             0,               // startCol
             2,               // numRows
             cols) = h_j;  // numCols

    // Residual vector (one feature)
    res_.block(row_h, 0, 2, 1) = res_j;

    // Measurement covariance matrix
    cov_m_diag_.segment(row_h, 2) = r_j_diag;

    row_h += 2;
  }
}

void SlamUpdate::computeInverseDepthsNew(const x::TrackList& new_trks,
                                         const double rho_0,
                                         MatrixXd& ivds) const
{
  const size_t n_new_slam_std_trks = new_trks.size();
  ivds = MatrixXd::Zero(n_new_slam_std_trks * 3, 1);

  // For each standard SLAM feature to init
  for (size_t j = 0; j < n_new_slam_std_trks; ++j) {
    // Compute inverse-depth coordinates of new feature state
    // (last observation in track)
    computeOneInverseDepthNew(new_trks[j].back(), rho_0, j, ivds);
  }
}

void SlamUpdate::computeOneInverseDepthNew(const x::Feature& feature,
                                           const double rho_0,
                                           const unsigned int& idx,
                                           MatrixXd& ivds) const
{
  // Inverse-depth parameters anchored in last observation frame
  const double alpha = feature.getX();
  const double beta  = feature.getY();
  // const double rho = 1.0 / G_p_Ci.back().tz; // height-based init:

  ivds(3*idx)     = alpha;
  ivds(3*idx + 1) = beta;
  ivds(3*idx + 2) = rho_0;
}


// Boost.Geometry

// Copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_PARAMETERS_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_PARAMETERS_HPP

#include <boost/geometry/formulas/andoyer_inverse.hpp>
#include <boost/geometry/formulas/thomas_direct.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/formulas/vincenty_inverse.hpp>

#include <boost/mpl/assert.hpp>
#include <boost/mpl/integral_c.hpp>

namespace boost { namespace geometry { namespace strategy
{

struct andoyer
{
    template
    <
        typename CT,
        bool EnableCoordinates = true,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct direct
            : formula::thomas_direct
              <
                  CT, false,
                  EnableCoordinates, EnableReverseAzimuth,
                  EnableReducedLength, EnableGeodesicScale
              >
    {};

    template
    <
        typename CT,
        bool EnableDistance,
        bool EnableAzimuth,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct inverse
        : formula::andoyer_inverse
            <
                CT, EnableDistance,
                EnableAzimuth, EnableReverseAzimuth,
                EnableReducedLength, EnableGeodesicScale
            >
    {};
};

struct thomas
{
    template
    <
        typename CT,
        bool EnableCoordinates = true,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct direct
            : formula::thomas_direct
              <
                  CT, true,
                  EnableCoordinates, EnableReverseAzimuth,
                  EnableReducedLength, EnableGeodesicScale
              >
    {};

    template
    <
        typename CT,
        bool EnableDistance,
        bool EnableAzimuth,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct inverse
        : formula::thomas_inverse
            <
                CT, EnableDistance,
                EnableAzimuth, EnableReverseAzimuth,
                EnableReducedLength, EnableGeodesicScale
            >
    {};
};

struct vincenty
{
    template
    <
        typename CT,
        bool EnableCoordinates = true,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct direct
            : formula::vincenty_direct
              <
                  CT, EnableCoordinates, EnableReverseAzimuth,
                  EnableReducedLength, EnableGeodesicScale
              >
    {};

    template
    <
        typename CT,
        bool EnableDistance,
        bool EnableAzimuth,
        bool EnableReverseAzimuth = false,
        bool EnableReducedLength = false,
        bool EnableGeodesicScale = false
    >
    struct inverse
        : formula::vincenty_inverse
            <
                CT, EnableDistance,
                EnableAzimuth, EnableReverseAzimuth,
                EnableReducedLength, EnableGeodesicScale
            >
    {};
};

template <typename FormulaPolicy>
struct default_order
{
    BOOST_MPL_ASSERT_MSG
    (
        false, NOT_IMPLEMENTED_FOR_THIS_TYPE
        , (types<FormulaPolicy>)
    );
};

template<>
struct default_order<andoyer>
    : boost::mpl::integral_c<unsigned int, 1>
{};

template<>
struct default_order<thomas>
    : boost::mpl::integral_c<unsigned int, 2>
{};

template<>
struct default_order<vincenty>
    : boost::mpl::integral_c<unsigned int, 4>
{};

}}} // namespace boost::geometry::strategy

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_PARAMETERS_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_ASHAPE_INCLUDE
#define MTL_ASHAPE_INCLUDE

#include <vector>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/root.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/static_assert.hpp>

// Not elegant but necessary to treat ITL types right
#include <boost/numeric/itl/itl_fwd.hpp>

#ifdef MTL_WITH_INITLIST
# include <initializer_list>
#endif

namespace mtl {

/// Namespace for algebraic shapes; used for sophisticated dispatching between operations
namespace ashape {

// forward declaration
template <typename T> struct ashape_aux;

/// Tag for arbitrary algebraic shape
struct universe {};

// Types (tags)
/// Scalar algebraic shape
struct scal : universe {};

/// Non-scalar algebraic shape
struct nonscal : universe {};
/// Row vector as algebraic shape
template <typename Value> struct rvec : nonscal {};
/// Column vector as algebraic shape
template <typename Value> struct cvec : nonscal {};
/// Matrix as algebraic shape
template <typename Value> struct mat : nonscal {};
/// Undefined shape, e.g., for undefined results of operations
struct ndef {};
/// Future shape, i.e. after appropriate evaluation it will have the shape \p Value
template <typename Value> struct future : nonscal {};

/// Meta-function for algebraic shape of T
/** Unknown types are treated like scalars. ashape of collections are template
    parameterized with ashape of their elements, e.g., ashape< matrix < vector < double > > >::type is
    mat< rvec < scal > > >.
    Implemented with ashape_aux after type is cleaned up with mtl::traits::root.
**/
template <typename T>
struct ashape
  : ashape_aux<typename mtl::traits::root<T>::type> {};

template <typename T>
struct ashape_aux
{
    typedef scal type;
};

/// Vectors must be distinguished between row and column vectors
template <typename Value, typename Parameters>
struct ashape_aux<mtl::vector::dense_vector<Value, Parameters> >
{
    typedef typename boost::mpl::if_<
	boost::is_same<typename Parameters::orientation, row_major>
      , rvec<typename ashape<Value>::type>
      , cvec<typename ashape<Value>::type>
    >::type type;
};

/// Same as dense vector
template <typename Value, typename Parameters>
struct ashape_aux<vector::strided_vector_ref<Value, Parameters> >
  : ashape<mtl::vector::dense_vector<Value, Parameters> > {};

/// Same as dense vector
template <typename Value, typename Parameters>
struct ashape_aux<vector::sparse_vector<Value, Parameters> >
  : ashape<mtl::vector::dense_vector<Value, Parameters> > {};

/// One-dimensional arrays have rvec ashape; 2D arrays are matrices see below
template <typename Value, unsigned Rows>
struct ashape_aux<Value[Rows]>
{
    typedef rvec<typename ashape<Value>::type> type;
};

#ifdef MTL_WITH_INITLIST
/// Non-nested initializer_list have rvec ashape, nested lists are matrices see below
template <typename Value>
struct ashape_aux<std::initializer_list<Value> >
{
    typedef rvec<typename ashape<Value>::type> type;
};
#endif

/// std::vectors have rvec ashape
template <typename Value, typename Allocator>
struct ashape_aux<std::vector<Value, Allocator> >
{
    typedef rvec<typename ashape<Value>::type> type;
};

/// One-dimensional arrays have rvec ashape; 2D arrays are matrices see below
template <typename Value>
struct ashape_aux<Value*>
{
    typedef rvec<typename ashape<Value>::type> type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< vector::vec_vec_pmop_expr<E1, E2, SFunctor> >
{
    MTL_STATIC_ASSERT((boost::is_same<typename ashape<E1>::type,
			                typename ashape<E2>::type>::value), "Operands must have same algebraic shape.");
    typedef typename ashape<E1>::type type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< vector::vec_vec_op_expr<E1, E2, SFunctor> >
{
#if 0 // not sure if this is true in all operations
    MTL_STATIC_ASSERT((boost::is_same<typename ashape<E1>::type,
				      typename ashape<E2>::type>::value), "Operands must have same algebraic shape.");
#endif
    typedef typename ashape<E1>::type type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< vector::vec_vec_aop_expr<E1, E2, SFunctor> >
{
    typedef typename ashape<E1>::type type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< vector::vec_scal_aop_expr<E1, E2, SFunctor> >
{
    typedef typename ashape<E1>::type type;
};

template <typename Vector>
struct ashape_aux< vector::vec_const_ref_expr<Vector> >
{
    typedef typename ashape<Vector>::type type;
};

// ========
// Matrices
// ========

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::compressed2D<Value, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::coordinate2D<Value, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::sparse_banded<Value, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::dense2D<Value, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, std::size_t Mask, typename Parameters>
struct ashape_aux<mtl::matrix::morton_dense<Value, Mask, Parameters> >
{
    typedef mat<typename ashape<Value>::type> type;
};

template <typename Functor>
struct ashape_aux<mtl::matrix::implicit_dense<Functor> >
{
    typedef mat<typename ashape<typename Functor::result_type>::type> type;
};

/// Two-dimensional arrays have mat ashape; 1D arrays are vectors see above
template <typename Value, unsigned Rows, unsigned Cols>
struct ashape_aux<Value[Rows][Cols]>
{
    typedef mat<typename ashape<Value>::type> type;
};

/// Two-dimensional arrays have mat ashape; 1D arrays are vectors see above
template <typename Value, unsigned Cols>
struct ashape_aux<Value (*)[Cols]>
{
    typedef mat<typename ashape<Value>::type> type;
};

#ifdef MTL_WITH_INITLIST
/// Nested initializer_list are matrices, non-nested are vectors see above
template <typename Value>
struct ashape_aux<std::initializer_list<std::initializer_list<Value> > >
{
    typedef mat<typename ashape<Value>::type> type;
};
#endif

template <typename Vector>
struct ashape_aux<mtl::matrix::multi_vector<Vector> >
{
    typedef mat<typename ashape<typename mtl::Collection<mtl::matrix::multi_vector<Vector> >::value_type>::type> type;
};

template <typename Value>
struct ashape_aux<matrix::element_structure<Value> >
{
   typedef mat<typename ashape<Value>::type> type;
};

template <typename Value, typename Parameters>
struct ashape_aux<mtl::matrix::ell_matrix<Value, Parameters> >
{
   typedef mat<typename ashape<Value>::type> type;
};

template <typename Vector>
struct ashape_aux<matrix::multi_vector_range<Vector> >
{
    typedef mat<typename ashape<typename mtl::Collection<matrix::multi_vector_range<Vector> >::value_type>::type> type;
};

template <typename E1, typename E2, typename SFunctor>
struct ashape_aux< matrix::mat_mat_op_expr<E1, E2, SFunctor> >
{
    MTL_STATIC_ASSERT((boost::is_same<typename ashape<E1>::type,
				      typename ashape<E2>::type>::value), "Operands must have same algebraic shape.");
    typedef typename ashape<E1>::type type;
};

template <typename Vector1, typename Vector2>
struct ashape< matrix::outer_product_matrix<Vector1, Vector2> >
{
    // BOOST_STATIC_ASSERT((boost::is_same<typename ashape<E1>::type,
    // 			                typename transposed_shape<typename ashape<E2>::type>::type>::value));
    typedef mat<typename ashape<typename mtl::Collection<Vector1>::value_type>::type> type;
};

template <typename Matrix, typename VectorIn>
struct ashape< vector::mat_cvec_multiplier<Matrix, VectorIn> >
{
    typedef cvec<scal> type;
};

// =====
// Views
// =====

template <typename Functor, typename Coll>
struct ashape_aux<matrix::map_view<Functor, Coll> >
{
    typedef typename ashape<Coll>::type type;
};

template <typename Functor, typename Coll>
struct ashape_aux<vector::map_view<Functor, Coll> >
{
    typedef typename ashape<Coll>::type type;
};

template <typename Coll>
struct ashape_aux<vector::conj_view<Coll> >
{
    typedef typename ashape<Coll>::type type;
};

template <typename Coll>
struct ashape_aux<vector::real_view<Coll> >
{
    typedef typename ashape<Coll>::type type;
};

template <typename Coll>
struct ashape_aux<vector::imag_view<Coll> >
{
    typedef typename ashape<Coll>::type type;
};

#if 1
// shouldn't be needed
template <typename Coll>
struct ashape_aux<mtl::matrix::transposed_view<const matrix::conj_view<Coll> > >
{
    typedef typename ashape<Coll>::type type;
};
#endif

template <typename Matrix>
struct ashape_aux<matrix::transposed_view<Matrix> >
{
    typedef typename ashape<Matrix>::type type;
};

template <typename Matrix>
struct ashape_aux<matrix::banded_view<Matrix> >
{
    typedef typename ashape<Matrix>::type type;
};

template <typename Matrix>
struct ashape_aux<matrix::indirect<Matrix> >
{
    typedef typename ashape<Matrix>::type type;
};

// Rule out other types as algebraic shape
template <typename IFStream, typename OFStream>
struct ashape_aux<io::matrix_file<IFStream, OFStream> >
{
    typedef ndef type;
};

// =====================
// Shapes of products:
// =====================

// a) The result's shape
// b) Classify operation in terms of shape

// Operation types:

struct scal_scal_mult {};
struct cvec_rvec_mult {}; // outer product
struct rvec_cvec_mult {}; // inner product (without conj)
struct rvec_mat_mult {};
struct mat_cvec_mult {};
struct mat_mat_mult {};
struct scal_rvec_mult {};
struct scal_cvec_mult {};
struct scal_mat_mult {};
struct rvec_scal_mult {};
struct cvec_scal_mult {};
struct mat_scal_mult {};

// =====================
// Results of operations
// =====================

/*
      s  cv  rv   m
-------------------
 s |  s  cv* rv*  m*
cv | cv*  x   m   x
rv | rv*  s   x  rv
 m |  m* cv   x   m

 * only on outer level, forbidden for elements of collections

*/

// Results for elements of collections, i.e. scalar * matrix (vector) are excluded

/// Algebraic shape of multiplication's result when elements of collections are multiplied.
/** The types are the same as for multiplications of entire collections except that scalar *
    matrix (or vector) is excluded to avoid ambiguities.
    emult_shape <Shape1, Shape2> is only properly defined if emult_op <Shape1, Shape2>::type is not ndef!
**/
template <typename Shape1, typename Shape2>
struct emult_shape
{
    typedef ndef type;
};

/// Type of operation when values of Shape1 and Shape2 are multiplied (so far only for elements of collections)
/** The types are the same as for multiplications of entire collections except that scalar *
    matrix (or vector) is excluded to avoid ambiguities. **/
template <typename Shape1, typename Shape2>
struct emult_op
{
    typedef ndef type;
};

// Scalar * scalar -> scalar
template <>
struct emult_shape<scal, scal>
{
    typedef scal type;
};

template <>
struct emult_op<scal, scal>
{
    typedef scal_scal_mult type;
};

// Column times row vector, i.e. outer product
template <typename Value1, typename Value2>
struct emult_shape<cvec<Value1>, rvec<Value2> >
{
    typedef mat<typename emult_shape<Value1, Value2>::type> type;
};

template <typename Value1, typename Value2>
struct emult_op<cvec<Value1>, rvec<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , cvec_rvec_mult
    >::type type;
};

// Row times column vector, i.e. inner product (without conj)
template <typename Value1, typename Value2>
struct emult_shape<rvec<Value1>, cvec<Value2> >
{
    typedef typename emult_shape<Value1, Value2>::type type;
};

template <typename Value1, typename Value2>
struct emult_op<rvec<Value1>, cvec<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , rvec_cvec_mult
    >::type type;
};

// Row vector times matrix
template <typename Value1, typename Value2>
struct emult_shape<rvec<Value1>, mat<Value2> >
{
    typedef rvec<typename emult_shape<Value1, Value2>::type> type;
};

template <typename Value1, typename Value2>
struct emult_op<rvec<Value1>, mat<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , rvec_mat_mult
    >::type type;
};

// Matrix times column vector
template <typename Value1, typename Value2>
struct emult_shape<mat<Value1>, cvec<Value2> >
{
    typedef cvec<typename emult_shape<Value1, Value2>::type> type;
};

template <typename Value1, typename Value2>
struct emult_op<mat<Value1>, cvec<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , mat_cvec_mult
    >::type type;
};

// Matrix product
template <typename Value1, typename Value2>
struct emult_shape<mat<Value1>, mat<Value2> >
{
    typedef mat<typename emult_shape<Value1, Value2>::type> type;
};

template <typename Value1, typename Value2>
struct emult_op<mat<Value1>, mat<Value2> >
{
    // if product of elements is undefined then product is undefined too
    typedef typename boost::mpl::if_<
	boost::is_same<typename emult_op<Value1, Value2>::type, ndef>
      , ndef
      , mat_mat_mult
    >::type type;
};

// Results for entire collections, i.e. scalar * matrix (vector) are allowed

// Multiplying collections as emult

template  <typename Shape1, typename Shape2>
struct mult_shape
    : public emult_shape<Shape1, Shape2>
{};

template  <typename Shape1, typename Shape2>
struct mult_op
    : public emult_op<Shape1, Shape2>
{};

// Scale collection from left

template <typename Shape2>
struct mult_shape<scal, Shape2>
{
    typedef Shape2 type;
};

template <typename Value2>
struct mult_op<scal, rvec<Value2> >
{
    typedef scal_rvec_mult type;
};

template <typename Value2>
struct mult_op<scal, cvec<Value2> >
{
    typedef scal_cvec_mult type;
};

template <typename Value2>
struct mult_op<scal, mat<Value2> >
{
    typedef scal_mat_mult type;
};

// Scale collection from right

template <typename Shape1>
struct mult_shape<Shape1, scal>
{
    typedef Shape1 type;
};

template <typename Value1>
struct mult_op<rvec<Value1>, scal>
{
    typedef rvec_scal_mult type;
};

template <typename Value1>
struct mult_op<cvec<Value1>, scal>
{
    typedef cvec_scal_mult type;
};

template <typename Value1>
struct mult_op<mat<Value1>, scal>
{
    typedef mat_scal_mult type;
};

// Arbitration
template <>
struct mult_shape<scal, scal>
{
    typedef scal type;
};

// Needs to be verified for nested matrix types, cf. #140
template <typename E1, typename E2>
struct ashape< matrix::mat_mat_times_expr<E1, E2> >
{
    // typedef typename ashape<E1>::type type;
    typedef typename mult_shape<typename ashape<E1>::type,
				typename ashape<E2>::type>::type type;
};

template <typename E1, typename E2>
struct ashape< mat_cvec_times_expr<E1, E2> >
{
    // Resulting vector has the same shape as the multiplied
    typedef typename ashape<E2>::type type;
};

template <typename E1, typename E2>
struct ashape< vector::rvec_mat_times_expr<E1, E2> >
{
    // Resulting vector has the same shape as the multiplied
    typedef typename ashape<E1>::type type;
};

// added by Hui Li (below) -----------------------------------------

// =====================
// Shapes of divisions:
// =====================

// Operation types:

struct scal_scal_div {};
struct cvec_scal_div {};
struct rvec_scal_div {};
struct mat_scal_div {};

template < typename Shape1, typename Shape2 >
struct div_shape
{
	typedef ndef type;
};

template < typename Shape1, typename Shape2 >
struct div_op
{
	typedef ndef type;
};

template <>
struct div_shape<scal,scal>
{
	typedef scal type;
};

template<>
struct div_op<scal,scal>
{
	typedef scal type;
};

template < typename Value1 >
struct div_shape < rvec<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<Value1,scal>::type,ndef>::type,
		ndef,
		rvec<typename div_shape<Value1,scal>::type>
	>::type type;
};

template < typename Value1 >
struct div_op< rvec<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<rvec<Value1>,scal>::type,ndef>::type,
		ndef,
		rvec_scal_div
	>::type type;
};

template < typename Value1 >
struct div_shape < cvec<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<Value1,scal>::type,ndef>::type,
		ndef,
		cvec<typename div_shape<Value1,scal>::type>
	>::type type;
};

template < typename Value1 >
struct div_op< cvec<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<cvec<Value1>,scal>::type,ndef>::type,
		ndef,
		cvec_scal_div
	>::type type;
};

template < typename Value1 >
struct div_shape < mat<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<Value1,scal>::type,ndef>::type,
		ndef,
		mat<typename div_shape<Value1,scal>::type>
	>::type type;
};

template < typename Value1 >
struct div_op < mat<Value1>, scal >
{
	typedef typename boost::mpl::if_<
		typename boost::is_same<typename div_shape<mat<Value1>,scal>::type,ndef>::type,
		ndef,
		mat_scal_div
	>::type type;
};

// added by Hui Li (above) -----------------------------------------

// ==================== ITL types ==================================

template <typename PC, typename Vector, bool Adjoint>
struct ashape<itl::pc::solver<PC, Vector, Adjoint> >
{
    typedef future<cvec<scal> > type; // might be a problem with nested matrices and vectors
};

}} // namespace mtl::ashape

#endif // MTL_ASHAPE_INCLUDE


#include <iostream>
#include <stdexcept>

#include <boost/lexical_cast.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/timeConversions.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/External/SpiceInterface/spiceRotationalEphemeris.h"

namespace tudat
{

namespace ephemerides
{

//! Function to calculate the rotation quaternion from target frame to original frame.
Eigen::Quaterniond SpiceRotationalEphemeris::getRotationToBaseFrame(
        const double secondsSinceEpoch, const double julianDayAtEpoch )
{
    // Set number of seconds since J2000.
    double ephemerisTime = secondsSinceEpoch;
    if ( julianDayAtEpoch != basic_astrodynamics::JULIAN_DAY_ON_J2000 )
    {
        ephemerisTime -= ( basic_astrodynamics::JULIAN_DAY_ON_J2000 - julianDayAtEpoch )
                * physical_constants::JULIAN_DAY;
    }

    // Get rotational quaternion from spice wrapper function
    return spice_interface::computeRotationQuaternionBetweenFrames(
                targetFrameOrientation_, baseFrameOrientation_, ephemerisTime );
}

//! Function to calculate the derivative of the rotation matrix from target frame to original
//! frame.
Eigen::Matrix3d SpiceRotationalEphemeris::getDerivativeOfRotationToBaseFrame(
        const double secondsSinceEpoch, const double julianDayAtEpoch )
{
    // Set number of seconds since J2000.
    double ephemerisTime = secondsSinceEpoch;
    if ( julianDayAtEpoch != basic_astrodynamics::JULIAN_DAY_ON_J2000 )
    {
        ephemerisTime -= ( basic_astrodynamics::JULIAN_DAY_ON_J2000 - julianDayAtEpoch )
                * physical_constants::JULIAN_DAY;
    }

    // Get rotation matrix derivative from spice wrapper function
    return spice_interface::computeRotationMatrixDerivativeBetweenFrames(
                targetFrameOrientation_, baseFrameOrientation_, ephemerisTime );
}

//! Function to calculate the full rotational state at given time
void SpiceRotationalEphemeris::getFullRotationalQuantitiesToTargetFrame(
        Eigen::Quaterniond& currentRotationToLocalFrame,
        Eigen::Matrix3d& currentRotationToLocalFrameDerivative,
        Eigen::Vector3d& currentAngularVelocityVectorInGlobalFrame,
        const double secondsSinceEpoch, const double julianDayAtEpoch)
{
    // Set number of seconds since J2000.
    double ephemerisTime = secondsSinceEpoch;
    if ( julianDayAtEpoch != basic_astrodynamics::JULIAN_DAY_ON_J2000 )
    {
        ephemerisTime -= ( basic_astrodynamics::JULIAN_DAY_ON_J2000 - julianDayAtEpoch )
                * physical_constants::JULIAN_DAY;
    }

    // Calculate rotation (and its time derivative) directly from spice.
    std::pair< Eigen::Quaterniond, Eigen::Matrix3d > fullRotation =
            spice_interface::computeRotationQuaternionAndRotationMatrixDerivativeBetweenFrames(
                baseFrameOrientation_, targetFrameOrientation_, ephemerisTime );
    currentRotationToLocalFrame = fullRotation.first;
    currentRotationToLocalFrameDerivative = fullRotation.second;

    // Calculate angular velocity vector.
    currentAngularVelocityVectorInGlobalFrame = getRotationalVelocityVectorInBaseFrameFromMatrices(
                Eigen::Matrix3d( currentRotationToLocalFrame ), currentRotationToLocalFrameDerivative.transpose( ) );
}

} // namespace ephemerides

} // namespace tudat


/* Copyright (C) 2012,2013 IBM Corp.
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * 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 GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include <algorithm>   // defines count(...), min(...)

#include "PAlgebra.h"
#include "hypercube.h"
#include "timing.h"

#include <NTL/ZZXFactoring.h>
#include <NTL/GF2EXFactoring.h>
#include <NTL/lzz_pEXFactoring.h>

// polynomials are sorted lexicographically, with the
// constant term being the "most significant"

template<class RX> bool poly_comp(const RX& a, const RX& b)
{
  long na = deg(a) + 1;
  long nb = deg(b) + 1;

  long i = 0;
  while (i < na && i < nb && coeff(a, i) == coeff(b, i)) i++;

  if (i < na && i < nb)
    return coeff(a, i) < coeff(b, i);
  else
    return na < nb;
}

namespace NTL {

// for some weird reason, these need to be in either the std or NTL
// namespace; otherwise, the compiler won't find them...

bool operator<(GF2 a, GF2 b) { return rep(a) < rep(b); }
bool operator<(zz_p a, zz_p b) { return rep(a) < rep(b); }

bool operator<(const GF2X& a, const GF2X& b) { return poly_comp(a, b); }
bool operator<(const zz_pX& a, const zz_pX& b) { return poly_comp(a, b); }

bool operator<(const GF2E& a, const GF2E& b) { return rep(a) < rep(b); }
bool operator<(const zz_pE& a, const zz_pE& b) { return rep(a) < rep(b); }

bool operator<(const GF2EX& a, const GF2EX& b) { return poly_comp(a, b); }
bool operator<(const zz_pEX& a, const zz_pEX& b) { return poly_comp(a, b); }

}

bool PAlgebra::operator==(const PAlgebra& other) const
{
  if (m != other.m) return false;
  if (p != other.p) return false;

  return true;
}

bool PAlgebra::nextExpVector(vector<unsigned long>& buffer) const
{
  // increment the vector in lexicographic order
  if (!isDryRun()) for (long i=gens.size()-1; i>=0; i--) {
    if (i>=(long)buffer.size()) continue; // sanity check
    // increment current index, set all the ones after it to zero
    if (buffer[i] < OrderOf(i)-1) {
      buffer[i]++;
      for (unsigned long j=i+1; j<buffer.size(); j++) buffer[j] = 0;
      return true;  // succeeded in incrementing the vector
    }
    // if buffer[i] >= OrderOf(i)-1, mover to previous index i
  }
  return false;     // cannot increment the vector anymore
}

long PAlgebra::coordinate(long i, long k) const
{
  if (isDryRun()) return 0;
  long t = ith_rep(k); // element of Zm^* representing the k'th slot

  // dLog returns the representation of t along the generators, so the
  // i'th entry there is the coordinate relative to i'th geneator
  return dLog(t)[i];
}

long PAlgebra::addCoord(long i, long k, long offset) const
{
  if (isDryRun()) return 0;
  assert(k >= 0 && k < (long) nSlots);
  assert(i >= 0 && i < (long) gens.size());

  offset = offset % ((long) OrderOf(i));
  if (offset < 0) offset += OrderOf(i);

  long k_i = coordinate(i, k);
  long k_i1 = (k_i + offset) % OrderOf(i);

  long k1 = k + (k_i1 - k_i) * prods[i+1];

  return k1;
}

unsigned long PAlgebra::exponentiate(const vector<unsigned long>& exps,
				bool onlySameOrd) const
{
  if (isDryRun()) return 1;
  unsigned long t = 1;
  unsigned long n = min(exps.size(),gens.size());
  for (unsigned long i=0; i<n; i++) {
    if (onlySameOrd && !SameOrd(i)) continue;
    unsigned long g = PowerMod(gens[i] ,exps[i], m);
    t = MulMod(t, g, m);
  }
  return t;
}

void PAlgebra::printout() const
{
  cout << "m = " << m << ", p = " << p;
  if (isDryRun()) { cout << " (dry run)\n"; return; }
  cout << ", phi(m) = " << phiM << endl;
  cout << "  ord(p)=" << ordP << endl;

  unsigned long i;
  for (i=0; i<gens.size(); i++) if (gens[i]) {
      cout << "  generator " << gens[i] << " has order ("
           << (SameOrd(i)? "=":"!") << "= Z_m^*) of "
	   << OrderOf(i) << endl;
  }
  if (qGrpOrd()<100) {
    cout << "  T = [";
    for (i=0; i<T.size(); i++) cout << T[i] << " ";
    cout << "]\n";
  }
}

PAlgebra::PAlgebra(unsigned long mm, unsigned long pp,
                   const vector<long>& _gens, const vector<long>& _ords )
{
  assert( ProbPrime(pp) );
  assert( (mm % pp) != 0 );
  assert( mm < NTL_SP_BOUND );
  assert( mm > 1 );

  cM  = 1.0; // default value for the ring constant
  m = mm;
  p = pp;

  long k = NextPowerOfTwo(m);
  if (mm == (1UL << k))
    pow2 = k;
  else
    pow2 = 0;

  // For dry-run, use a tiny m value for the PAlgebra tables
  if (isDryRun()) mm = (p==3)? 4 : 3;

  // Compute the generators for (Z/mZ)^* (defined in NumbTh.cpp)

  if (_gens.size() == 0 || isDryRun())
      ordP = findGenerators(this->gens, this->ords, mm, pp);
  else {
    assert(_gens.size() == _ords.size());
    gens = _gens;
    ords = _ords;
    ordP = multOrd(pp, mm);
  }
  nSlots = qGrpOrd();
  phiM = ordP * nSlots;

  // Allocate space for the various arrays
  T.resize(nSlots);
  dLogT.resize(nSlots*gens.size());
  Tidx.assign(mm,-1);    // allocate m slots, initialize them to -1
  zmsIdx.assign(mm,-1);  // allocate m slots, initialize them to -1
  long i, idx;
  for (i=idx=0; i<(long)mm; i++) if (GCD(i,mm)==1) zmsIdx[i] = idx++;

  // Now fill the Tidx and dLogT translation tables. We identify an element
  // t\in T with its representation t = \prod_{i=0}^n gi^{ei} mod m (where
  // the gi's are the generators in gens[]) , represent t by the vector of
  // exponents *in reverse order* (en,...,e1,e0), and order these vectors
  // in lexicographic order.

  // FIXME: is the comment above about reverse order true? It doesn't
  // seem like it to me.  VJS.

  // buffer is initialized to all-zero, which represents 1=\prod_i gi^0
  vector<unsigned long> buffer(gens.size()); // temporaty holds exponents
  i = idx = 0;
  long ctr = 0;
  do {
    ctr++;
    unsigned long t = exponentiate(buffer);
    for (unsigned long j=0; j<buffer.size(); j++) dLogT[idx++] = buffer[j];

    assert(GCD(t,mm) == 1); // sanity check for user-supplied gens
    assert(Tidx[t] == -1);

    T[i] = t;       // The i'th element in T it t
    Tidx[t] = i++;  // the index of t in T is i

    // increment buffer by one (in lexigoraphic order)
  } while (nextExpVector(buffer)); // until we cover all the group

  assert(ctr == long(nSlots)); // sanity check for user-supplied gens

  PhimX = Cyclotomic(mm); // compute and store Phi_m(X)

  // initialize prods array
  long ndims = gens.size();
  prods.resize(ndims+1);
  prods[ndims] = 1;
  for (long j = ndims-1; j >= 0; j--) {
    prods[j] = OrderOf(j) * prods[j+1];
  }
  //  pp_factorize(mFactors,mm); // prime-power factorization from NumbTh.cpp
}

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

  PAlgebraMod stuff....

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

PAlgebraModBase *buildPAlgebraMod(const PAlgebra& zMStar, long r)
{
  unsigned long p = zMStar.getP();
  assert(r > 0);

  if (p == 2 && r == 1)
    return new PAlgebraModDerived<PA_GF2>(zMStar, r);
  else
    return new  PAlgebraModDerived<PA_zz_p>(zMStar, r);
}

template<class T>
void PAlgebraLift(const ZZX& phimx, const T& lfactors, T& factors, T& crtc, long r);

// Missing NTL functionality

void EDF(vec_zz_pX& v, const zz_pX& f, long d)
{
   EDF(v, f, PowerXMod(zz_p::modulus(), f), d);
}

zz_pEX FrobeniusMap(const zz_pEXModulus& F)
{
  return PowerXMod(zz_pE::cardinality(), F);
}

template<class type>
PAlgebraModDerived<type>::PAlgebraModDerived(const PAlgebra& _zMStar, long _r)
  : zMStar(_zMStar), r(_r)

{
  long p = zMStar.getP();
  long m = zMStar.getM();

  // For dry-run, use a tiny m value for the PAlgebra tables
  if (isDryRun()) m = (p==3)? 4 : 3;

  assert(r > 0);

  ZZ BigPPowR = power_ZZ(p, r);
  assert(BigPPowR.SinglePrecision());
  pPowR = to_long(BigPPowR);

  long nSlots = zMStar.getNSlots();

  RBak bak; bak.save();
  SetModulus(p);

  // Compute the factors Ft of Phi_m(X) mod p, for all t \in T

  RX phimxmod;

  conv(phimxmod, zMStar.getPhimX()); // Phi_m(X) mod p

  vec_RX localFactors;

  EDF(localFactors, phimxmod, zMStar.getOrdP()); // equal-degree factorization

  RX* first = &localFactors[0];
  RX* last = first + localFactors.length();
  RX* smallest = min_element(first, last);
  swap(*first, *smallest);

  // We make the lexicographically smallest factor have index 0.
  // The remaining factors are ordered according to their representives.

  RXModulus F1(localFactors[0]);
  for (long i=1; i<nSlots; i++) {
    unsigned long t =zMStar.ith_rep(i); // Ft is minimal polynomial of x^{1/t} mod F1
    unsigned long tInv = InvMod(t, m);  // tInv = t^{-1} mod m
    RX X2tInv = PowerXMod(tInv,F1);     // X2tInv = X^{1/t} mod F1
    IrredPolyMod(localFactors[i], X2tInv, F1);
  }
  /* Debugging sanity-check #1: we should have Ft= GCD(F1(X^t),Phi_m(X))
  for (i=1; i<nSlots; i++) {
    unsigned long t = T[i];
    RX X2t = PowerXMod(t,phimxmod);  // X2t = X^t mod Phi_m(X)
    RX Ft = GCD(CompMod(F1,X2t,phimxmod),phimxmod);
    if (Ft != localFactors[i]) {
      cout << "Ft != F1(X^t) mod Phi_m(X), t=" << t << endl;
      exit(0);
    }
  }*******************************************************************/

  if (r == 1) {
    build(PhimXMod, phimxmod);
    factors = localFactors;
    pPowRContext.save();

    // Compute the CRT coefficients for the Ft's
    crtCoeffs.SetLength(nSlots);
    for (long i=0; i<nSlots; i++) {
      RX te = phimxmod / factors[i]; // \prod_{j\ne i} Fj
      te %= factors[i];              // \prod_{j\ne i} Fj mod Fi
      InvMod(crtCoeffs[i], te, factors[i]); // \prod_{j\ne i} Fj^{-1} mod Fi
    }
  }
  else {
    PAlgebraLift(zMStar.getPhimX(), localFactors, factors, crtCoeffs, r);
    RX phimxmod1;
    conv(phimxmod1, zMStar.getPhimX());
    build(PhimXMod, phimxmod1);
    pPowRContext.save();
  }

  // set factorsOverZZ
  factorsOverZZ.resize(nSlots);
  for (long i = 0; i < nSlots; i++)
    conv(factorsOverZZ[i], factors[i]);

  genCrtTable();
  genMaskTable();
}

// Assumes current zz_p modulus is p^r
// computes S = F^{-1} mod G via Hensel lifting
void InvModpr(zz_pX& S, const zz_pX& F, const zz_pX& G, long p, long r)
{
  ZZX ff, gg, ss, tt;

  ff = to_ZZX(F);
  gg = to_ZZX(G);

  zz_pBak bak;
  bak.save();
  zz_p::init(p);

  zz_pX f, g, s, t;
  f = to_zz_pX(ff);
  g = to_zz_pX(gg);
  s = InvMod(f, g);
  t = (1-s*f)/g;
  assert(s*f + t*g == 1);
  ss = to_ZZX(s);
  tt = to_ZZX(t);

  ZZ pk = to_ZZ(1);

  for (long k = 1; k < r; k++) {
    // lift from p^k to p^{k+1}
    pk = pk * p;

    assert(divide(ss*ff + tt*gg - 1, pk));

    zz_pX d = to_zz_pX( (1 - (ss*ff + tt*gg))/pk );
    zz_pX s1, t1;
    s1 = (s * d) % g;
    t1 = (d-s1*f)/g;
    ss = ss + pk*to_ZZX(s1);
    tt = tt + pk*to_ZZX(t1);
  }

  bak.restore();

  S = to_zz_pX(ss);

  assert((S*F) % G == 1);
}

template<class T>
void PAlgebraLift(const ZZX& phimx, const T& lfactors, T& factors, T& crtc, long r)
{
   Error("uninstatiated version of PAlgebraLift");
}

// This specialized version of PAlgebraLift does the hensel
// lifting needed to finish off the initialization.
// It assumes the zz_p modulus is initialized to p
// when called, and leaves it set to p^r

template<>
void PAlgebraLift(const ZZX& phimx, const vec_zz_pX& lfactors, vec_zz_pX& factors, vec_zz_pX& crtc, long r)
{
  long p = zz_p::modulus();
  long nSlots = lfactors.length();

  vec_ZZX vzz;             // need to go via ZZX

  // lift the factors of Phi_m(X) from mod-2 to mod-2^r
  if (lfactors.length() > 1)
    MultiLift(vzz, lfactors, phimx, r); // defined in NTL::ZZXFactoring
  else {
    vzz.SetLength(1);
    vzz[0] = phimx;
  }

  // Compute the zz_pContext object for mod p^r arithmetic
  zz_p::init(power_long(p, r));

  zz_pX phimxmod = to_zz_pX(phimx);
  factors.SetLength(nSlots);
  for (long i=0; i<nSlots; i++)             // Convert from ZZX to zz_pX
    conv(factors[i], vzz[i]);

  // Finally compute the CRT coefficients for the factors
  crtc.SetLength(nSlots);
  for (long i=0; i<nSlots; i++) {
    zz_pX& fct = factors[i];
    zz_pX te = phimxmod / fct; // \prod_{j\ne i} Fj
    te %= fct;                // \prod_{j\ne i} Fj mod Fi
    InvModpr(crtc[i], te, fct, p, r);// \prod_{j\ne i} Fj^{-1} mod Fi
  }

}

// Returns a vector crt[] such that crt[i] = p mod Ft (with t = T[i])
template<class type>
void PAlgebraModDerived<type>::CRT_decompose(vector<RX>& crt, const RX& H) const
{
  unsigned long nSlots = zMStar.getNSlots();

  if (isDryRun()) {
    crt.clear();
    return;
  }
  crt.resize(nSlots);
  for (unsigned long i=0; i<nSlots; i++)
    rem(crt[i], H, factors[i]); // crt[i] = H % factors[i]
}

template<class type>
void PAlgebraModDerived<type>::embedInAllSlots(RX& H, const RX& alpha,
                                            const MappingData<type>& mappingData) const
{
  if (isDryRun()) {
    H = RX::zero();
    return;
  }
  FHE_TIMER_START;
  long nSlots = zMStar.getNSlots();

  vector<RX> crt(nSlots); // alloate space for CRT components

  // The i'th CRT component is (H mod F_t) = alpha(maps[i]) mod F_t,
  // where with t=T[i].

  if (IsX(mappingData.G) || deg(alpha) <= 0) {
    // special case...no need for CompMod, which is
    // is not optimized for this case

    for (long i=0; i<nSlots; i++)   // crt[i] = alpha(maps[i]) mod Ft
      crt[i] = ConstTerm(alpha);
  }
  else {
    // general case...

    for (long i=0; i<nSlots; i++)   // crt[i] = alpha(maps[i]) mod Ft
      CompMod(crt[i], alpha, mappingData.maps[i], factors[i]);
  }

  CRT_reconstruct(H,crt); // interpolate to get H
  FHE_TIMER_STOP;
}

template<class type>
void PAlgebraModDerived<type>::embedInSlots(RX& H, const vector<RX>& alphas,
                                         const MappingData<type>& mappingData) const
{
  if (isDryRun()) {
    H = RX::zero();
    return;
  }
  FHE_TIMER_START;

  long nSlots = zMStar.getNSlots();
  assert(lsize(alphas) == nSlots);

  for (long i = 0; i < nSlots; i++) assert(deg(alphas[i]) < mappingData.degG);

  vector<RX> crt(nSlots); // alloate space for CRT components

  // The i'th CRT component is (H mod F_t) = alphas[i](maps[i]) mod F_t,
  // where with t=T[i].

  if (IsX(mappingData.G)) {
    // special case...no need for CompMod, which is
    // is not optimized for this case

    for (long i=0; i<nSlots; i++)   // crt[i] = alpha(maps[i]) mod Ft
      crt[i] = ConstTerm(alphas[i]);
  }
  else {
    // general case...still try to avoid CompMod when possible,
    // which is the common case for encoding masks

    for (long i=0; i<nSlots; i++) {   // crt[i] = alpha(maps[i]) mod Ft
      if (deg(alphas[i]) <= 0)
        crt[i] = alphas[i];
      else
        CompMod(crt[i], alphas[i], mappingData.maps[i], factors[i]);
    }
  }

  CRT_reconstruct(H,crt); // interpolate to get p

  FHE_TIMER_STOP;
}

template<class type>
void PAlgebraModDerived<type>::CRT_reconstruct(RX& H, vector<RX>& crt) const
{
  if (isDryRun()) {
    H = RX::zero();
    return;
  }
  FHE_TIMER_START;
  long nslots = zMStar.getNSlots();

  const vector<RX>& ctab = crtTable;

  clear(H);
  RX tmp1, tmp2;

  bool easy = true;
  for (long i = 0; i < nslots; i++)
    if (!IsZero(crt[i]) && !IsOne(crt[i])) {
      easy = false;
      break;
    }

  if (easy) {
    for (long i=0; i<nslots; i++)
      if (!IsZero(crt[i]))
        H += ctab[i];
  }
  else {
    vector<RX> crt1;
    crt1.resize(nslots);
    for (long i = 0; i < nslots; i++)
       MulMod(crt1[i], crt[i], crtCoeffs[i], factors[i]);

    evalTree(H, crtTree, crt1, 0, nslots);
  }
  FHE_TIMER_STOP;
}

template<class type>
void PAlgebraModDerived<type>::mapToFt(RX& w,
			     const RX& G,unsigned long t,const RX* rF1) const
{
  if (isDryRun()) {
    w = RX::zero();
    return;
  }
  long i = zMStar.indexOfRep(t);
  if (i < 0) { clear(w); return; }

  if (rF1==NULL) {               // Compute the representation "from scratch"
    // special case
    if (G == factors[i]) {
      SetX(w);
      return;
    }

    //special case
    if (deg(G) == 1) {
      w = -ConstTerm(G);
      return;
    }

    // the general case: currently only works when r == 1
    assert(r == 1);

    REBak bak; bak.save();
    RE::init(factors[i]);        // work with the extension field GF_p[X]/Ft(X)
    REX Ga;
    conv(Ga, G);                 // G as a polynomial over the extension field

    vec_RE roots;
    FindRoots(roots, Ga);        // Find roots of G in this field
    RE* first = &roots[0];
    RE* last = first + roots.length();
    RE* smallest = min_element(first, last);
                                // make a canonical choice
    w=rep(*smallest);
    return;
  }
  // if rF1 is set, then use it instead, setting w = rF1(X^t) mod Ft(X)
  RXModulus Ft(factors[i]);
  //  long tInv = InvMod(t,m);
  RX X2t = PowerXMod(t,Ft);    // X2t = X^t mod Ft
  w = CompMod(*rF1,X2t,Ft);      // w = F1(X2t) mod Ft

  /* Debugging sanity-check: G(w)=0 in the extension field (Z/2Z)[X]/Ft(X)
  RE::init(factors[i]);
  REX Ga;
  conv(Ga, G); // G as a polynomial over the extension field
  RE ra;
  conv(ra, w);         // w is an element in the extension field
  eval(ra,Ga,ra);  // ra = Ga(ra)
  if (!IsZero(ra)) {// check that Ga(w)=0 in this extension field
    cout << "rF1(X^t) mod Ft(X) != root of G mod Ft, t=" << t << endl;
    exit(0);
  }*******************************************************************/
}

template<class type>
void PAlgebraModDerived<type>::mapToSlots(MappingData<type>& mappingData, const RX& G) const
{
  assert(deg(G) > 0 && zMStar.getOrdP() % deg(G) == 0);
  assert(LeadCoeff(G) == 1);
  mappingData.G = G;
  mappingData.degG = deg(mappingData.G);

  long nSlots = zMStar.getNSlots();
  long m = zMStar.getM();

  mappingData.maps.resize(nSlots);

  mapToF1(mappingData.maps[0],mappingData.G); // mapping from base-G to base-F1
  for (long i=1; i<nSlots; i++)
    mapToFt(mappingData.maps[i], mappingData.G, zMStar.ith_rep(i), &(mappingData.maps[0]));

  REBak bak; bak.save();
  RE::init(mappingData.G);
  mappingData.contextForG.save();

  if (deg(mappingData.G)==1) return;

  mappingData.rmaps.resize(nSlots);

  if (G == factors[0]) {
    // an important special case

    for (long i = 0; i < nSlots; i++) {
        long t = zMStar.ith_rep(i);
        long tInv = InvMod(t, m);

        RX ct_rep;
        PowerXMod(ct_rep, tInv, G);

        RE ct;
        conv(ct, ct_rep);

        REX Qi;
        SetCoeff(Qi, 1, 1);
        SetCoeff(Qi, 0, -ct);

        mappingData.rmaps[i] = Qi;
    }
  }
  else
  {
    // the general case: currently only works when r == 1

    assert(r == 1);

    vec_REX FRts;
    for (long i=0; i<nSlots; i++) {
      // We need to lift Fi from R[Y] to (R[X]/G(X))[Y]
      REX  Qi;
      long t, tInv=0;

      if (i == 0) {
        conv(Qi,factors[i]);
        FRts=EDF(Qi, FrobeniusMap(Qi), deg(Qi)/deg(G));
        // factor Fi over GF(p)[X]/G(X)
      }
      else {
        t = zMStar.ith_rep(i);
        tInv = InvMod(t, m);
      }

      // need to choose the right factor, the one that gives us back X
      long j;
      for (j=0; j<FRts.length(); j++) {
        // lift maps[i] to (R[X]/G(X))[Y] and reduce mod j'th factor of Fi

        REX FRtsj;
        if (i == 0)
           FRtsj = FRts[j];
        else {
            REX X2tInv = PowerXMod(tInv, FRts[j]);
            IrredPolyMod(FRtsj, X2tInv, FRts[j]);
        }

        // FRtsj is the jth factor of factors[i] over the extension field.
        // For j > 0, we save some time by computing it from the jth factor
        // of factors[0] via a minimal polynomial computation.

        REX GRti;
        conv(GRti, mappingData.maps[i]);
        GRti %= FRtsj;

        if (IsX(rep(ConstTerm(GRti)))) { // is GRti == X?
          Qi = FRtsj;                // If so, we found the right factor
          break;
        } // If this does not happen then move to the next factor of Fi
      }

      assert(j < FRts.length());
      mappingData.rmaps[i] = Qi;
    }
  }
}

template<class type>
void PAlgebraModDerived<type>::decodePlaintext(
   vector<RX>& alphas, const RX& ptxt, const MappingData<type>& mappingData) const
{
  long nSlots = zMStar.getNSlots();
  if (isDryRun()) {
    alphas.assign(nSlots, RX::zero());
    return;
  }

  // First decompose p into CRT components
  vector<RX> CRTcomps(nSlots); // allocate space for CRT component
  CRT_decompose(CRTcomps, ptxt);  // CRTcomps[i] = p mod facors[i]

  if (mappingData.degG==1) {
    alphas = CRTcomps;
    return;
  }

  alphas.resize(nSlots);

  REBak bak; bak.save(); mappingData.contextForG.restore();

  for (long i=0; i<nSlots; i++) {
    REX te;
    conv(te, CRTcomps[i]);   // lift i'th CRT componnet to mod G(X)
    te %= mappingData.rmaps[i];  // reduce CRTcomps[i](Y) mod Qi(Y), over (Z_2[X]/G(X))

    // the free term (no Y component) should be our answer (as a poly(X))
    alphas[i] = rep(ConstTerm(te));
  }
}

template<class type>
void PAlgebraModDerived<type>::decodeSlots(std::vector <RX> &alphas,
                                           const RX &ptxt,
                                           const std::vector<long> &positions,
                                           const MappingData <type> &mappingData) const
{
  long nSlots = zMStar.getNSlots();
  for (auto position : positions)
    assert(position >= 0 && position < nSlots && "Invalid positions");

  if (isDryRun()) {
    alphas = std::vector<RX>(positions.size(), RX::zero());
    return;
  }

  size_t nPos = positions.size();
  vector<RX> CRTcomps(nPos); // allocate space for CRT component
  for (size_t i = 0; i < nPos; i++)
    rem(CRTcomps[i], ptxt, factors[positions[i]]); // CRTcomp = H % factors[i]

  if (mappingData.degG==1) {
    alphas = CRTcomps;
    return;
  }

  alphas.resize(nPos);

  REBak bak; bak.save(); mappingData.contextForG.restore();

  for (size_t i = 0; i < nPos; i++) {
    REX te;
    conv(te, CRTcomps[i]);   // lift i'th CRT componnet to mod G(X)
    te %= mappingData.rmaps[positions[i]];  // reduce CRTcomps[i](Y) mod Qi(Y), over (Z_2[X]/G(X))

    // the free term (no Y component) should be our answer (as a poly(X))
    alphas[i] = rep(ConstTerm(te));
  }
}

template<class type>
void PAlgebraModDerived<type>::decode1Slot(RX &alpha, const RX &ptxt, const long i,
                                           const MappingData <type> &mappingData) const
{
  std::vector<RX> oneSlot;
  std::vector<long> position(1, i);
  decodeSlots(oneSlot, ptxt, position, mappingData);
  alpha = position.front();
}

template<class type>
void PAlgebraModDerived<type>::
buildLinPolyCoeffs(vector<RX>& C, const vector<RX>& L,
                   const MappingData<type>& mappingData) const
{
  REBak bak; bak.save(); mappingData.contextForG.restore();

  long d = RE::degree();
  long p = zMStar.getP();

  assert(lsize(L) == d);

  vec_RE LL;
  LL.SetLength(d);

  for (long i = 0; i < d; i++)
    conv(LL[i], L[i]);

  vec_RE CC;
  ::buildLinPolyCoeffs(CC, LL, p, r);

  C.resize(d);
  for (long i = 0; i < d; i++)
    C[i] = rep(CC[i]);
}

// code for generating mask tables
// the tables are generated "on demand"

template<class type>
void PAlgebraModDerived<type>::genMaskTable()
{
  // This is only called by the constructor, which has already
  // set the zz_p context

  RX tmp1;

  maskTable.resize(zMStar.numOfGens());
  for (long i = 0; i < (long)zMStar.numOfGens(); i++) {
    long ord = zMStar.OrderOf(i);
    maskTable[i].resize(ord+1);
    maskTable[i][ord] = 0;
    for (long j = ord-1; j >= 1; j--) {
      // initialize mask that is 1 whenever the ith coordinate is at least j
      // Note: maskTable[i][0] = constant 1, maskTable[i][ord] = constant 0
      maskTable[i][j] = maskTable[i][j+1];
      for (long k = 0; k < (long)zMStar.getNSlots(); k++) {
         if (zMStar.coordinate(i, k) == j) {
           div(tmp1, PhimXMod, factors[k]);
           mul(tmp1, tmp1, crtCoeffs[k]);
           add(maskTable[i][j], maskTable[i][j], tmp1);
         }
      }
    }
    maskTable[i][0] = 1;
  }
}

// code for generating crt tables
// the tables are generated "on demand"

template<class type>
void PAlgebraModDerived<type>::genCrtTable()
{
  // This is only called by the constructor, which has already
  // set the zz_p context

  long nslots = zMStar.getNSlots();
  crtTable.resize(nslots);
  for (long i = 0; i < nslots; i++) {
    RX allBut_i = PhimXMod / factors[i]; // = \prod_{j \ne i }Fj
    allBut_i *= crtCoeffs[i]; // = 1 mod Fi and = 0 mod Fj for j \ne i
    crtTable[i] = allBut_i;
  }

  buildTree(crtTree, 0, nslots);
}

template<class type>
void PAlgebraModDerived<type>::
  buildTree(shared_ptr< TNode<RX> >& res, long offset, long extent) const
{
  if (extent == 1)
    res = buildTNode<RX>(nullTNode<RX>(), nullTNode<RX>(),
                            factors[offset]);
  else {
    long half = extent/2;
    shared_ptr< TNode<RX> > left, right;
    buildTree(left, offset, half);
    buildTree(right, offset+half, extent-half);
    RX data = left->data * right->data;
    res = buildTNode<RX>(left, right, data);
  }
}

template<class type>
void PAlgebraModDerived<type>::evalTree(RX& res,
              shared_ptr< TNode<RX> > tree,
              const vector<RX>& crt1,
              long offset, long extent) const
{
  if (extent == 1)
    res = crt1[offset];
  else {
    long half = extent/2;
    RX lres, rres;
    evalTree(lres, tree->left, crt1, offset, half);
    evalTree(rres, tree->right, crt1, offset+half, extent-half);
    RX tmp1, tmp2;
    mul(tmp1, lres, tree->right->data);
    mul(tmp2, rres, tree->left->data);
    add(tmp1, tmp1, tmp2);
    res = tmp1;
  }
}

// Explicit instantiation

template class PAlgebraModDerived<PA_GF2>;
template class PAlgebraModDerived<PA_zz_p>;

// Helper function
CubeSignature::CubeSignature(const PAlgebra& alg): ndims(0)
{
  Vec<long> _dims(INIT_SIZE, alg.numOfGens());
  for (long i=0; i<(long)alg.numOfGens(); i++) _dims[i] = alg.OrderOf(i);
  initSignature(_dims);
}


// Copyright (c) 2019, Torsten Sattler
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * 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.
//
//     * 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 <COPYRIGHT HOLDER> 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.
//
// author: Torsten Sattler, torsten.sattler.de@googlemail.com

#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <random>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

#include "line_estimator.h"

namespace ransac_lib {

LineEstimator::LineEstimator(const Eigen::Matrix2Xd& data) {
  data_ = data;
  num_data_ = data_.cols();
}

int LineEstimator::MinimalSolver(const std::vector<int>& sample,
                                 std::vector<Eigen::Vector3d>* lines) const {
  lines->clear();
  if (sample.size() < 2u) return 0;

  lines->resize(1);
  Eigen::Vector3d p1(data_(0, sample[0]), data_(1, sample[0]), 1.0);
  Eigen::Vector3d p2(data_(0, sample[1]), data_(1, sample[1]), 1.0);
  (*lines)[0] = p1.cross(p2);
  // Normalizes the line such that the normal of the line has unit length.
  double normal_norm = (*lines)[0].head<2>().norm();
  if (normal_norm == 0.0) {
    lines->clear();
    return 0;
  }

  (*lines)[0] /= normal_norm;

  return 1;
}

int LineEstimator::NonMinimalSolver(const std::vector<int>& sample,
                                    Eigen::Vector3d* line) const {
  if (sample.size() < 6u) return 0;

  const int kNumSamples = static_cast<int>(sample.size());

  // We fit the line by estimating the eigenvectors of the covariance matrix
  // of the data.
  Eigen::Vector2d mean(0.0, 0.0);
  for (int i = 0; i < kNumSamples; ++i) {
    mean += data_.col(sample[i]);
  }
  mean /= static_cast<double>(kNumSamples);

  // Builds the covariance matrix C.
  Eigen::Matrix2d C = Eigen::Matrix2d::Zero();

  for (int i = 0; i < kNumSamples; ++i) {
    Eigen::Vector2d d = data_.col(sample[i]) - mean;
    C += d * d.transpose();
  }
  C /= static_cast<double>(kNumSamples - 1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig_solver(C);
  if (eig_solver.info() != Eigen::Success) return 0;

  line->head<2>() = eig_solver.eigenvectors().col(1);

  // Re-estimates the translation along the line to account for subtraction
  // of mean.
  (*line)[2] = -line->head<2>().dot(mean);

  return 1;
}

// Evaluates the line on the i-th data point.
double LineEstimator::EvaluateModelOnPoint(const Eigen::Vector3d& line,
                                           int i) const {
  double residual = line.dot(data_.col(i).homogeneous());
  return residual * residual;
}

}  // namespace ransac_lib