/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  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 Autonomous Systems Lab / ETH Zurich 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 OWNER 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.
 *
 *  Created on: Jan 7, 2014
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file odeHybrid.hpp
 * @brief Runge Kutta based IMU forward and backward propagation with an extended IMU model.
 * developed from ode.hpp in okvis.
 * @author Jianzhu Huai
 */

#ifndef INCLUDE_SWIFT_VIO_ODE_HYBRID_HPP_
#define INCLUDE_SWIFT_VIO_ODE_HYBRID_HPP_

#include <swift_vio/imu/ImuErrorModel.h>

#include <Eigen/Core>

#include <okvis/FrameTypedefs.hpp>
#include <okvis/Measurements.hpp>
#include <okvis/Variables.hpp>
#include <okvis/assert_macros.hpp>
#include <okvis/kinematics/Transformation.hpp>
#include <okvis/kinematics/operators.hpp>

namespace swift_vio {
namespace ode {
const int kNavErrorStateDim = 9;
// Note this function assume that the W frame has z axis along the negative gravity.
// This fact is used in computing velocity in the W frame.
__inline__ void evaluateContinuousTimeOde(
    const Eigen::Vector3d& gyr, const Eigen::Vector3d& acc, double g,
    const Eigen::Vector3d& p_WS_W, const Eigen::Quaterniond& q_WS,
    const okvis::SpeedAndBiases& sb,
    const ImuErrorModel<double>& iem, Eigen::Vector3d& p_WS_W_dot,
    Eigen::Vector4d& q_WS_dot, okvis::SpeedAndBiases& sb_dot,
    Eigen::MatrixXd* F_c_ptr = 0) {
  Eigen::Vector3d omega_S;
  Eigen::Vector3d acc_S;
  iem.estimate(gyr, acc, &omega_S, &acc_S);

  // nonlinear states
  // start with the pose
  p_WS_W_dot = sb.head<3>();

  // now the quaternion
  Eigen::Vector4d dq;
  q_WS_dot.head<3>() = 0.5 * omega_S;
  q_WS_dot[3] = 0.0;
  Eigen::Matrix3d C_WS = q_WS.toRotationMatrix();

  // the rest is straightforward
  // consider Earth's radius. Model the Earth as a sphere, since we neither
  // know the position nor yaw (except if coupled with GPS and magnetometer).
  Eigen::Vector3d G =
      -p_WS_W - Eigen::Vector3d(0, 0, 6371009);  // vector to Earth center
  sb_dot.head<3>() = (C_WS * acc_S + g * G.normalized());  // s
  // biases
  sb_dot.tail<6>().setZero();

  // linearized system:
  if (F_c_ptr) {
    F_c_ptr->setZero();
    F_c_ptr->block<3, 3>(0, 6) = Eigen::Matrix3d::Identity();
    //    F_c_ptr->block<3, 3>(3, 9) -= C_WS;
    F_c_ptr->block<3, 3>(6, 3) -= okvis::kinematics::crossMx(C_WS * acc_S);
    //    F_c_ptr->block<3, 3>(6, 12) -= C_WS;
    Eigen::Matrix<double, 9, 6> N = Eigen::Matrix<double, 9, 6>::Zero();
    N.block<3, 3>(3, 0) = C_WS;
    N.block<3, 3>(6, 3) = C_WS;
    int colsF = F_c_ptr->cols();
    Eigen::Matrix<double, 6, 6 + 27> dwaB_dbgbaSTS;
    iem.dwa_B_dbgbaSTS(omega_S, acc_S, dwaB_dbgbaSTS);
    F_c_ptr->block(0, 9, 9, colsF - 9) = N * dwaB_dbgbaSTS.rightCols(colsF - 9);
  }
}

// p_WS_W, q_WS, sb are states at k
/* * covariance error states $\Delta y = \Delta[\mathbf{p}_{WS}^W,
\Delta\mathbf{\alpha}, \Delta\mathbf{v}_S^W,
 * \Delta\mathbf{b_g}, \Delta\mathbf{b_a}, \Delta\mathbf{Tg}, \Delta\mathbf{Ts},
\Delta\mathbf{Ta}]$
 * $\tilde{R}_s^w=(I-[\delta\alpha^w]_\times)R_s^w$
 * this DEFINITION OF ROTATION ERROR is the same in Mingyang Li's later
publications, okvis, and huai ION GNSS+ 2015 $y = [\mathbf{p}_{WS}^W,
\mathbf{q}_S^W, \mathbf{v}_S^W, \mathbf{b_g}, \mathbf{b_a}]$ $u =
[\mathbf{\omega}_{WS}^S,\mathbf{a}^S]$ $h = t_{n+1}-t_n$
$\mathbf{p}_{WS}^W \oplus = \mathbf{p}_{WS}^W +$
$\mathbf{v}_{WS}^W \oplus = \mathbf{v}_{WS}^W +$
$\mathbf{q}_{S}^W \oplus \mathbf{\omega}_{WS}^S h/2 =
\mathbf{q}_{S}^W\begin{bmatrix}
cos(\mathbf{\omega}_{WS}^S h/2) \\
sin(\mathbf{\omega}_{WS}^S
h/2)\frac{\mathbf{\omega}_{WS}^S/2}{\vert\mathbf{\omega}_{WS}^S/2\vert}
\end{bmatrix}$

$k_1 = f(t_n,y_n,u_n)$
$k_2 = f(t_n+h/2,y_n\oplus k_1 h/2 ,(u_n +u_{n+1})/2)$
$k_3 = f(t_n+h/2,y_n\oplus k_2 h/2 ,(u_n +u_{n+1})/2)$
$k_4 = f(t_n+h,y_n\oplus k_3 h , u_{n+1})$
$y_{n+1}=y_n\oplus\left(h(k_1 +2k_2 +2k_3 +k_4)/6 \right )$
Caution: provide both F_tot_ptr(e.g., identity) and P_ptr(e.g., zero matrix) if
covariance is to be computed
*/
__inline__ void integrateOneStep_RungeKutta(
    const Eigen::Vector3d& gyr_0, const Eigen::Vector3d& acc_0,
    const Eigen::Vector3d& gyr_1, const Eigen::Vector3d& acc_1, double g,
    double sigma_g_c, double sigma_a_c, double sigma_gw_c, double sigma_aw_c,
    double dt, Eigen::Vector3d& p_WS_W, Eigen::Quaterniond& q_WS,
    okvis::SpeedAndBiases& sb, const ImuErrorModel<double>& iem,
    Eigen::MatrixXd* P_ptr = 0,
    Eigen::MatrixXd* F_tot_ptr = 0) {
  Eigen::Vector3d k1_p_WS_W_dot;
  Eigen::Vector4d k1_q_WS_dot;

  okvis::SpeedAndBiases k1_sb_dot;
  int covRows = 0;
  Eigen::MatrixXd k1_F_c;
  Eigen::MatrixXd k2_F_c;
  Eigen::MatrixXd k3_F_c;
  Eigen::MatrixXd k4_F_c;
  Eigen::MatrixXd* k1_F_c_ptr = nullptr;
  Eigen::MatrixXd* k2_F_c_ptr = nullptr;
  Eigen::MatrixXd* k3_F_c_ptr = nullptr;
  Eigen::MatrixXd* k4_F_c_ptr = nullptr;
  if (P_ptr || F_tot_ptr) {
      covRows = P_ptr ? P_ptr->rows() : F_tot_ptr->rows();
      k1_F_c.resize(covRows, covRows);
      k2_F_c.resize(covRows, covRows);
      k3_F_c.resize(covRows, covRows);
      k4_F_c.resize(covRows, covRows);
      k1_F_c_ptr = &k1_F_c;
      k2_F_c_ptr = &k2_F_c;
      k3_F_c_ptr = &k3_F_c;
      k4_F_c_ptr = &k4_F_c;
  }

  evaluateContinuousTimeOde(gyr_0, acc_0, g, p_WS_W, q_WS, sb, iem,
                            k1_p_WS_W_dot, k1_q_WS_dot, k1_sb_dot, k1_F_c_ptr);

  Eigen::Vector3d p_WS_W1 = p_WS_W;
  Eigen::Quaterniond q_WS1 = q_WS;
  okvis::SpeedAndBiases sb1 = sb;
  // state propagation:
  p_WS_W1 += k1_p_WS_W_dot * 0.5 * dt;
  Eigen::Quaterniond dq;
  double theta_half = k1_q_WS_dot.head<3>().norm() * 0.5 * dt;
  double sinc_theta_half = okvis::kinematics::sinc(theta_half);
  double cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * k1_q_WS_dot.head<3>() * 0.5 * dt;
  dq.w() = cos_theta_half;
  q_WS1 = q_WS * dq;
  sb1 += k1_sb_dot * 0.5 * dt;

  Eigen::Vector3d k2_p_WS_W_dot;
  Eigen::Vector4d k2_q_WS_dot;
  okvis::SpeedAndBiases k2_sb_dot;
  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
                            p_WS_W1, q_WS1, sb1, iem, k2_p_WS_W_dot,
                            k2_q_WS_dot, k2_sb_dot, k2_F_c_ptr);

  Eigen::Vector3d p_WS_W2 = p_WS_W;
  Eigen::Quaterniond q_WS2 = q_WS;
  okvis::SpeedAndBiases sb2 = sb;
  // state propagation:
  p_WS_W2 += k2_p_WS_W_dot * 0.5 * dt;
  theta_half = k2_q_WS_dot.head<3>().norm() * 0.5 * dt;
  sinc_theta_half = okvis::kinematics::sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * k2_q_WS_dot.head<3>() * 0.5 * dt;
  dq.w() = cos_theta_half;
  // std::cout<<dq.transpose()<<std::endl;
  q_WS2 = q_WS2 * dq;
  sb2 += k1_sb_dot * 0.5 * dt;

  Eigen::Vector3d k3_p_WS_W_dot;
  Eigen::Vector4d k3_q_WS_dot;
  okvis::SpeedAndBiases k3_sb_dot;
  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
                            p_WS_W2, q_WS2, sb2, iem, k3_p_WS_W_dot,
                            k3_q_WS_dot, k3_sb_dot, k3_F_c_ptr);

  Eigen::Vector3d p_WS_W3 = p_WS_W;
  Eigen::Quaterniond q_WS3 = q_WS;
  okvis::SpeedAndBiases sb3 = sb;
  // state propagation:
  p_WS_W3 += k3_p_WS_W_dot * dt;
  theta_half = k3_q_WS_dot.head<3>().norm() * dt;
  sinc_theta_half = okvis::kinematics::sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * k3_q_WS_dot.head<3>() * dt;
  dq.w() = cos_theta_half;
  // std::cout<<dq.transpose()<<std::endl;
  q_WS3 = q_WS3 * dq;
  sb3 += k3_sb_dot * dt;

  Eigen::Vector3d k4_p_WS_W_dot;
  Eigen::Vector4d k4_q_WS_dot;
  okvis::SpeedAndBiases k4_sb_dot;
  evaluateContinuousTimeOde(gyr_1, acc_1, g, p_WS_W3, q_WS3, sb3, iem,
                            k4_p_WS_W_dot, k4_q_WS_dot, k4_sb_dot, k4_F_c_ptr);

  // now assemble
  p_WS_W +=
      (k1_p_WS_W_dot + 2 * (k2_p_WS_W_dot + k3_p_WS_W_dot) + k4_p_WS_W_dot) *
      dt / 6.0;
  Eigen::Vector3d theta_half_vec =
      (k1_q_WS_dot.head<3>() +
       2 * (k2_q_WS_dot.head<3>() + k3_q_WS_dot.head<3>()) +
       k4_q_WS_dot.head<3>()) *
      dt / 6.0;
  theta_half = theta_half_vec.norm();
  sinc_theta_half = okvis::kinematics::sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * theta_half_vec;
  dq.w() = cos_theta_half;
  q_WS = q_WS * dq;
  sb += (k1_sb_dot + 2 * (k2_sb_dot + k3_sb_dot) + k4_sb_dot) * dt / 6.0;

  q_WS.normalize();  // do not accumulate errors!

  if (F_tot_ptr) {
    // compute state transition matrix, note $\frac{d\Phi(t, t_0)}{dt}=
    // F(t)\Phi(t, t_0)$
    Eigen::MatrixXd& F_tot = *F_tot_ptr;
    const Eigen::MatrixXd& J1 = k1_F_c;
    const Eigen::MatrixXd J2 =
        k2_F_c * (Eigen::MatrixXd::Identity(covRows, covRows) + 0.5 * dt * J1);
    const Eigen::MatrixXd J3 =
        k3_F_c * (Eigen::MatrixXd::Identity(covRows, covRows) + 0.5 * dt * J2);
    const Eigen::MatrixXd J4 =
        k4_F_c * (Eigen::MatrixXd::Identity(covRows, covRows) + dt * J3);
    Eigen::MatrixXd F = Eigen::MatrixXd::Identity(covRows, covRows) +
                        dt * (J1 + 2 * (J2 + J3) + J4) / 6.0;
    F_tot =
        (F * F_tot)
            .eval();  // F is $\Phi(t_k, t_{k-1})$, F_tot is $\Phi(t_k, t_{0})$

    if (P_ptr) {
      Eigen::MatrixXd& cov = *P_ptr;
      cov = F * (cov * F.transpose()).eval();

      // add process noise
      const double Q_g = sigma_g_c * sigma_g_c * dt;
      const double Q_a = sigma_a_c * sigma_a_c * dt;
      const double Q_gw = sigma_gw_c * sigma_gw_c * dt;
      const double Q_aw = sigma_aw_c * sigma_aw_c * dt;
      cov(3, 3) += Q_g;
      cov(4, 4) += Q_g;
      cov(5, 5) += Q_g;
      cov(6, 6) += Q_a;
      cov(7, 7) += Q_a;
      cov(8, 8) += Q_a;
      cov(9, 9) += Q_gw;
      cov(10, 10) += Q_gw;
      cov(11, 11) += Q_gw;
      cov(12, 12) += Q_aw;
      cov(13, 13) += Q_aw;
      cov(14, 14) += Q_aw;

      // force symmetric
      // huai: this may help keep cov positive semi-definite after propagation
      cov = 0.5 * cov + 0.5 * cov.transpose().eval();
    }
  }
}

/* p_WS_W, q_WS, sb are states at k+1, dt= t(k+1) -t(k)
$y = [\mathbf{p}_{WS}^W, \mathbf{q}_S^W, \mathbf{v}_S^W, \mathbf{b_g},
\mathbf{b_a}]$ $u = [\mathbf{\omega}_{WS}^S,\mathbf{a}^S]$ $h = t_{n}-t_{n+1}$
$\mathbf{p}_{WS}^W \oplus = \mathbf{p}_{WS}^W +$
$\mathbf{v}_{WS}^W \oplus = \mathbf{v}_{WS}^W +$
$\mathbf{q}_{S}^W \oplus \mathbf{\omega}_{WS}^S h/2 =
\mathbf{q}_{S}^W\begin{bmatrix}
cos(\mathbf{\omega}_{WS}^S h/2) \\
sin(\mathbf{\omega}_{WS}^S
h/2)\frac{\mathbf{\omega}_{WS}^S/2}{\vert\mathbf{\omega}_{WS}^S/2\vert}
\end{bmatrix}$

$k_1 = f(t_{n+1},y_{n+1},u_{n+1})$
$k_2 = f(t_{n+1}+h/2,y_{n+1}\oplus k_1 h/2 ,(u_n +u_{n+1})/2)$
$k_3 = f(t_{n+1}+h/2,y_{n+1}\oplus k_2 h/2 ,(u_n +u_{n+1})/2)$
$k_4 = f(t_n,y_{n+1}\oplus k_3 h , u_{n})$
$y_{n}=y_{n+1}\oplus\left(h(k_1 +2k_2 +2k_3 +k_4)/6 \right )$
*/
__inline__ void integrateOneStepBackward_RungeKutta(
    const Eigen::Vector3d& gyr_0, const Eigen::Vector3d& acc_0,
    const Eigen::Vector3d& gyr_1, const Eigen::Vector3d& acc_1, double g,
    double sigma_g_c, double sigma_a_c, double sigma_gw_c, double sigma_aw_c,
    double dt, Eigen::Vector3d& p_WS_W, Eigen::Quaterniond& q_WS,
    okvis::SpeedAndBiases& sb, const ImuErrorModel<double>& iem,
    Eigen::MatrixXd* P_ptr = 0,
    Eigen::MatrixXd* F_tot_ptr = 0) {
  Eigen::Vector3d k1_p_WS_W_dot;
  Eigen::Vector4d k1_q_WS_dot;
  okvis::SpeedAndBiases k1_sb_dot;

  int covRows = 0;
  Eigen::MatrixXd k1_F_c;
  Eigen::MatrixXd k2_F_c;
  Eigen::MatrixXd k3_F_c;
  Eigen::MatrixXd k4_F_c;
  Eigen::MatrixXd* k1_F_c_ptr = nullptr;
  Eigen::MatrixXd* k2_F_c_ptr = nullptr;
  Eigen::MatrixXd* k3_F_c_ptr = nullptr;
  Eigen::MatrixXd* k4_F_c_ptr = nullptr;
  if (P_ptr || F_tot_ptr) {
      covRows = P_ptr ? P_ptr->rows() : F_tot_ptr->rows();
      k1_F_c.resize(covRows, covRows);
      k2_F_c.resize(covRows, covRows);
      k3_F_c.resize(covRows, covRows);
      k4_F_c.resize(covRows, covRows);
      k1_F_c_ptr = &k1_F_c;
      k2_F_c_ptr = &k2_F_c;
      k3_F_c_ptr = &k3_F_c;
      k4_F_c_ptr = &k4_F_c;
  }

  evaluateContinuousTimeOde(gyr_1, acc_1, g, p_WS_W, q_WS, sb, iem,
                            k1_p_WS_W_dot, k1_q_WS_dot, k1_sb_dot, k1_F_c_ptr);

  Eigen::Vector3d p_WS_W1 = p_WS_W;
  Eigen::Quaterniond q_WS1 = q_WS;
  okvis::SpeedAndBiases sb1 = sb;
  // state propagation:
  p_WS_W1 -= k1_p_WS_W_dot * 0.5 * dt;
  Eigen::Quaterniond dq;
  double theta_half = -k1_q_WS_dot.head<3>().norm() * 0.5 * dt;
  double sinc_theta_half = okvis::kinematics::sinc(theta_half);
  double cos_theta_half = cos(theta_half);
  dq.vec() = -sinc_theta_half * k1_q_WS_dot.head<3>() * 0.5 * dt;
  dq.w() = cos_theta_half;
  q_WS1 = q_WS * dq;
  sb1 -= k1_sb_dot * 0.5 * dt;

  Eigen::Vector3d k2_p_WS_W_dot;
  Eigen::Vector4d k2_q_WS_dot;
  okvis::SpeedAndBiases k2_sb_dot;
  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
                            p_WS_W1, q_WS1, sb1, iem, k2_p_WS_W_dot,
                            k2_q_WS_dot, k2_sb_dot, k2_F_c_ptr);

  Eigen::Vector3d p_WS_W2 = p_WS_W;
  Eigen::Quaterniond q_WS2 = q_WS;
  okvis::SpeedAndBiases sb2 = sb;
  // state propagation:
  p_WS_W2 -= k2_p_WS_W_dot * 0.5 * dt;
  theta_half = -k2_q_WS_dot.head<3>().norm() * 0.5 * dt;
  sinc_theta_half = okvis::kinematics::sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = -sinc_theta_half * k2_q_WS_dot.head<3>() * 0.5 * dt;
  dq.w() = cos_theta_half;
  // std::cout<<dq.transpose()<<std::endl;
  q_WS2 = q_WS2 * dq;
  sb2 -= k1_sb_dot * 0.5 * dt;

  Eigen::Vector3d k3_p_WS_W_dot;
  Eigen::Vector4d k3_q_WS_dot;
  okvis::SpeedAndBiases k3_sb_dot;
  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
                            p_WS_W2, q_WS2, sb2, iem, k3_p_WS_W_dot,
                            k3_q_WS_dot, k3_sb_dot, k3_F_c_ptr);

  Eigen::Vector3d p_WS_W3 = p_WS_W;
  Eigen::Quaterniond q_WS3 = q_WS;
  okvis::SpeedAndBiases sb3 = sb;
  // state propagation:
  p_WS_W3 -= k3_p_WS_W_dot * dt;
  theta_half = -k3_q_WS_dot.head<3>().norm() * dt;
  sinc_theta_half = okvis::kinematics::sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = -sinc_theta_half * k3_q_WS_dot.head<3>() * dt;
  dq.w() = cos_theta_half;
  // std::cout<<dq.transpose()<<std::endl;
  q_WS3 = q_WS3 * dq;
  sb3 -= k3_sb_dot * dt;

  Eigen::Vector3d k4_p_WS_W_dot;
  Eigen::Vector4d k4_q_WS_dot;
  okvis::SpeedAndBiases k4_sb_dot;
  evaluateContinuousTimeOde(gyr_0, acc_0, g, p_WS_W3, q_WS3, sb3, iem,
                            k4_p_WS_W_dot, k4_q_WS_dot, k4_sb_dot, k4_F_c_ptr);

  // now assemble
  p_WS_W -=
      (k1_p_WS_W_dot + 2 * (k2_p_WS_W_dot + k3_p_WS_W_dot) + k4_p_WS_W_dot) *
      dt / 6.0;
  Eigen::Vector3d theta_half_vec =
      -(k1_q_WS_dot.head<3>() +
        2 * (k2_q_WS_dot.head<3>() + k3_q_WS_dot.head<3>()) +
        k4_q_WS_dot.head<3>()) *
      dt / 6.0;
  theta_half = theta_half_vec.norm();
  sinc_theta_half = okvis::kinematics::sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * theta_half_vec;
  dq.w() = cos_theta_half;
  q_WS = q_WS * dq;
  sb -= (k1_sb_dot + 2 * (k2_sb_dot + k3_sb_dot) + k4_sb_dot) * dt / 6.0;

  q_WS.normalize();  // do not accumulate errors!

  if (F_tot_ptr) {
    assert(false);  // the following section is not well perused and tested
    // compute state transition matrix, note $\frac{d\Phi(t, t_0)}{dt}=
    // F(t)\Phi(t, t_0)$
    Eigen::MatrixXd& F_tot = *F_tot_ptr;
    const Eigen::MatrixXd& J1 = k1_F_c;
    const Eigen::MatrixXd J2 =
        k2_F_c * (Eigen::MatrixXd::Identity(covRows, covRows) - 0.5 * dt * J1);
    const Eigen::MatrixXd J3 =
        k3_F_c * (Eigen::MatrixXd::Identity(covRows, covRows) - 0.5 * dt * J2);
    const Eigen::MatrixXd J4 =
        k4_F_c * (Eigen::MatrixXd::Identity(covRows, covRows) - dt * J3);
    Eigen::MatrixXd F = Eigen::MatrixXd::Identity(covRows, covRows) -
                        dt * (J1 + 2 * (J2 + J3) + J4) / 6.0;
    F_tot = (F * F_tot).eval();

    if (P_ptr) {
      Eigen::MatrixXd& cov = *P_ptr;
      cov = F * (cov * F.transpose()).eval();

      // add process noise
      const double Q_g = sigma_g_c * sigma_g_c * dt;
      const double Q_a = sigma_a_c * sigma_a_c * dt;
      const double Q_gw = sigma_gw_c * sigma_gw_c * dt;
      const double Q_aw = sigma_aw_c * sigma_aw_c * dt;
      cov(3, 3) += Q_g;
      cov(4, 4) += Q_g;
      cov(5, 5) += Q_g;
      cov(6, 6) += Q_a;
      cov(7, 7) += Q_a;
      cov(8, 8) += Q_a;
      cov(9, 9) += Q_gw;
      cov(10, 10) += Q_gw;
      cov(11, 11) += Q_gw;
      cov(12, 12) += Q_aw;
      cov(13, 13) += Q_aw;
      cov(14, 14) += Q_aw;

      // force symmetric - TODO: is this really needed here?
      // cov = 0.5 * cov + 0.5 * cov.transpose().eval();
    }
  }
}
}  // namespace ode
}  // namespace swift_vio

#endif // INCLUDE_SWIFT_VIO_ODE_HYBRID_HPP_


/*
 *  sinker.cpp
 *  pybg
 *
 *  Created by BART MOSLEY on 8/12/12.
 *  Copyright 2012 BG Research LLC. All rights reserved.
 *
 */

#include <bg/bondgeek.hpp>

#include <iostream>

#include <boost/timer.hpp>

using namespace std;
using namespace QuantLib;
using namespace bondgeek;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {
    Integer sessionId() { return 0; }
}
#endif

void showCashFlows(Bond &bnd)
{
    Leg fixedLeg = bnd.cashflows();

    cout << endl << "Fixed Leg: " << endl;
    Leg::iterator fxIt;
    Date cfDate;
    double cfAmt;
    for (fxIt=fixedLeg.begin(); fxIt < fixedLeg.end(); fxIt++)
    {
        cfDate = (*fxIt)->date();
        cfAmt = (*fxIt)->amount();
        cout << cfDate << " | " << cfAmt << endl;
    }

}

void showAssetSwapCashFlows(AssetSwap &aswap)
{
    Leg fixedLeg = aswap.bondLeg();
    Leg floatLeg = aswap.floatingLeg();

    Date cfDate;
    double cfAmt;

    cout << endl << "Fixed Leg: " << endl;
    Leg::iterator fxIt;
    for (fxIt=fixedLeg.begin(); fxIt < fixedLeg.end(); fxIt++)
    {
        cfDate = (*fxIt)->date();
        cfAmt = (*fxIt)->amount();
        cout << cfDate << " | " << cfAmt << endl;
    }
    cout << endl << "Floating Leg: " << endl;
    Leg::iterator flIt;
    for (flIt=floatLeg.begin(); flIt < floatLeg.end(); flIt++)
    {
        cfDate = (*flIt)->date();
        cfAmt = (*flIt)->amount();
        cout << cfDate << " | " << cfAmt << endl;
    }

}

void showRedemptions(Bond &bnd)
{
    Leg fixedLeg = bnd.redemptions();

    cout << endl << "Redemptions: " << endl;
    Leg::iterator fxIt;
    Date cfDate;
    double cfAmt;
    for (fxIt=fixedLeg.begin(); fxIt < fixedLeg.end(); fxIt++)
    {
        cfDate = (*fxIt)->date();
        cfAmt = (*fxIt)->amount();
        cout << cfDate << " | " << cfAmt << endl;
    }

}

void showNotionals(Bond &bnd)
{
    std::vector<Real> bnd_notionals = bnd.notionals();

    cout << endl << "Notionals: " << endl;
    std::vector<Real>::iterator it;
    int notional_count = 0;
    for (it=bnd_notionals.begin(); it < bnd_notionals.end(); it++) {
        notional_count++;
        cout << notional_count << ": " << *it << endl;
    }

}

int main (int argc, char * const argv[])
{

    Date today = Date(16,October,2007);
    Settings::instance().evaluationDate() = today;

    cout << endl << "Today: " << today << endl;
    cout << "Price with a bg curve " << endl;
    string depotenors[] = {"1W", "1M", "3M", "6M", "9M"};
    double depospots[] = {0.0018400, 0.0023050, 0.0041825,
                          0.0070765, 0.0086780};
    string swaptenors[] = {"2y", "3y", "5y", "10Y",
                           "15Y", "20Y", "30Y"};
    double swapspots[] = {.0040100, 0.0042600, 0.0077300, 0.0106595,
                          0.0211555, 0.02310205, 0.0247007};

    cout << "Test with new curve " << endl;
    RateHelperCurve usdLiborCurve = RateHelperCurve(USDLiborCurve("3M"));
    usdLiborCurve.update(depotenors,
                         depospots,
                         5,
                         swaptenors,
                         swapspots,
                         7,
                         today);

    boost::shared_ptr<PricingEngine> discEngine = \
    createPriceEngine<DiscountingBondEngine>(
                                             usdLiborCurve.discountingTermStructure()
                                             );

    // set up the callable bond
    Real coupon = .06;
    Date maturity(15, September,2012);
    Date maturity1(15, September, 2011);
    Date maturity0(15, September, 2010);

    Date dated(16,September,2004);

    Natural settlementDays = 3;  // Bloomberg OAS1 settle is Oct 19, 2007
    Frequency frequency = Semiannual;

    Calendar bondCalendar = UnitedStates(UnitedStates::GovernmentBond);
    DayCounter bondDayCounter = ActualActual(ActualActual::Bond);

    Real faceAmount = 100.0; // Notional amount
    Real redemption = 100.0; // Amount paid on redemption

    BusinessDayConvention accrualConvention = Unadjusted;
    BusinessDayConvention paymentConvention = Unadjusted;

    cout << endl << endl
    << "Generic Bond"
    << endl;
    SinkingFundBond bnd(coupon,
                        maturity,
                        std::vector<Real>(1, faceAmount),
                        Annual,
                        dated,
                        bondCalendar,
                        settlementDays,
                        bondDayCounter,
                        frequency,
                        redemption,
                        faceAmount,
                        accrualConvention,
                        paymentConvention
                        );

    showCashFlows(bnd);

    cout << "\n\none year shorter" << endl;
    SinkingFundBond bnd0(coupon,
                         maturity0,
                         std::vector<Real>(1, faceAmount),
                         Annual,
                         dated,
                         bondCalendar,
                         settlementDays,
                         bondDayCounter,
                         frequency,
                         redemption,
                         faceAmount,
                         accrualConvention,
                         paymentConvention
                         );

    showCashFlows(bnd0);

    cout << "\n\ntwo years shorter" << endl;
    SinkingFundBond bnd1(coupon,
                         maturity1,
                         std::vector<Real>(1, faceAmount),
                         Annual,
                         dated,
                         bondCalendar,
                         settlementDays,
                         bondDayCounter,
                         frequency,
                         redemption,
                         faceAmount,
                         accrualConvention,
                         paymentConvention
                         );
    showCashFlows(bnd1);

    /* Sinkfing fund amortization:

     notionals start at par on the dated date and end at zero on maturity.
     redemptions are the prices redemption prices (pct of par, e.g. 101.)

     */

    double sf_sch[] = {
        40000, 40000, 40000, 40000, 40000, 40000,
        40000, 40000, 40000, 40000, 40000, 40000,
        40000
    };
    int sf_num = 3;
    Frequency sf_freq = Annual;

    std::vector<double> sf_bal;
    sf_bal.assign(sf_sch, sf_sch+sf_num);

    Real rval = 100.;

    cout << endl << endl
    << "Amortizing Generic Bond "
    << endl;
    SinkingFundBond bnd2(coupon,
                         maturity,
                         sf_bal,
                         sf_freq,
                         dated,
                         bondCalendar,
                         settlementDays,
                         bondDayCounter,
                         frequency,
                         rval,
                         faceAmount,
                         accrualConvention,
                         paymentConvention
                         );

    // show the results
    showCashFlows(bnd2);

    bnd0.setPricingEngine(discEngine);
    bnd1.setPricingEngine(discEngine);
    bnd.setPricingEngine(discEngine);
    bnd2.setPricingEngine(discEngine);

    double prc0 = bnd0.cleanPrice();
    double prc1 = bnd1.cleanPrice();
    double prc = bnd.cleanPrice();
    double prc2 = bnd2.cleanPrice();

    cout << endl << "Pricing component bonds" << endl
    << "priced0: " << prc0 << endl
    << "priced1: " << prc1 << endl
    << "priced: " << prc << endl
    << "avg price: " << (prc0+prc1+prc)/3. << endl
    << endl
    << "priced sinking fund bond: " << prc2 << endl;
    cout << "cf yield: " << bnd.toYield(prc2) << endl;

    return 0;
}


/*
 * Prover.cpp
 *
 *  Created on: 26.10.2010
 *      Author: stephaniebayer
 */

#include "Prover.h"

#include<vector>
#include "Cipher_elg.h"
#include "G_q.h"
#include "Mod_p.h"
#include "Functions.h"
#include "ElGammal.h"
#include "func_pro.h"
#include "fft.h"
#include<fstream>

#include <time.h>
#include <NTL/ZZ.h>
NTL_CLIENT

extern G_q H;
extern ElGammal El;

Prover::Prover() {
	// TODO Auto-generated constructor stub

}

Prover::Prover(vector<vector<Cipher_elg>* >* Cin, vector<vector<ZZ>* >* Rin, vector<vector<vector<long>* >* >* piin, vector<long> num, ZZ genq){

	// set the dimensions of the row and columns according to the user input
	m = num[1]; //number of rows
	n = num[2]; //number of columns
	C = Cin; //sets the reencrypted cipertexts to the input
	R = Rin; //sets the random elements to the input
	pi = piin; // sets the permutation to the input

	fft *fft_new = new fft;
	//o is a root of unity
	o = fft_new->r_o_u(genq, m, H.get_ord());

	A = new vector<vector<ZZ>* >(m);
	func_pro::set_A(A,pi, m, n);

	//Allocate the storage needed for the vectors
	chal_x6 = new vector<ZZ>(2*m); //x6, x6^2, ... challenges from round 6
	chal_y6 = new vector<ZZ>(n); //y6, y6^2, ... challenges form round 6
	chal_x8 = new vector<ZZ>(2*m +1); //x8, x8^2, ... challenges from round 8

	//Allocate the storage needed for the vectors
	c_A = new vector<Mod_p>(m+1); //commitments to the rows in A
	r_A = new vector<ZZ>(m+1); //random elements used for the commitments

	D = new vector<vector<ZZ>* >(m+1); //vector containing in the first row random values and in all others y*A(ij) + B(ij)-z
	D_h = new vector<vector<ZZ>* >(m); //Vector of the Hadamare products of the rows in D
	D_s = new vector<vector<ZZ>* >(m+1); //Shifted rows of D_h
	d = new vector<ZZ>(n); //containing random elements to proof product of D_hm
	Delta = new vector<ZZ>(n); //containing random elements to proof product of D_hm
	d_h = new vector<ZZ>(n); // vector containing the last row of D-h

	r_D_h = new vector<ZZ>(m);//random elements for commitments to D_h
	c_D_h = new vector<Mod_p>(m+2);//commitments to the rows in D_h

	B = new vector<vector<ZZ>* >(m);//matrix of permuted exponents, exponents are x2^i, i=1, ..N
	B_0 = new vector<ZZ>(n); //vector containing random exponents
	r_B = new vector<ZZ>(m); //random elements used to commit to T
	c_B = new vector<Mod_p>(m); //vector of commitments to rows in T
	a = new vector<ZZ>(2*m); //elements used for reencryption in round 5
	r_a = new vector<ZZ>(2*m); //random elements to commit to elements in a
	c_a = new vector<Mod_p>(2*m); //commitments to elements a
	E = new vector<Cipher_elg>(2*m); //
	rho_a = new vector<ZZ>(2*m); //

	Dl = new vector<ZZ>(2*m+1); //bilinear_map(Y_pi, U, chal_t)
	r_Dl = new vector<ZZ>(2*m+1); //random elements to commit to the C_ls
	c_Dl = new vector<Mod_p>(2*m +1); //commitments to the C_ls

	d_bar = new vector<ZZ>(n);// chal_x8*D_h(m-1) +d
	Delta_bar = new vector<ZZ>(n);//chal_x8*d_h+Delta
	D_h_bar = new vector<ZZ>(n);//sum over the rows in D_h

	B_bar  = new vector<ZZ>(n); // sum over the rows in T multiplied by chal^i

	A_bar = new vector<ZZ>(n); //sum over the rows in Y times the challenges
	D_s_bar = new vector<ZZ>(n); // sum over the rows in U times thes challenges

}
//Destructor deletes all pointers and frees the storage
Prover::~Prover() {
	delete chal_x6;
	delete chal_y6;
	delete chal_x8;
	delete c_A;
	delete r_A;

	Functions::delete_vector(D);
	Functions::delete_vector(D_h);
	Functions::delete_vector(D_s);
	delete d;
	delete Delta;
	delete d_h;

	delete r_D_h;
	delete c_D_h;
	Functions::delete_vector(B);
	delete B_0;
	delete r_B;
	delete c_B;
	delete a;
	delete r_a;
	delete c_a;
	delete rho_a;
	delete Dl;
	delete r_Dl;
	delete c_Dl;

	delete D_h_bar;
	delete d_bar;
	delete Delta_bar;
	delete B_bar;
	delete A_bar;
	delete D_s_bar;
}

//round_1 picks random elements and commits to the rows of Y
string Prover::round_1(){
	long i;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//calculates commitments to rows of Y
	name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"commitments to A and random values "<<endl;
	ost.close();
	Functions::commit(A,r_A, c_A);

	//write commitments in the file name
	name = "round_1 ";
	name = name + ctime(&rawtime);
	ost.open(name.c_str());
	for (i=0; i<m; i++){
		ost << c_A->at(i)<< " ";
	}
	return name;

}

//round_3, permuted the exponents in s,  picks random elements and commits to values
string Prover::round_3(string in_name){
	long i;
	ZZ x2;
	vector<vector<ZZ>* >* chal_x2 = new vector<vector<ZZ>* >(m);

	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads in values of s
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	ist >> x2;
	name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"x ist "<<x2<<endl;
	//creates a matrix with entries x2,..., x2^N
	func_pro::set_x2(chal_x2,x2, m,n);

	//permutes chal_x2 according pi to create B
	func_pro::set_B(B, chal_x2, pi);

	//commits to the rows in B
	ost<<"commitment to B "<<endl;
	Functions::commit(B,r_B,c_B);
	ost.close();

	name = "round_3 ";
	name = name + ctime(&rawtime);

	//write data in the file name
	 ost.open(name.c_str());
	for (i=0; i<m;i++){
		ost << c_B->at(i) <<" ";
	}

	Functions::delete_vector(chal_x2);
	return name;
}

//round_5a calculates Y_pi and the commitments to the vectors alpha, W
void Prover::round_5a(){
	long i;
	ZZ temp, t; //temporary variables
	vector<ZZ>* r = new vector<ZZ>(n);
	vector<ZZ>* v_z = new vector<ZZ>(n); //row containing the challenge alpha
	ZZ ord = H.get_ord();
	time_t rawtime;
	time ( &rawtime );

	//calculate for each value in the first m rows in D: y* A_ij + A_ij -z
	func_pro::set_D(D, A,B, chal_z4, chal_y4);

	//Set the matrix D_h as the Hadamard product of the rows in D
	func_pro::set_D_h(D_h, D);

	for( i=0; i<n;i++){
		v_z->at(i) = chal_z4; //fills the vector z with the challenge z
		NegateMod(r->at(i),to_ZZ(1),ord);
	}

	//Sets the additional row in D to contain -1
	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"Last row in D ist -1 "<<endl;
	D->at(m) = r;
	//random number to commit to last row in A
	ost<<"r_A to commit to last row in A ist "<<0<<endl;
	r_A->at(m) = 0;

	//calculate commitment to
	ost<<"commitment  to vector z"<<endl;
	Functions::commit(v_z, r_z, c_z);
	//calculate commitment to the rows in D_h
	ost<<"Commitments to Hadamard product over D "<<endl;
	Functions::commit(D_h,r_D_h,c_D_h);

	delete v_z;
}

void Prover::round_5b(){

	//picks random values to set B0 and commits to it
	func_pro::commit_B0(B_0, r_B0, c_B0);
	//picks random values to set A and commits to them;
	func_pro::commit_a(a, r_a, c_a);

}

void Prover::round_5c(){
	vector<Cipher_elg>*  e = 0;
	double tstart, tstop, ttime;

	//calculates the value R_t as sum(T(ij)*r(ij));
	func_pro::set_Rb(B,R,R_b);

	tstart = (double)clock()/CLOCKS_PER_SEC;
		e= calculate_e();
	tstop = (double)clock()/CLOCKS_PER_SEC;
	ttime= tstop-tstart;
	//cout << "To calculate the di's took " << ttime << " second(s)." << endl;

	calculate_E(e);
	delete e;
}

string Prover::round_5(string in_name ){
	long i;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads the values out of the file
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	ist>> chal_z4;
	ist >> chal_y4;

	 name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"z "<<chal_z4<<" y "<<chal_y4<<endl;
	ost.close();
	round_5a();
	round_5b();
	round_5c();
	//Set name of the output file and open stream
	name = "round_5 ";
	name = name + ctime(&rawtime);

	ost.open(name.c_str());

	//writes the commitments in the file
	ost<<  c_z<< "\n";
	for (i = 0; i<m ; i++){
		ost << c_D_h ->at(i)<< " ";
	}
	ost << c_B0<< "\n ";
	for (i = 0; i<2*m ; i++){
		ost << c_a ->at(i)<< " ";
	}
	ost << "\n";
	for (i = 0; i<2*m ; i++){
		ost << E ->at(i)<< " ";
	}

	delete E;
	return name;
}

void Prover::round_7a(){
	//Set the rows in D_s as D_s(i) = chal_t_1^i+1*D_h(i) for i<m-1 and D_s(m-1) = sum(chal_x6^i+1 * D_s(i+1) and set last row of D_s to random values and also D(0)

	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"matrix D_s = chal_t_1^i+1*D_h(i) for i<m-1 and D_s(m-1) = sum(chal_x6^i+1 * D_s(i+1) "<<endl;
	func_pro::set_D_s(D_s,D_h,D,chal_x6,r_Dl_bar);

	//calculate the values Dls as Dl(l) = sum(D(i)*D_s->at(i)*chal_y6) for j=n+i-l and commits to the values
	func_pro::commit_Dl(c_Dl,Dl, r_Dl, D, D_s, chal_y6, o);

	//commitments to D(0) and D_s(m)
	ost<<"commitment to D(0) and D_s(m) "<<endl;
	Functions::commit(D->at(0),r_D0,c_D0);
	Functions::commit(D_s->at(m), r_Dm, c_Dm);

	//commitments to prove that the product over the elements in D_h->at(m) is the desired product of n *y + x2n -z
	ost<<"commitment to d "<<endl;
	func_pro::commit_d(d,r_d,c_d);
	func_pro::commit_Delta(Delta, d, r_Delta, c_Delta);
	func_pro::commit_d_h(D_h,d_h,d,Delta, r_d_h, c_d_h);
}

void Prover::round_7b(){
	//calculate B_bar = sum(chal_x6^i B(i)), opening to prove knowledge of B
	func_pro::calculate_B_bar(B_0, B,chal_x6, B_bar );

	//calculate r_B_bar= sum(chal_x6^i r_B(i)), opening to prove knowledge of B
	func_pro::calculate_r_B_bar(r_B, chal_x6,r_B0, r_B_bar );

	//calculate a_bar = sum(chal_x6 ^i a(i)), opening to prove reencryption
	func_pro::calculate_a_bar(a, chal_x6, a_bar);

	//calculate r_a_bar= sum(chal_x6 ^i r_a(i)), opening to prove reencryption
	func_pro::calculate_r_a_bar(r_a, chal_x6, r_a_bar);

	//calculate rho_a_bar = sum(chal_x6 ^i rho_a(i)), opening to prove reencryption
	func_pro::calculate_rho_a_bar(rho_a, chal_x6, rho_bar);
}

string Prover::round_7(string in_name){
	long i;
	long l=2*m;
	string name;
	time_t rawtime;
	time ( &rawtime );
	//reads the values out of the file
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	//reads the vector t_1 and t
	for(i=0; i<l; i++){
		ist>>chal_x6->at(i);
	}
	for (i = 0; i<n; i++){
		ist >> chal_y6->at(i);
	}

	round_7a();
	round_7b();

	//Set name of the output file and open stream
	name = "round_7 ";
	name = name + ctime(&rawtime);

	ofstream ost(name.c_str());
	for (i = 0; i<=l ; i++){
		ost << c_Dl ->at(i)<< " ";
	}
	ost << "\n";
	ost<<c_D0<<"\n";
	ost <<c_Dm<<"\n";
	ost<<c_d<<"\n";
	ost<<c_Delta<<"\n";
	ost<<c_d_h<<"\n";
	for (i = 0; i<n; i++){
		ost << B_bar->at(i)<<" ";
	}
	ost <<"\n";

	ost<< r_B_bar;
	ost <<"\n";

	ost<< a_bar;
	ost <<"\n";

	ost<< r_a_bar;
	ost <<"\n";

	ost<< rho_bar;

	return name;
}

void Prover::round_9a(){

	//Calculate D_h_bar = sum(chal^i*D_h(row(i)))
	func_pro::calculate_D_h_bar(D_h_bar,D_h,chal_x8);

	//calculate r_Dh_bar = sum(chal^i*r_Dh_bar(i)), opening to prove correctness of D_h
	func_pro::calculate_r_Dh_bar(r_D_h, chal_x8, r_Dh_bar);

	//calculate d_bar, r_d_bar, Delta_bar, r_Delta_bar, openings to prove product over elements in D_h->at(m-1)
	func_pro::calculate_dbar_rdbar(D_h, chal_x8, d_bar,d,r_D_h, r_d, r_d_bar);
	func_pro::calculate_Deltabar_rDeltabar(d_h, chal_x8, Delta_bar, Delta, r_d_h, r_Delta, r_Delta_bar);
}

void  Prover::round_9b(){
	//A_bar and r_A_bar, openings to prove permutation in D
	func_pro::calculate_A_bar(D, A_bar, chal_x8);
	func_pro::calculate_r_A_bar(r_D0, r_A, r_B, chal_x8, r_z, chal_y4, r_A_bar);

	//D_s_bar and r_Ds_bar, openings to prove correctness of D_s
	func_pro::calculate_D_s_bar(D_s, D_s_bar, chal_x8);
	func_pro::calculate_r_Ds_bar(r_D_h, chal_x6, chal_x8, r_Ds_bar, r_Dm);

	//sum of the random values used to commit to the Dl's, to prover correctness of them
	func_pro::calculate_r_Dl_bar(r_Dl, chal_x8, r_Dl_bar);
}

string Prover::round_9(string in_name){
	long i;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads the values out of the file
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	//reads the vector e
	for (i = 0; i<(signed) chal_x8->size(); i++){
		ist >> chal_x8->at(i);
	}

	name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);

	ost<<"chal_x8 "<< chal_x8->at(0)<<endl;
	ost.close();

	round_9a();
	round_9b();

	//Set name of the output file and open stream
	name = "round_9 ";
	name = name + ctime(&rawtime);

	ost.open(name.c_str());

	for (i = 0; i<n; i++){
		ost << D_h_bar->at(i)<<" ";
	}
	ost <<"\n";
	ost<< r_Dh_bar;
	ost <<"\n";

	for(i=0; i<n;i++){
		ost<<d_bar->at(i)<<" ";
	}
	ost<<"\n";
	ost<< r_d_bar <<"\n";

	for(i=0; i<n; i++){
		ost<<Delta_bar->at(i)<<" ";
	}
	ost<<"\n";
	ost<<r_Delta_bar <<"\n";

	for (i = 0; i<n; i++){
		ost << A_bar->at(i)<<" ";
	}
	ost<<"\n";
	ost<<r_A_bar<<"\n";
	for(i=0; i<n; i++){
		ost<<D_s_bar->at(i)<<" ";
	}
	ost<<"\n";
	ost<<r_Ds_bar<<"\n";
	ost<<r_Dl_bar<<"\n";

	return name;
}

Cipher_elg Prover::calculate_e_1(long pos){
	long i;
	Cipher_elg temp, temp_1;

	Cipher_elg::expo(temp, C->at(pos)->at(0), B_0->at(0));
	for (i = 1; i<n; i++){
		Cipher_elg::expo(temp_1, C->at(pos)->at(i), B_0->at(i));
		Cipher_elg::mult(temp,temp, temp_1);
	}

	return temp;
}

Cipher_elg Prover::calculate_e_2(long k){
	long i,j,l;
	Cipher_elg temp, temp_1;
	ZZ mod = H.get_mod();

	temp=Cipher_elg(1,1,mod);
	for (i = 1; i<=m; i++){
		l = k-m+i;
		if((l>0) & (l<=m)){
			for(j = 0; j<n;j++){
				Cipher_elg::expo(temp_1,C->at(i-1)->at(j), B->at(l-1)->at(j));
				Cipher_elg::mult(temp,temp,temp_1);
			}
		}
	}

	return temp;
}

vector<Cipher_elg>* Prover::calculate_e(){
	long k,l;
	Cipher_elg temp, temp_1;
	vector<Cipher_elg>* e = new vector<Cipher_elg>(2*m);

	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"calculate the product of the diagonlas, values E "<<endl;
	e->at(0)= calculate_e_1(m-1);
	ost<<e->at(0)<<" ";

	l=m-1;
	for (k =1; k<=l; k++){
		temp_1 = calculate_e_1(m-k-1);
		temp = calculate_e_2(k);
		Cipher_elg::mult(temp,temp,temp_1);
		e->at(k) = temp;
		ost<<e->at(k)<<" ";
	}
	l=2*m-1;
	for (k = m; k<=l; k++){
		e->at(k) = calculate_e_2(k);
		ost<<e->at(k)<<" ";
	}
	ost<<endl;
	return e;
}

void Prover::calculate_E(vector<Cipher_elg>* e){
	long i,l;
	Mod_p t;
	Mod_p gen = H.get_gen();
	ZZ ord = H.get_ord();
	l=2*m;
	string name = "example.txt";
	ofstream ost;
	ost.open(name.c_str(),ios::app);
	ost<<"reencrypt values in e, pick random rho "<<endl;
	for (i = 0; i<l; i++){
		rho_a->at(i)= RandomBnd(ord);
		ost<<rho_a->at(i)<<" ";
	}
	ost<<endl;
	NegateMod(rho_a->at(m),R_b,ord);
	ost<<"rho(m) = -R_b  m"<<rho_a->at(m)<<" "<<m<<endl;
	ost<<"t=g^a encrypt(t, rho)e "<<endl;
	for (i = 0; i<l; i++){
		 t = gen.expo(a->at(i));
		 E->at(i) = El.encrypt(t,rho_a->at(i))*e->at(i);
		 ost<<t<<" "<<E->at(i)<<endl;
	}
}


// This file is part of DM-HEOM (https://github.com/noma/dm-heom)
//
// Copyright (c) 2015-2019 Matthias Noack, Zuse Institute Berlin
//
// Licensed under the 3-clause BSD License, see accompanying LICENSE,
// CONTRIBUTORS.md, and README.md for further information.

#include <exception>
#include <iostream>

#include <boost/format.hpp>
#include <noma/num/meta_stepper.hpp>

#include "heom/command_line.hpp"
#include "heom/common.hpp"
#include "heom/handle_main_exception.hpp"
#include "heom/hierarchy_graph.hpp"
#include "heom/hierarchy_mask.hpp"
#include "heom/hierarchy_norm.hpp"
#include "heom/instance.hpp"
#include "heom/make_file_observer_list.hpp"
#include "heom/ocl_config.hpp"
#include "heom/ode.hpp"
#include "heom/population_dynamics_config.hpp"
#include "heom/population_dynamics_solver.hpp"

namespace bmt = ::heom::bmt;
namespace num = ::heom::num;
namespace ocl = ::heom::ocl;
using num::int_t;
using num::real_t;

int main(int argc, char* argv[])
{
	// start runtime measurement
	bmt::timer app_timer;

	// output compile-time configuration
	std::cout << "-------------------- Compile-Time Configuration ------------" << std::endl;
	heom::write_compile_config(std::cout);
	std::cout << "------------------------------------------------------------" << std::endl;

	std::exception_ptr eptr;
	try {
		// command line parsing
		heom::command_line command_line { argc, argv };

		// parse config files
		std::cout << "Parsing OpenCL configuration from file: " << command_line.ocl_config_filename() << std::endl;
		heom::ocl_config ocl_config(command_line.ocl_config_filename());
		std::cout << "Parsing HEOM configuration from file: " << command_line.heom_config_filename() << std::endl;
		heom::population_dynamics_config heom_config(command_line.heom_config_filename());

		heom::hierarchy_graph complete_graph(heom_config.baths_number(), heom_config.baths_matsubaras(), heom_config.system_ado_depth());

		heom::instance heom_instance(heom_config, heom::sites_to_states_mode_t::identity, complete_graph);
		heom_instance.set_hierarchy_top(heom_config.population_dynamics_rho_init().data());

		using stepper_t = num::meta_stepper; // uses solver_stepper_type from config
		using solver_t = heom::population_dynamics_solver<heom::ode, stepper_t, heom::hierarchy_norm>;

		// create a solver from configuration and instance
		const ocl::nd_range ocl_nd_range {
				{}, // offset
		        {1, static_cast<std::uint64_t>(heom_instance.matrices())}, // global size
		        {1, 1} // local size
			};

		solver_t solver(ocl_config, ocl_nd_range, heom_config, heom_instance);
		std::cout << "-------------------- OpenCL Runtime Configuration ----------" << std::endl;
		solver.write_ocl_runtime_config(std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;

		// with create_hierarchy_mask we create mask corresponding to configured filtering strategy, then update solver
		// NOTE: .data() returns pointer to mask_t array
		heom::hierarchy_mask hierarchy_mask = heom::create_hierarchy_mask(heom_config.filtering_strategy(), complete_graph, heom_config.baths_matsubaras(), heom_config.filtering_first_layer());
		solver.update_hierarchy_mask(hierarchy_mask.data());

		std::cout << "-------------------- Hierarchy Mask Counter ----------------" << std::endl;
		heom::write_hierarchy_mask_stats(hierarchy_mask, std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;

		// setup output
		heom::observer_list observer_list = heom::make_file_observer_list(heom_config, complete_graph, solver);
		observer_list.observe(0.0);

		// run the solver, print output every n steps
		int_t iterations = heom_config.solver_steps() / heom_config.program_observe_steps();
		int_t steps_per_iteration = heom_config.program_observe_steps();
		int_t total_steps = iterations * heom_config.program_observe_steps();

		for (int_t i = 0; i < iterations; ++i) {
			// propagate
			solver.step_forward(steps_per_iteration);

			// after propagation, we are at:
			const auto current_step = (i + 1) * steps_per_iteration;
			const real_t current_time = current_step * heom_config.solver_step_size();

			// observe
//			solver.get_top_result(result_buffer_top);
			observer_list.observe(current_time);

			// update status
			heom::write_progress(current_step - 1, total_steps, "Calculation population dynamics: ", std::cout);
		}

		// output benchmark results
		std::cout << "-------------------- Solver Runtime Summary ----------------" << std::endl;
		solver.write_runtime_summary(std::cout);
		std::cout << "------------------------------------------------------------" << std::endl;
		std::cout << "-------------------- Memory Summary ------------------------" << std::endl;
		std::cout << "local hierarchy buffer size:            "
		          << boost::format("%11.2f MiB\n") % (heom_instance.size_hierarchy_byte() / 1024.0 / 1024.0);
		std::cout << "local instance size:                    "
		          << boost::format("%11.2f MiB\n") % (heom_instance.allocated_byte() / 1024.0 / 1024.0);
		std::cout << "max. heap via aligned::allocate(..):    "
		          << boost::format("%11.2f MiB") % (heom::memory::aligned::instance().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
		          << boost::format("count: %6i") % heom::memory::aligned::instance().allocations_max()
		          << ')' << std::endl;
		std::cout << "max. OpenCL buffer allocations:         "
		          << boost::format("%11.2f MiB") % (solver.ocl_helper().allocated_byte_max() / 1024.0 / 1024.0)
		          << " ("
			      << boost::format("count: %6i") % solver.ocl_helper().allocations_max()
			      << ')' << std::endl;
		std::cout << "------------------------------------------------------------" << std::endl;

	} catch (...) {
		eptr = std::current_exception();
	}
	int ret = heom::handle_main_exception(eptr);

	// print application runtime
	std::cout << "main(): application runtime: " << boost::format("%11.2f") % std::chrono::duration_cast<bmt::seconds>(bmt::duration(app_timer.elapsed())).count() << " s" << std::endl;

	return ret;
}


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "rcbdd_synthesis.hpp"
#include "synthesis_utils_p.hpp"

#include <fstream>

#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext/push_back.hpp>

#include <core/utils/terminal.hpp>
#include <core/utils/timer.hpp>
#include <reversible/functions/add_circuit.hpp>
#include <reversible/functions/add_gates.hpp>
#include <reversible/functions/pattern_to_circuit.hpp>
#include <reversible/io/print_circuit.hpp>
#include <classical/optimization/optimization.hpp>

#define timer timer_class
#include <boost/progress.hpp>
#undef timer

#include <cuddInt.h>

namespace cirkit
{

enum Direction {
  ChangeLeft = 0,
  ChangeRight
};

int Cudd_bddPickOneCubeForRCBDD( DdManager * ddm, DdNode * node, char * repr)
{
  DdNode *N, *T, *E;
  DdNode *one, *bzero;
  char   dir;
  int    i;

  if ( repr == nullptr || node == nullptr ) return 0;

  /* The constant 0 function has no on-set cubes. */
  one = DD_ONE(ddm);
  bzero = Cudd_Not(one);
  if (node == bzero) return 0;

  for ( i = 0; i < ddm->size; i++ ) repr[i] = 2;

  for (;;)
  {
    if ( node == one ) break;

    N = Cudd_Regular(node);

    T = cuddT(N); E = cuddE(N);
    if ( Cudd_IsComplement( node ) )
    {
	    T = Cudd_Not(T); E = Cudd_Not(E);
    }
    if ( T == bzero )
    {
	    repr[N->index] = 0;
	    node = E;
    }
    else if ( E == bzero )
    {
	    repr[N->index] = 1;
	    node = T;
    }
    else
    {
      if ( N->index % 3 == 1 )
      {
        repr[N->index] = repr[N->index - 1];
        node = repr[N->index] ? T : E;
      }
      else
      {
        dir = (char) ((Cudd_Random( ddm ) & 0x2000) >> 13);
        repr[N->index] = dir;
        node = dir ? T : E;
      }
    }
  }
  return 1;
}

int pick_one_cube_for_rcbdd( BDD node, char * repr )
{
  return Cudd_bddPickOneCubeForRCBDD( node.manager(), node.getNode(), repr );
}

struct rcbdd_synthesis_manager
{
  rcbdd_synthesis_manager( const rcbdd& _cf, circuit& _circ )
    : cf( _cf ),
      circ( _circ ),
      insert_position( 0u )
  {
    f = _cf.chi();

    circ.set_lines( _cf.num_vars() );

    std::vector<std::string> inputs( _cf.num_vars(), _cf.constant_value() ? "1" : "0" );
    boost::copy( cf.input_labels(), inputs.end() - _cf.num_inputs() );
    circ.set_inputs( inputs );

    std::vector<std::string> outputs( _cf.num_vars(), "-" );
    boost::copy( cf.output_labels(), outputs.begin() );
    circ.set_outputs( outputs );

    std::vector<constant> constants( _cf.num_vars(), constant() );
    std::fill( constants.begin(), constants.end() - _cf.num_inputs(), _cf.constant_value() );
    circ.set_constants( constants );

    std::vector<bool> garbage( _cf.num_vars(), true );
    std::fill( garbage.begin(), garbage.begin() + _cf.num_outputs(), false );
    circ.set_garbage( garbage );

    node_count += f.nodeCount();
  }

  void set_var(unsigned v)
  {
    _var = v;

    left_f = cf.manager().bddZero();
    right_f = cf.manager().bddZero();
  }

  void compute_cofactors()
  {
    n  = cf.cofactor(f, _var, false, false);
    pp = cf.cofactor(f, _var, true,  false);
    np = cf.cofactor(f, _var, false, true);
    p  = cf.cofactor(f, _var, true,  true);

    nx  = cf.remove_ys(n);
    ppx = cf.remove_ys(pp);
    npx = cf.remove_ys(np);
    px  = cf.remove_ys(p);

    ny  = cf.remove_xs(n);
    ppy = cf.remove_xs(pp);
    npy = cf.remove_xs(np);
    py  = cf.remove_xs(p);
  }

  void apply_gates(const BDD& lf, const BDD& rf)
  {
    left_f ^= lf;
    right_f ^= rf;

    BDD gate_left = cf.create_from_gate(_var, lf);
    BDD gate_right = cf.create_from_gate(_var, cf.move_ys_to_xs(rf));
    f = cf.compose(cf.compose(gate_left, f), gate_right);
    node_count += f.nodeCount();
  }

  void only_left_gate_shortcut()
  {
    BDD chi_prime = f;

    // Copy y_var to x_var
    chi_prime = chi_prime.ExistAbstract(cf.x(_var)) & cf.x(_var).Xnor(cf.y(_var));

    // Check whether chi' is reversible
    if (mpz_class(chi_prime.CountMinterm(2u * cf.num_vars())) == pow2(cf.num_vars())
        && cf.remove_ys(chi_prime) == cf.manager().bddOne()
        && cf.remove_xs(chi_prime) == cf.manager().bddOne())
    {
      BDD lf = cf.remove_ys(!f & chi_prime).ExistAbstract(cf.x(_var));
      apply_gates(lf, cf.manager().bddZero());
    }
  }

  void resolve_one_cycles()
  {
    // Via F_n
    compute_cofactors();

    //BDD lf = (!nx & ppx) ^ (!px & npx);
    BDD lf = ppx & npx;

    apply_gates(lf, cf.manager().bddZero());

    compute_cofactors();

    BDD rf = ppy & npy;

    apply_gates(cf.manager().bddZero(), rf);

    // // Via F_p
    // compute_cofactors();

    // lf = !px & npx;
    // rf = !py & ppy;

    // apply_gates(lf, rf);
  }

  void resolve_two_cycles()
  {
    // Via F_p
    compute_cofactors();

    BDD _f = ppy & p & npx;
    BDD lf = cf.remove_ys(_f);
    BDD rf = cf.remove_xs(_f);

    apply_gates(lf, rf);

    // Via F_n
    compute_cofactors();

    _f = npy & n & ppx;
    lf = cf.remove_ys(_f);
    rf = cf.remove_xs(_f);

    apply_gates(lf, rf);
  }

  void cycle_step()
  {
    compute_cofactors();

    std::vector<BDD> variables;
    boost::push_back(variables, cf.xs());
    boost::push_back(variables, cf.ys());

    // Pick an arbitrary cube from pp
    BDD cube;

    if ( pp != cf.manager().bddZero() )
    {
      cube = pp.PickOneMinterm(variables);
    }
    else
    {
      cube = ( !cf.remove_xs( f ) & !cf.remove_ys( f ) & cf.x( _var) & !cf.y( _var ) ).PickOneMinterm( variables );
      f |= cube;
    }
    char change = ChangeLeft;

    BDD lf = cf.manager().bddZero();
    BDD rf = cf.manager().bddZero();
    BDD cube_part;
    char *scube = new char[3u * cf.num_vars()];

    ++access;

    do
    {
      if (change == ChangeLeft)
      {
        // Update left gate
        cube_part = cf.remove_ys(cube).ExistAbstract(cf.x(_var));
        lf |= cube_part;
        //change = CHANGE_RIGHT;
        cube = !cf.x(_var) & cube_part & f;
      }
      else
      {
        cube_part = cf.remove_xs(cube).ExistAbstract(cf.y(_var));
        rf |= cube_part;
        //change = CHANGE_LEFT;
        cube = cf.y(_var) & cube_part & f;
      }

      /* Cube is not part of the function? */
      if (cube == cf.manager().bddZero())
      {
        BDD unused_outputs = !cf.remove_xs(f);
        BDD unused_inputs = !cf.remove_ys(f);

        BDD icube, ocube;

        if (change == ChangeLeft)
        {
          icube = !cf.x(_var) & cube_part;

          ocube = cf.manager().bddOne();
          if ((unused_outputs & cf.y(_var)) != cf.manager().bddZero())
          {
            (unused_outputs & cf.y(_var)).PickOneCube(scube);
          }
          else
          {
            unused_outputs.PickOneCube(scube);
          }
          for (unsigned i = 0u; i < cf.num_vars(); ++i)
          {
            ocube &= (scube[3u * i + 1u] == 1) ? cf.y(i) : !cf.y(i);
          }
        }
        else
        {
          ocube = cf.y(_var) & cube_part;

          icube = cf.manager().bddOne();
          if ((unused_inputs & !cf.x(_var)) != cf.manager().bddZero())
          {
            (unused_inputs & !cf.x(_var)).PickOneCube(scube);
          }
          else
          {
            unused_inputs.PickOneCube(scube);
          }
          for (unsigned i = 0u; i < cf.num_vars(); ++i)
          {
            icube &= (scube[3u * i] == 1) ? cf.x(i) : !cf.x(i);
          }
        }

        cube = icube & ocube;
        f |= cube;

        compute_cofactors();
      }

      change = ((char)1u - change);
      ++access;
    } while ( ( cube & ( pp | np ) ) == cf.manager().bddZero() );

    /* Cleanup */
    delete[] scube;

    apply_gates(lf, rf);
  }

  void resolve_k_cycles()
  {
    while (cf.cofactor(f, _var, true, false) != cf.manager().bddZero() || cf.cofactor(f, _var, false, true) != cf.manager().bddZero())
    {
      compute_cofactors();

      /* special case #P' is 0 and #N' is not zero, then swap */
      if ( pp == cf.manager().bddZero() && np != cf.manager().bddZero() )
      {
        apply_gates( cf.manager().bddOne(), cf.manager().bddOne() );
      }

      if ( false /*verbose*/ )
      {
        std::cout << "#N: " << cf.cofactor( f, _var, false, false ).CountMinterm( 2u * cf.num_vars() ) << " "
                  << "#P': " << cf.cofactor( f, _var, true, false ).CountMinterm( 2u * cf.num_vars() ) << " "
                  << "#N': " << cf.cofactor( f, _var, false, true ).CountMinterm( 2u * cf.num_vars() ) << " "
                  << "#P: " << cf.cofactor( f, _var, true, true ).CountMinterm( 2u * cf.num_vars() ) << std::endl;
      }

      cycle_step();
    }
  }

  typedef std::tuple<
    bool,                                                   // p1[_var] value
    std::function<BDD(rcbdd_synthesis_manager*)>,           // compute fc
    unsigned,                                               // scube offset
    std::function<BDD(rcbdd_synthesis_manager*, BDD, BDD)>, // update f
    std::function<void(circuit&, const circuit&)>           // update circ
    > resolve_cycles_configuration_t;

  /* DISABLE FOR NOW
  resolve_cycles_configuration_t resolve_x = std::make_tuple(
    true,
    []( rcbdd_synthesis_manager* manager ) {
      BDD fc = manager->ppx & manager->cf.move_xs_to_tmp( manager->npx );
      for ( unsigned j = 0u; j < manager->_var; ++j )
      {
        fc &= ( manager->cf.x( j ).Xnor( manager->cf.z( j ) ) );
      }
      return fc;
    },
    0u,
    []( rcbdd_synthesis_manager* manager, BDD gcirc, BDD f ) { return manager->cf.compose( gcirc, f ); },
    []( circuit& circ, const circuit& c ) { append_circuit( circ, c ); } );

  resolve_cycles_configuration_t resolve_y = std::make_tuple(
    false,
    []( rcbdd_synthesis_manager* manager ) {
      BDD fc = manager->ppy & manager->cf.move_ys_to_tmp( manager->npy );
      for ( unsigned j = 0u; j < manager->_var; ++j )
      {
        fc &= ( manager->cf.y( j ).Xnor( manager->cf.z( j ) ) );
      }
      return fc;
    },
    1u,
    []( rcbdd_synthesis_manager* manager, BDD gcirc, BDD f ) { return manager->cf.compose( f, gcirc ); },
    []( circuit& circ, const circuit& c ) { prepend_circuit( circ, c ); } );
  */

  void resolve_cycles_with_transpositions( const resolve_cycles_configuration_t& configuration )
  {
    if ( false /*verbose*/ )
    {
      std::cout << boost::format( "[i] resolve cycles for var %d" ) % _var << std::endl;
    }

    while ( cf.cofactor( f, _var, true, false ) != cf.manager().bddZero() ||
            cf.cofactor( f, _var, false, true ) != cf.manager().bddZero() )
    {
      compute_cofactors();

      /* Extract two cubes and save them into bitset */
      boost::dynamic_bitset<> p1( cf.num_vars() ), p2( cf.num_vars() );

      p1[_var] =  std::get<0>( configuration );
      p2[_var] = !std::get<0>( configuration );

      char *scube = new char[3u * cf.num_vars()];

      BDD fc = std::get<1>( configuration )( this );

      if ( !smart_pickcube )
      {
        fc.PickOneCube( scube );
      }
      else
      {
        pick_one_cube_for_rcbdd( fc, scube );
      }

      for ( unsigned i = 0u; i < cf.num_vars(); ++i )
      {
        if ( i == _var ) continue;
        p1[i] = scube[3u * i + std::get<2>( configuration )];
        p2[i] = scube[3u * i + 2u];
      }

      delete[] scube;

      /* Create circuit for p1 and p2 */
      if ( verbose )
      {
        //std::cout << "[i] realize transposition (" << p1 << " " << p2 << ")" << std::endl;
      }
      circuit c( cf.num_vars() );
      pattern_to_circuit( c, p1, p2 );
      if ( verbose )
      {
        //std::cout << "[i] result: " << std::endl << c;
      }

      /* Apply circuit to f */
      BDD gcirc = cf.create_from_circuit( c );
      f = std::get<3>( configuration )( this, gcirc, f );
      std::get<4>( configuration )( circ, c );
    }
  }

  void add_toffoli_gate( const cube_t& cube, unsigned offset )
  {
    gate::control_container controls;
    for ( unsigned i = 0u; i < cf.num_vars(); ++i )
    {
      if ( cube.second[3u * i + offset] )
      {
        controls += make_var( i, cube.first[3u * i + offset] );
      }
    }

    insert_toffoli( circ, insert_position, controls, _var );
    insert_position++;
  }

  void create_toffoli_gates_with_exorcism(const BDD& gate, unsigned var, unsigned offset, bool add_gates_to_circuit = true)
  {
    if (gate == cf.manager().bddZero()) return;

    if ( genesop )
    {
      std::ofstream esopout;
      esopout.open( boost::str( boost::format( "/tmp/%s_%d_%d.pla" ) % name % var % offset ) );
      esopout << ".i " << cf.num_vars() << std::endl;
      esopout << ".o " << 1 << std::endl;

      int *cube;
      CUDD_VALUE_TYPE value;
      DdGen *gen;

      Cudd_ForeachCube(gate.manager(), gate.getNode(), gen, cube, value)
      {
        char v;

        for (unsigned i = 0u; i < cf.num_vars(); ++i)
        {
          v = cube[3u * i + offset];
          if (v != 2)
          {
            esopout << ((v == 0) ? "0" : "1");
          }
          else
          {
            esopout << "-";
          }
        }

        esopout << " 1" << std::endl;
      }

      esopout << ".e" << std::endl;
      esopout.close();
    }

    if ( create_gates )
    {
      if ( add_gates_to_circuit )
      {
        esopmin.settings()->set( "on_cube", cube_function_t( [this, &offset]( const cube_t& c ) { add_toffoli_gate( c, offset ); } ) );
      }
      else
      {
        esopmin.settings()->set( "on_cube", cube_function_t( [this, &offset]( const cube_t& c ) {} ) );
      }
      esopmin.settings()->set( "verify", false );
      esopmin( gate.manager(), gate.getNode() );

      if ( add_gates_to_circuit && offset == 1u )
      {
        insert_position -= esopmin.statistics()->get<unsigned>( "cube_count" );
      }

      total_toffoli_gates += esopmin.statistics()->get<unsigned>( "cube_count" );
      total_control_lines += esopmin.statistics()->get<unsigned>( "literal_count" );
    }

    /*
    system("exorcism /tmp/test.esop");

    // Get number of gates
    total_toffoli_gates += boost::lexical_cast<unsigned long long>(execute_and_return_output("cat /tmp/test.esop | grep -v -e \"^[#\\.]\" | wc -l"));
    total_control_lines += boost::lexical_cast<unsigned long long>(execute_and_return_output("cat /tmp/test.esop | grep -v -e \"^[#\\.]\" | awk '{print $1}' | tr '\\n' ' ' | sed -e \"s/[^01]//g\" | wc -c"));

    // Get gates
    using boost::format;
    using boost::str;

    std::ifstream is;
    is.open("/tmp/test.esop");

    std::string line;
    while (std::getline(is, line)) {
      boost::trim(line);
      if (line.empty()) continue;

      if (line[0] == '0' || line[0] == '1' || line[0] == '-') {
        std::string negate, vars;
        unsigned c = 0u;
        for (unsigned i = 0u; i < mgr.num_vars(); ++i) {
          if (line[i] == '0') {
            negate += str(format("t1 x%d\n") % i);
          }
          if (line[i] == '0' || line[i] == '1') {
            vars += str(format(" x%d") % i);
            ++c;
          }
        }
        std::string _gate = str(format("%st%d%s x%d\n%s") % negate % (c + 1u) % vars % _var % negate);
        if (offset == 0u) {
          real_l += _gate;
        } else {
          real_r = _gate + real_r;
        }
      }
    }
    is.close();
    */
  }

  void default_synthesis()
  {
    null_stream ns;
    std::ostream null_out( &ns );
    boost::progress_display show_progress( cf.num_vars(), progress ? std::cout : null_out );

    for (unsigned var = 0; var < cf.num_vars(); ++var)
    {
      ++show_progress;
      set_var(var);

      if ( synthesis_method == ResolveCycles )
      {
        only_left_gate_shortcut();
        resolve_one_cycles();
        resolve_two_cycles();
        resolve_k_cycles();

        if ( false /*verbose*/ )
        {
          std::cout << "Target: " << _var << std::endl << " - left control function:" << std::endl;
          left_f.PrintMinterm();
          std::cout << " - right control function:" << std::endl;
          right_f.PrintMinterm();
        }

        create_toffoli_gates_with_exorcism(left_f, var, 0u);
        create_toffoli_gates_with_exorcism(right_f, var, 1u);
      }
      else if ( synthesis_method == TranspositionsX )
      {
        //resolve_cycles_with_transpositions( resolve_x );
      }
      else if ( synthesis_method == TranspositionsY )
      {
        //resolve_cycles_with_transpositions( resolve_y );
      }
    }
  }

  void heuristic_swap() //Get chi
  {
    std::vector<unsigned> list_lines;
    for (unsigned var = 0; var < cf.num_vars(); ++var)
    {
      list_lines.push_back(var);
    }

    BDD lf_c, lr_c;

    while (!list_lines.empty())
    {
      unsigned min_cost = UINT_MAX;
      unsigned best_line = 0u;

      for (unsigned i = 0u; i < list_lines.size(); ++i)
      {
        BDD oldchi = f; //make a copy of chi
        unsigned old_control_lines = total_control_lines;
        unsigned old_toffoli_gates = total_toffoli_gates;
        if ( false /*verbose*/ )
        {
          std::cout << "[I] - total_toffoli_gates" << total_toffoli_gates << std::endl;
        }
        set_var(list_lines[i]);

        if ( false /*verbose*/ )
        {
          std::cout << "[I] set_var(var): " << _var << std::endl;
        }
        only_left_gate_shortcut();
        resolve_one_cycles();
        resolve_two_cycles();
        resolve_k_cycles();

        if ( false /*verbose*/ )
        {
          std::cout << "Target: " << _var << std::endl << " - left control function:" << std::endl;
          left_f.PrintMinterm();
          std::cout << "[I] - right control function:" << std::endl;
          right_f.PrintMinterm();
        }

        create_toffoli_gates_with_exorcism( left_f, list_lines[i], 0u, false );
        create_toffoli_gates_with_exorcism( right_f, list_lines[i], 1u, false );

        // Determine cost and save in new_cost
        unsigned new_cost = total_toffoli_gates - old_toffoli_gates;

        if ( false /*verbose*/ )
        {
          std::cout << "[I] h1: Lines:    " << cf.num_vars() << std::endl;
          std::cout << "[I] h1: Gates:    " << new_cost << std::endl;
          std::cout << "[I] Controls:     " << total_control_lines << std::endl;
        }

        if (new_cost < min_cost)
        {
          best_line = list_lines[i];
          min_cost = new_cost;
          if ( false /*verbose*/ )
          {
            std::cout << "[I] Min cost: " << min_cost << std::endl;
          }
        }

        f = oldchi;
        total_toffoli_gates = old_toffoli_gates;
        total_control_lines = old_control_lines;
      }

      set_var(best_line);
      // Synthesis with best_line
      only_left_gate_shortcut();
      resolve_one_cycles();
      resolve_two_cycles();
      resolve_k_cycles();

      create_toffoli_gates_with_exorcism(left_f, best_line, 0u);
      create_toffoli_gates_with_exorcism(right_f, best_line, 1u);

      list_lines.erase(std::remove(list_lines.begin(),list_lines.end(),best_line));

      if ( false /*verbose*/ )
      {
        std::cout << "[I] Best Line: " << best_line << std::endl;
      }
    }
  }

  void heuristic_hamming()
  {
    std::vector<unsigned> list_lines;
    for (unsigned var = 0; var < cf.num_vars(); ++var)
    {
      list_lines.push_back(var);
    }

    BDD lf_c, lr_c;

    while (!list_lines.empty())
    {
      double min_cost = DBL_MAX;
      unsigned best_line = 0u;

      for (unsigned i = 0u; i < list_lines.size(); ++i)
      {
        // Determine costs and save in new_cost
        double new_cost = cf.cofactor(f, list_lines[i], false, true).CountMinterm(2 * cf.num_vars());

        if (new_cost < min_cost)
        {
          best_line = list_lines[i];
          min_cost = new_cost;
          if ( false /*verbose*/ )
          {
            std::cout << "[I] Min cost: " << min_cost << std::endl;
          }
        }
      }

      set_var(best_line);
      // Synthesis with best_line
      only_left_gate_shortcut();
      resolve_one_cycles();
      resolve_two_cycles();
      resolve_k_cycles();

      create_toffoli_gates_with_exorcism(left_f, best_line, 0u);
      create_toffoli_gates_with_exorcism(right_f, best_line, 1u);

      list_lines.erase(std::remove(list_lines.begin(),list_lines.end(),best_line));

      if ( false /*verbose*/ )
      {
        std::cout << "[I] Best Line: " << best_line << std::endl;
      }
    }
  }

  const rcbdd& cf;
  circuit& circ;

  bool verbose;
  bool progress;
  std::string name;
  bool genesop;
  dd_based_esop_optimization_func esopmin;
  bool create_gates;
  bool smart_pickcube;
  SynthesisMethod synthesis_method;

  BDD f;
  BDD left_f, right_f;
  unsigned _var;
  unsigned insert_position;
  BDD n, pp, np, p;
  BDD nx, ppx, npx, px;
  BDD ny,  ppy, npy, py;
  unsigned total_control_lines = 0u, total_toffoli_gates = 0u;
  unsigned long long access = 0ull;
  std::vector<int> node_count;
};

bool rcbdd_synthesis( circuit& circ, const rcbdd& cf, properties::ptr settings, properties::ptr statistics )
{
  /* Settings */
  auto verbose          = get( settings, "verbose",          false                             );
  auto progress         = get( settings, "progress",         false                             );
  auto name             = get( settings, "name",             std::string( "test" )             );
  auto genesop          = get( settings, "genesop",          false                             );
  auto esopmin          = get( settings, "esopmin",          dd_based_esop_optimization_func() );
  auto create_gates     = get( settings, "create_gates",     true                              );
  /* 0: default, 1: swap, 2: hamming */
  auto mode             = get( settings, "mode",             0u                                );
  auto synthesis_method = get( settings, "synthesis_method", ResolveCycles                     );
  auto smart_pickcube   = get( settings, "smart_pickcube",   true                              );

  /* Timing */
  properties_timer t( statistics );

  rcbdd_synthesis_manager mgr( cf, circ );
  mgr.verbose          = verbose;
  mgr.progress         = progress;
  mgr.name             = name;
  mgr.genesop          = genesop;
  mgr.esopmin          = esopmin;
  mgr.create_gates     = create_gates;
  mgr.synthesis_method = synthesis_method;
  mgr.smart_pickcube   = smart_pickcube;
  switch ( mode )
  {
  case 1u:
    mgr.heuristic_swap();
    break;
  case 2u:
    mgr.heuristic_hamming();
    break;
  default:
    mgr.default_synthesis();
  };

  if ( statistics )
  {
    statistics->set( "access", mgr.access );
    statistics->set( "node_count", mgr.node_count );
  }

  return true;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/*
 * Copyright (c) 2008-2009 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
 *
 * 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.
 *
 * 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 OWNER 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.
 *
 * $Id: sac_model_sphere.cpp 21050 2009-08-07 21:24:30Z jfaustwg $
 *
 */

/** \author Radu Bogdan Rusu */

#include <point_cloud_mapping/sample_consensus/sac_model_sphere.h>
#include <point_cloud_mapping/geometry/nearest.h>
#include <Eigen/LU>

#include <cminpack.h>

namespace sample_consensus
{
  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Get 4 random points (3 non-collinear) as data samples and return them as point indices.
    * \param iterations the internal number of iterations used by SAC methods
    * \param samples the resultant model samples
    * \note assumes unique points!
    * \note Two different points could be enough in theory, to infere some sort of a center and a radius,
    *       but in practice, we might end up with a lot of points which are just 'close' to one another.
    *       Therefore we have two options:
    *       a) use normal information (good but I wouldn't rely on it in extremely noisy point clouds, no matter what)
    *       b) get two more points and uniquely identify a sphere in space (3 unique points define a circle)
    */
  void
    SACModelSphere::getSamples (int &iterations, std::vector<int> &samples)
  {
    samples.resize (4);
    double trand = indices_.size () / (RAND_MAX + 1.0);

    // Get a random number between 1 and max_indices
    int idx = (int)(rand () * trand);
    // Get the index
    samples[0] = indices_.at (idx);

    // Get a second point which is different than the first
    do
    {
      idx = (int)(rand () * trand);
      samples[1] = indices_.at (idx);
      iterations++;
    } while (samples[1] == samples[0]);
    iterations--;

    double Dx1, Dy1, Dz1, Dx2, Dy2, Dz2, Dy1Dy2;
    // Compute the segment values (in 3d) between XY
    Dx1 = cloud_->points[samples[1]].x - cloud_->points[samples[0]].x;
    Dy1 = cloud_->points[samples[1]].y - cloud_->points[samples[0]].y;
    Dz1 = cloud_->points[samples[1]].z - cloud_->points[samples[0]].z;

    int iter = 0;
    do
    {
      // Get the third point, different from the first two
      do
      {
        idx = (int)(rand () * trand);
        samples[2] = indices_.at (idx);
        iterations++;
      } while ( (samples[2] == samples[1]) || (samples[2] == samples[0]) );
      iterations--;

      // Compute the segment values (in 3d) between XZ
      Dx2 = cloud_->points[samples[2]].x - cloud_->points[samples[0]].x;
      Dy2 = cloud_->points[samples[2]].y - cloud_->points[samples[0]].y;
      Dz2 = cloud_->points[samples[2]].z - cloud_->points[samples[0]].z;

      Dy1Dy2 = Dy1 / Dy2;
      iter++;

      if (iter > MAX_ITERATIONS_COLLINEAR )
      {
        ROS_WARN ("[SACModelSphere::getSamples] WARNING: Could not select 3 non collinear points in %d iterations!", MAX_ITERATIONS_COLLINEAR);
        break;
      }
      iterations++;
    }
    // Use Zoli's method for collinearity check
    while (((Dx1 / Dx2) == Dy1Dy2) && (Dy1Dy2 == (Dz1 / Dz2)));
    iterations--;

    // Need to improve this: we need 4 points, 3 non-collinear always, and the 4th should not be in the same plane as the other 3
    // otherwise we can encounter degenerate cases
    do
    {
      samples[3] = (int)(rand () * trand);
      iterations++;
    } while ( (samples[3] == samples[2]) || (samples[3] == samples[1]) || (samples[3] == samples[0]) );
    iterations--;

    return;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Select all the points which respect the given model coefficients as inliers.
    * \param model_coefficients the coefficients of a sphere model that we need to compute distances to
    * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers
    * \param inliers the resultant model inliers
    * \note: To get the refined inliers of a model, use:
    * ANNpoint refined_coeff = refitModel (...); selectWithinDistance (refined_coeff, threshold);
    */
  void
    SACModelSphere::selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers)
  {
    int nr_p = 0;
    inliers.resize (indices_.size ());

    // Iterate through the 3d points and calculate the distances from them to the sphere
    for (unsigned int i = 0; i < indices_.size (); i++)
    {
      // Calculate the distance from the point to the sphere as the difference between
      //dist(point,sphere_origin) and sphere_radius
      if (fabs (sqrt (
                      ( cloud_->points.at (indices_[i]).x - model_coefficients.at (0) ) *
                      ( cloud_->points.at (indices_[i]).x - model_coefficients.at (0) ) +

                      ( cloud_->points.at (indices_[i]).y - model_coefficients.at (1) ) *
                      ( cloud_->points.at (indices_[i]).y - model_coefficients.at (1) ) +

                      ( cloud_->points.at (indices_[i]).z - model_coefficients.at (2) ) *
                      ( cloud_->points.at (indices_[i]).z - model_coefficients.at (2) )
                     ) - model_coefficients.at (3)) < threshold)
      {
        // Returns the indices of the points whose distances are smaller than the threshold
        inliers[nr_p] = indices_[i];
        nr_p++;
      }
    }
    inliers.resize (nr_p);
    return;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Compute all distances from the cloud data to a given sphere model.
    * \param model_coefficients the coefficients of a sphere model that we need to compute distances to
    * \param distances the resultant estimated distances
    */
  void
    SACModelSphere::getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances)
  {
      distances.resize (indices_.size ());

    // Iterate through the 3d points and calculate the distances from them to the sphere
    for (unsigned int i = 0; i < indices_.size (); i++)
      // Calculate the distance from the point to the sphere as the difference between
      //dist(point,sphere_origin) and sphere_radius
      distances[i] = fabs (sqrt (
                                 ( cloud_->points.at (indices_[i]).x - model_coefficients.at (0) ) *
                                 ( cloud_->points.at (indices_[i]).x - model_coefficients.at (0) ) +

                                 ( cloud_->points.at (indices_[i]).y - model_coefficients.at (1) ) *
                                 ( cloud_->points.at (indices_[i]).y - model_coefficients.at (1) ) +

                                 ( cloud_->points.at (indices_[i]).z - model_coefficients.at (2) ) *
                                 ( cloud_->points.at (indices_[i]).z - model_coefficients.at (2) )
                                ) - model_coefficients.at (3));
    return;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Create a new point cloud with inliers projected onto the sphere model.
    * \param inliers the data inliers that we want to project on the sphere model
    * \param model_coefficients the coefficients of a sphere model
    * \param projected_points the resultant projected points
    * \todo implement this.
    */
  void
    SACModelSphere::projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients,
                                   sensor_msgs::PointCloud &projected_points)
  {
    std::cerr << "[SACModelSphere::projecPoints] Not implemented yet." << std::endl;
    projected_points = *cloud_;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Project inliers (in place) onto the given sphere model.
    * \param inliers the data inliers that we want to project on the sphere model
    * \param model_coefficients the coefficients of a sphere model
    * \todo implement this.
    */
  void
    SACModelSphere::projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients)
  {
    std::cerr << "[SACModelSphere::projecPointsInPlace] Not implemented yet." << std::endl;
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Check whether the given index samples can form a valid sphere model, compute the model coefficients from
    * these samples and store them internally in model_coefficients_. The sphere coefficients are: x, y, z, R.
    * \param samples the point indices found as possible good candidates for creating a valid model
    */
  bool
    SACModelSphere::computeModelCoefficients (const std::vector<int> &samples)
  {
    model_coefficients_.resize (4);

    Eigen::Matrix4d temp;
    for (int i = 0; i < 4; i++)
    {
      temp (i, 0) = cloud_->points.at (samples.at (i)).x;
      temp (i, 1) = cloud_->points.at (samples.at (i)).y;
      temp (i, 2) = cloud_->points.at (samples.at (i)).z;
      temp (i, 3) = 1;
    }
    double m11 = temp.determinant ();
    if (m11 == 0)
      return (false);             // the points don't define a sphere!

    for (int i = 0; i < 4; i++)
      temp (i, 0) = (cloud_->points.at (samples.at (i)).x) * (cloud_->points.at (samples.at (i)).x) +
                    (cloud_->points.at (samples.at (i)).y) * (cloud_->points.at (samples.at (i)).y) +
                    (cloud_->points.at (samples.at (i)).z) * (cloud_->points.at (samples.at (i)).z);
    double m12 = temp.determinant ();

    for (int i = 0; i < 4; i++)
    {
      temp (i, 1) = temp (i, 0);
      temp (i, 0) = cloud_->points.at (samples.at (i)).x;
    }
    double m13 = temp.determinant ();

    for (int i = 0; i < 4; i++)
    {
      temp (i, 2) = temp (i, 1);
      temp (i, 1) = cloud_->points.at (samples.at (i)).y;
    }
    double m14 = temp.determinant ();

    for (int i = 0; i < 4; i++)
    {
      temp (i, 0) = temp (i, 2);
      temp (i, 1) = cloud_->points.at (samples.at (i)).x;
      temp (i, 2) = cloud_->points.at (samples.at (i)).y;
      temp (i, 3) = cloud_->points.at (samples.at (i)).z;
    }
    double m15 = temp.determinant ();

    // Center (x , y, z)
    model_coefficients_[0] = 0.5 * m12 / m11;
    model_coefficients_[1] = 0.5 * m13 / m11;
    model_coefficients_[2] = 0.5 * m14 / m11;
    // Radius
    model_coefficients_[3] = sqrt (
                                   model_coefficients_[0] * model_coefficients_[0] +
                                   model_coefficients_[1] * model_coefficients_[1] +
                                   model_coefficients_[2] * model_coefficients_[2] - m15 / m11);

    return (true);
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Recompute the sphere coefficients using the given inlier set and return them to the user.
    * @note: these are the coefficients of the sphere model after refinement (eg. after SVD)
    * \param inliers the data inliers found as supporting the model
    * \param refit_coefficients the resultant recomputed coefficients after non-linear optimization
    */
  void
    SACModelSphere::refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients)
  {
    if (inliers.size () == 0)
    {
      ROS_ERROR ("[SACModelSphere::RefitModel] Cannot re-fit 0 inliers!");
      refit_coefficients = model_coefficients_;
      return;
    }
    if (model_coefficients_.size () == 0)
    {
      ROS_WARN ("[SACModelSphere::RefitModel] Initial model coefficients have not been estimated yet - proceeding without an initial solution!");
      best_inliers_ = indices_;
    }

    tmp_inliers_ = &inliers;

    int m = inliers.size ();

    double *fvec = new double[m];

    int n = 4;      // 4 unknowns
    int iwa[n];

    int lwa = m * n + 5 * n + m;
    double *wa = new double[lwa];

    // Set the initial solution
    double x[4] = {0.0, 0.0, 0.0, 0.0};
    if ((int)model_coefficients_.size () == n)
      for (int d = 0; d < n; d++)
        x[d] = model_coefficients_.at (d);

    // Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
    double tol = sqrt (dpmpar (1));

    // Optimize using forward-difference approximation LM
    int info = lmdif1 (&sample_consensus::SACModelSphere::functionToOptimize, this, m, n, x, fvec, tol, iwa, wa, lwa);

    // Compute the L2 norm of the residuals
    ROS_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g \nFinal solution: %g %g %g %g",
               info, enorm (m, fvec), model_coefficients_.at (0), model_coefficients_.at (1), model_coefficients_.at (2), model_coefficients_.at (3),
               x[0], x[1], x[2], x[3]);

    refit_coefficients.resize (n);
    for (int d = 0; d < n; d++)
      refit_coefficients[d] = x[d];

    free (wa); free (fvec);
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////(
  /** \brief Cost function to be minimized
    * \param p a pointer to our data structure array
    * \param m the number of functions
    * \param n the number of variables
    * \param x a pointer to the variables array
    * \param fvec a pointer to the resultant functions evaluations
    * \param iflag set to -1 inside the function to terminate execution
    */
  int
    SACModelSphere::functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag)
  {
    SACModelSphere *model = (SACModelSphere*)p;

    for (int i = 0; i < m; i ++)
    {
      // Compute the difference between the center of the sphere and the datapoint X_i
      double xt = model->cloud_->points[model->tmp_inliers_->at (i)].x - x[0];
      double yt = model->cloud_->points[model->tmp_inliers_->at (i)].y - x[1];
      double zt = model->cloud_->points[model->tmp_inliers_->at (i)].z - x[2];

      // g = sqrt ((x-a)^2 + (y-b)^2 + (z-c)^2) - R
      fvec[i] = sqrt (xt * xt + yt * yt + zt * zt) - x[3];
    }
    return (0);
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  /** \brief Verify whether a subset of indices verifies the internal sphere model coefficients.
    * \param indices the data indices that need to be tested against the sphere model
    * \param threshold a maximum admissible distance threshold for determining the inliers from the outliers
    */
  bool
    SACModelSphere::doSamplesVerifyModel (const std::set<int> &indices, double threshold)
  {
    for (std::set<int>::iterator it = indices.begin (); it != indices.end (); ++it)
      // Calculate the distance from the point to the sphere as the difference between
      //dist(point,sphere_origin) and sphere_radius
      if (fabs (sqrt (
                      ( cloud_->points.at (*it).x - model_coefficients_.at (0) ) *
                      ( cloud_->points.at (*it).x - model_coefficients_.at (0) ) +

                      ( cloud_->points.at (*it).y - model_coefficients_.at (1) ) *
                      ( cloud_->points.at (*it).y - model_coefficients_.at (1) ) +

                      ( cloud_->points.at (*it).z - model_coefficients_.at (2) ) *
                      ( cloud_->points.at (*it).z - model_coefficients_.at (2) )
                     ) - model_coefficients_.at (3)) > threshold)
        return (false);

    return (true);
  }
}


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
#include <mpi.h>
//#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#ifdef USE_INTRINSICS
#include "vector_x86.hpp"
#endif
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

};

namespace SphericalHarmonics {
    PS::F64 CoefficientY00 = 0.5 * sqrt(1.0 / M_PI);
    PS::F64 CoefficientY10 = 0.5 * sqrt(3.0 / M_PI);
    PS::F64 CoefficientY11 = 0.5 * sqrt(1.5 / M_PI);
    PS::F64 CoefficientY20 = sqrt( 5. / (16. * M_PI));
    PS::F64 CoefficientY22 = sqrt(15. / (32. * M_PI));

    bool    FirstTime         = true;
    PS::F64 MinimumRadius     = 1e+08;
    PS::F64 MaximumRadius     = 1e+11;
    PS::S64 NumberOfBinPerDex = 10;

    PS::S64 NumberOfBin = 0;
    const PS::S64 MaximumNumberOfBin = 64;
    static PS::F64 m00re[MaximumNumberOfBin];
    static PS::F64 m10re[MaximumNumberOfBin];
    static PS::F64 m11re[MaximumNumberOfBin];
    static PS::F64 m11im[MaximumNumberOfBin];
    static PS::F64 m20re[MaximumNumberOfBin];
    static PS::F64 m22re[MaximumNumberOfBin];
    static PS::F64 m22im[MaximumNumberOfBin];
    static PS::S64 nptcl[MaximumNumberOfBin];
    static PS::F64 m00re_g[MaximumNumberOfBin];
    static PS::F64 m10re_g[MaximumNumberOfBin];
    static PS::F64 m11re_g[MaximumNumberOfBin];
    static PS::F64 m11im_g[MaximumNumberOfBin];
    static PS::F64 m20re_g[MaximumNumberOfBin];
    static PS::F64 m22re_g[MaximumNumberOfBin];
    static PS::F64 m22im_g[MaximumNumberOfBin];
    static PS::S64 nptcl_g[MaximumNumberOfBin];

    void initialize() {
        FirstTime = false;
        PS::F64 temp = log10(MaximumRadius / MinimumRadius) * NumberOfBinPerDex;
        NumberOfBin = (temp - (PS::S64)temp == 0.) ? (PS::S64)temp : (PS::S64)(temp + 1.);
        assert(NumberOfBin <= MaximumNumberOfBin);
        for(PS::S64 i = 0; i < NumberOfBin; i++) {
            m00re[i] = 0.;
            m10re[i] = 0.;
            m11re[i] = 0.;
            m11im[i] = 0.;
            m20re[i] = 0.;
            m22re[i] = 0.;
            m22im[i] = 0.;
            nptcl[i] = 0;
            m00re_g[i] = 0.;
            m10re_g[i] = 0.;
            m11re_g[i] = 0.;
            m11im_g[i] = 0.;
            nptcl_g[i] = 0.;
        }
    }

    void reinitialize() {
        for(PS::S64 i = 0; i < NumberOfBin; i++) {
            m00re[i] = 0.;
            m10re[i] = 0.;
            m11re[i] = 0.;
            m11im[i] = 0.;
            m20re[i] = 0.;
            m22re[i] = 0.;
            m22im[i] = 0.;
            nptcl[i] = 0;
            m00re_g[i] = 0.;
            m10re_g[i] = 0.;
            m11re_g[i] = 0.;
            m11im_g[i] = 0.;
            nptcl_g[i] = 0.;
        }
    }

    template <class Tsph>
    inline void addParticle(Tsph & isph) {
        PS::F64 r2 = isph.pos * isph.pos;
        if(r2 >= MaximumRadius * MaximumRadius) {
            return;
        }
        PS::F64 r1 = sqrt(r2);
        PS::S64 ibin = PS::S64(log10(r1 / MinimumRadius) * NumberOfBinPerDex);
        assert(ibin < NumberOfBin);

        PS::F64 cr = sqrt(isph.pos[0] * isph.pos[0] + isph.pos[1] * isph.pos[1]);
        PS::F64 prinv = 1. / r1;
        PS::F64 crinv = 1. / cr;
        PS::F64 costheta = isph.pos[2] * prinv;
        PS::F64 sintheta = cr          * prinv;
        PS::F64 cosphi   = isph.pos[0] * crinv;
        PS::F64 sinphi   = isph.pos[1] * crinv;

        PS::F64 costheta_sq   = costheta * costheta;
        PS::F64 sintheta_sq   = sintheta * sintheta;
        PS::F64 cosphi_2times = cosphi * cosphi - sinphi * sinphi;
        PS::F64 sinphi_2times = 2. * sinphi * cosphi;

        m00re[ibin] += isph.mass * CoefficientY00;
        m10re[ibin] += isph.mass * CoefficientY10 * costheta;
        m11re[ibin] += isph.mass * CoefficientY11 * sintheta * cosphi;
        m11im[ibin] += isph.mass * CoefficientY11 * sintheta * sinphi;
        m20re[ibin] += isph.mass * CoefficientY20 * (3. * costheta_sq - 1.);
        m22re[ibin] += isph.mass * CoefficientY22 * sintheta_sq * cosphi_2times;
        m22im[ibin] += isph.mass * CoefficientY22 * sintheta_sq * sinphi_2times;
        nptcl[ibin] += 1;
    }

    void reduce() {
        PS::S64 ierr = 0;
        ierr = MPI_Allreduce(m00re, m00re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m10re, m10re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m11re, m11re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m11im, m11im_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m20re, m20re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m22re, m22re_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(m22im, m22im_g, NumberOfBin, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
        ierr = MPI_Allreduce(nptcl, nptcl_g, NumberOfBin, MPI_LONG_LONG, MPI_SUM, MPI_COMM_WORLD);
    }

    void output(FILE * fp) {
        for(PS::S64 i = 0; i < NumberOfBin; i++) {
            PS::F64 r = (i == 0) ? (0.5 * MinimumRadius)
                : (MinimumRadius * pow(10., (i - 0.5) / NumberOfBinPerDex));
            fprintf(fp, "%+e %+e %+e %+e %+e %+e %+e %+e %8d\n",
                    r,
                    m00re_g[i], m10re_g[i], m11re_g[i], m11im_g[i],
                    m20re_g[i], m22re_g[i], m22im_g[i],
                    nptcl_g[i]);
        }
    }

};

template <class Tsph>
void calcDensityCenter(Tsph  & sph,
                       PS::F64vec & xdens_g,
                       PS::F64vec & vdens_g,
                       PS::F64vec & adens_g) {
    PS::F64    tdens = 0.;
    PS::F64vec xdens = 0.;
    PS::F64vec vdens = 0.;
    PS::F64vec adens = 0.;

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        if(sph[i].istar == 1) {
            continue;
        }
        tdens += sph[i].dens;
        xdens += sph[i].dens * sph[i].pos;
        vdens += sph[i].dens * sph[i].vel;
        adens += sph[i].dens * sph[i].acc;
    }

    PS::F64    tdens_g = PS::Comm::getSum(tdens);
    xdens_g = PS::Comm::getSum(xdens);
    vdens_g = PS::Comm::getSum(vdens);
    adens_g = PS::Comm::getSum(adens);

    xdens_g = xdens_g / tdens_g;
    vdens_g = vdens_g / tdens_g;
    adens_g = adens_g / tdens_g;

    return;
}

template <class Tsph>
void expandInSphericalHarmonics(char * ofile,
                                Tsph  & sph) {
    PS::F64vec xdens = 0.;
    PS::F64vec vdens = 0.;
    PS::F64vec adens = 0.;
    calcDensityCenter(sph, xdens, vdens, adens);
    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        sph[i].pos -= xdens;
        sph[i].vel -= vdens;
        sph[i].acc -= adens;
    }

    if(SphericalHarmonics::FirstTime) {
        SphericalHarmonics::initialize();
    } else {
        SphericalHarmonics::reinitialize();
    }

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        SphericalHarmonics::addParticle(sph[i]);
    }

    SphericalHarmonics::reduce();

    if(PS::Comm::getRank() == 0) {
        FILE * fp = fopen(ofile, "w");
        SphericalHarmonics::output(fp);
        fclose(fp);
    }

}

int main(int argc, char ** argv) {
    PS::Initialize(argc, argv);

    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);

    char idir[1024], odir[1024];
    PS::S64 ibgn, iend, dsnp;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", odir);
    fscanf(fp, "%lld%lld%lld", &ibgn, &iend, &dsnp);
    fclose(fp);

    for(PS::S64 itime = ibgn; itime <= iend; itime += dsnp) {
        char tfile[1024];
        FILE *fp = NULL;
        PS::S64 tdir = 0;
        for(PS::S64 iidir = 0; iidir < 100; iidir++) {
            sprintf(tfile, "%s/t%02d/sph_t%04d_p%06d_i%06d.dat", idir, iidir, itime,
                    PS::Comm::getNumberOfProc(), 0);
            fp = fopen(tfile, "r");
            if(fp != NULL) {
                tdir = iidir;
                break;
            }
        }
        if(fp == NULL) {
            if(PS::Comm::getRank() == 0) {
                fprintf(stderr, "Not found %s\n", tfile);
            }
            continue;
        }
        fclose(fp);

        char sfile[1024];
        sprintf(sfile, "%s/t%02d/sph_t%04d", idir, tdir, itime);
        sph.readParticleAscii(sfile, "%s_p%06d_i%06d.dat");

        char ofile[1024];
        sprintf(ofile, "%s/harmonics_t%04d.dat", odir, itime);
        expandInSphericalHarmonics(ofile, sph);
    }

    PS::Finalize();

    return 0;
}