/*
 * Copyright 2009-2019 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "../../../include/votca/csg/potentialfunctions/potentialfunctioncbspl.h"
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <votca/tools/table.h>

using namespace std;
using namespace votca::tools;

namespace votca {
namespace csg {

PotentialFunctionCBSPL::PotentialFunctionCBSPL(const string &name, Index nlam,
                                               double min, double max)
    : PotentialFunction(name, nlam, min, max) {

  /* Here nlam_ is the total number of coeff values that are to be optimized
   * To ensure that potential and force go to zero smoothly near cut-off,
   * as suggested in Ref. PCCP, 11, 1901, 2009, coeff values leading up to
   * cut-off and beyond take a value of zero.
   *
   * Since region less than rmin is not sampled sufficiently for stability
   * first  nexcl_ coefficients are not optimized instead their values are
   * extrapolated from first statistically significant knot values near rmin
   */

  Index nknots;

  nknots = lam_.size();

  nbreak_ = nknots - 2;

  dr_ = (cut_off_) / (double(nbreak_ - 1));

  // break point locations
  // since ncoeff = nbreak +2 , r values for last two coefficients are also
  // computed
  rbreak_ = Eigen::VectorXd::Zero(nknots);

  for (Index i = 0; i < nknots; i++) {
    rbreak_(i) = double(i) * dr_;
  }

  // exclude knots corresponding to r <=  min_
  nexcl_ = std::min((Index)(min_ / dr_), nbreak_ - 2) + 1;

  // account for finite numerical division of  min_/ dr_
  // e.g. 0.24/0.02 may result in 11.99999999999999
  if (rbreak_(nexcl_) == min_) {
    nexcl_++;
  }

  // fixing last 4 knots to zeros is reasonable
  ncutcoeff_ = 4;

  // check if we have enough parameters to optimize
  if ((Index(lam_.size()) - nexcl_ - ncutcoeff_) < 1) {
    throw std::runtime_error(
        "In potential " + name_ +
        ": no parameters to optimize!\n"
        "All the knot values fall in the range of either excluded (due to high "
        "repulsive region) or cut-off region.\n"
        "This issue can be resolved by one or combination of following steps:\n"
        "1. Make sure you are using large-enough cut-off for this CG "
        "potential.\n"
        "2. Make sure the CG-MD runs are sufficiently Index and CG-MD RDF are "
        "statistically reliable.\n"
        "3. Use more knot values.\n");
  }

  M_ = Eigen::MatrixXd::Zero(4, 4);
  M_(0, 0) = 1.0;
  M_(0, 1) = 4.0;
  M_(0, 2) = 1.0;
  M_(0, 3) = 0.0;
  M_(1, 0) = -3.0;
  M_(1, 1) = 0.0;
  M_(1, 2) = 3.0;
  M_(1, 3) = 0.0;
  M_(2, 0) = 3.0;
  M_(2, 1) = -6.0;
  M_(2, 2) = 3.0;
  M_(2, 3) = 0.0;
  M_(3, 0) = -1.0;
  M_(3, 1) = 3.0;
  M_(3, 2) = -3.0;
  M_(3, 3) = 1.0;
  M_ /= 6.0;
}

Index PotentialFunctionCBSPL::getOptParamSize() const {

  return lam_.size() - nexcl_ - ncutcoeff_;
}

void PotentialFunctionCBSPL::setParam(string filename) {

  Table param;
  param.Load(filename);
  lam_.setZero();

  if (param.size() != lam_.size()) {

    throw std::runtime_error("In potential " + name_ +
                             ": parameters size mismatch!\n"
                             "Check input parameter file \"" +
                             filename + "\" \nThere should be " +
                             boost::lexical_cast<string>(lam_.size()) +
                             " parameters");
  } else {
    // force last  ncutcoeff_ to zero
    Index nonzero = lam_.size() - ncutcoeff_;
    lam_.head(nonzero) = param.y().head(nonzero);
  }
}

void PotentialFunctionCBSPL::SaveParam(const string &filename) {

  extrapolExclParam();

  Table param;
  param.SetHasYErr(false);
  param.resize(lam_.size());

  // write extrapolated knots with flag 'o'
  // points close to rmin can also be stastically not reliable
  // so flag 3 more points next to rmin as 'o'
  for (Index i = 0; i < nexcl_ + 3; i++) {
    param.set(i, rbreak_(i), lam_(i), 'o');
  }

  for (Index i = nexcl_ + 3; i < lam_.size(); i++) {
    param.set(i, rbreak_(i), lam_(i), 'i');
  }

  param.Save(filename);
}

void PotentialFunctionCBSPL::SavePotTab(const string &filename, double step,
                                        double rmin, double rcut) {
  extrapolExclParam();
  PotentialFunction::SavePotTab(filename, step, rmin, rcut);
}

void PotentialFunctionCBSPL::SavePotTab(const string &filename, double step) {
  extrapolExclParam();
  PotentialFunction::SavePotTab(filename, step);
}

void PotentialFunctionCBSPL::extrapolExclParam() {

  double u0 = lam_(nexcl_);
  double m = (lam_(nexcl_ + 1) - lam_(nexcl_)) /
             (rbreak_(nexcl_ + 1) - rbreak_(nexcl_));
  double r0 = rbreak_(nexcl_);

  /* If the slope m is positive then the potential core
   * will be attractive. So, artificially forcing core to be
   * repulsive by setting m = -m
   */
  if (m > 0) {
    cout << name_ << " potential's extrapolated core is attractive!" << endl;
    cout << "Artifically enforcing repulsive core.\n" << endl;
    m *= -1.0;
  }
  // using linear extrapolation
  // u(r) = ar + b
  // a = m
  // b = - m*r0 + u0
  // m = (u1-u0)/(r1-r0)

  double a = m;
  double b = -1.0 * m * r0 + u0;
  for (Index i = 0; i < nexcl_; i++) {
    lam_(i) = a * rbreak_(i) + b;
  }
}

void PotentialFunctionCBSPL::setOptParam(Index i, double val) {

  lam_(i + nexcl_) = val;
}

double PotentialFunctionCBSPL::getOptParam(Index i) const {

  return lam_(i + nexcl_);
}

double PotentialFunctionCBSPL::CalculateF(double r) const {

  if (r <= cut_off_) {

    double u = 0.0;
    Index indx = std::min((Index)(r / dr_), nbreak_ - 2);
    double rk = (double)indx * dr_;
    double t = (r - rk) / dr_;

    Eigen::Vector4d R = Eigen::Vector4d::Zero();
    R(0) = 1.0;
    R(1) = t;
    R(2) = t * t;
    R(3) = t * t * t;
    Eigen::Vector4d B = lam_.segment<4>(indx);
    u += ((R.transpose() * M_) * B).value();
    return u;

  } else {
    return 0.0;
  }
}

// calculate first derivative w.r.t. ith parameter
double PotentialFunctionCBSPL::CalculateDF(Index i, double r) const {

  // since first  nexcl_ parameters are not optimized for stability reasons

  if (r <= cut_off_) {

    Index i_opt = i + nexcl_;
    Index indx;
    double rk;

    indx = std::min((Index)(r / dr_), nbreak_ - 2);
    rk = (double)indx * dr_;

    if (i_opt >= indx && i_opt <= indx + 3) {

      Eigen::Vector4d R = Eigen::Vector4d::Zero();

      double t = (r - rk) / dr_;

      R(0) = 1.0;
      R(1) = t;
      R(2) = t * t;
      R(3) = t * t * t;

      Eigen::Vector4d RM = R.transpose() * M_;

      return RM(i_opt - indx);

    } else {
      return 0.0;
    }

  } else {
    return 0.0;
  }
}

// calculate second derivative w.r.t. ith parameter
double PotentialFunctionCBSPL::CalculateD2F(Index, Index, double) const {

  return 0.0;
}

}  // namespace csg
}  // namespace votca


#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "sensor_msgs/Joy.h"
#include "create_driver/vicon_driver.h"
#include "geometry_msgs/Twist.h"

#include "create_controller/ControlMsgs.h"

#include <string>
#include <vector>
#include <map>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>

#define PI 3.141592
using namespace Eigen;
using namespace std;
Vector3d position(-5,-5,-5);

void position_callback(const geometry_msgs::Vector3::ConstPtr& msg)
{
	position(0) = msg->x;
	position(1) = msg->y;
	position(2) = msg->z;
}

int main(int argc, char **argv)
{
	// ROS Initalization
	ros::init(argc, argv, "falcon_ellipse_feedback");

	ros::NodeHandle n;
	ros::NodeHandle private_n("~");

	// List of robot names
	vector<string> robot_names;
	XmlRpc::XmlRpcValue robot_list;
	private_n.getParam("robot_list", robot_list);
	ROS_ASSERT(robot_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < robot_list.size(); i++)
	{
		ROS_ASSERT(robot_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
		robot_names.push_back(static_cast<string>(robot_list[i]));
	}

	// Number of robots
	const int num_robots = robot_names.size();

	// ROS Subscribers
	map<string, create_driver::ViconStream> vicon;
	vector<string>::iterator name_it;
	vector<ros::Subscriber> sub;
	for (name_it = robot_names.begin(); name_it != robot_names.end(); name_it++)
	{
		vicon.insert(pair<string, create_driver::ViconStream>(*name_it, create_driver::ViconStream()));
		sub.push_back(n.subscribe("/" + *name_it + "/tf", 10, &create_driver::ViconStream::callback, &vicon[*name_it]));
	}

	sub.push_back(n.subscribe("position", 10, position_callback));

	ros::Publisher joy_pub = n.advertise<sensor_msgs::Joy>("joy", 10);
	ros::Publisher force_pub = n.advertise<geometry_msgs::Vector3>("force", 10);

	// ROS loop
	ros::Rate loop_rate(1000); // 1 kHz
	bool uninitialized = true;
	while (ros::ok())
	{
		ros::spinOnce();

		Vector3d joy(position(0)/0.06,  position(1)/0.06, (2.0*(position(2)-0.073))/0.1035 - 1.0);

		Vector3d force;
		double resistance;

		double p[num_robots];
		double q[num_robots];

		for (int i = 0; i < num_robots; i++) {
			p[i] = cos(vicon[robot_names[i]].theta());
			q[i] = sin(vicon[robot_names[i]].theta());
		}

		double p_bar = 0.0;
		double q_bar = 0.0;

		for (int i = 0; i < num_robots; i++) {
			p_bar += p[i]/num_robots;
			q_bar += q[i]/num_robots;
		}

		double r = sqrt(pow(p_bar, 2) + pow(q_bar, 2))/2.0;

		double phi = atan2(q_bar, p_bar);

		double b = 0.2;

		double a = sqrt(pow(r, 2) + pow(b, 2));

		double x = joy(0) - r*cos(phi);
		double y = joy(1) - r*sin(phi);

		double x_barE = x * cos(phi) + y*sin(phi);
		double y_barE = -x * sin(phi) + y*cos(phi);

		double c = pow(x_barE,2) / pow(a,2) + pow(y_barE,2) / pow(b,2);

		double lowerbound = 1.0;
		double upperbound = 25.0;
		double gain = 0.0;
		if(lowerbound < c && c < upperbound){
			gain = (-500)*(pow(upperbound,2) - pow(lowerbound,2) )*(pow(c,2) - pow(lowerbound,2) ) / pow((pow(c,2) - pow(upperbound,2) ),3);

		}

		double v_bar1 = -2 * x_barE/pow(a,2);
		double v_bar2 = -2 * y_barE/pow(b,2);

		double v_bar_norm = sqrt(pow(v_bar1, 2) + pow(v_bar2, 2));

		Vector3d v_bar(v_bar1*cos(phi)-v_bar2*sin(phi), v_bar1*sin(phi)+v_bar2*cos(phi), 0);
		force = gain*v_bar_norm*v_bar;
		// if (joy.norm()<0.25) {
		// 	resistance =1;
		// } else {
		// 	double radius = joy.norm() - 0.25;

		// 	double sum_etheta = fabs(etheta[0])+ fabs(etheta[1])+fabs(etheta[2])+fabs(etheta[3]);
		// 	resistance = 1; // + 6 * (radius * sum_etheta);
		// 	printf("%f\n", sum_etheta);
		// }

		// for (int i = 0; i < 3; i++)
		// {
		// 	force(i) = -5*joy(i)*resistance;
		// }

		geometry_msgs::Vector3 fmsg;
		fmsg.x = force(0);
		fmsg.y = force(1);
		fmsg.z = -joy(2)*5;
		force_pub.publish(fmsg);

		// Deadband
		if (joy.norm() < 0.25) {
			for (int i = 0; i < 2; ++i)
			{
				joy(i) =0;
			}
		}

		sensor_msgs::Joy jmsg;
		jmsg.axes.resize(3);
		jmsg.axes[0] = -joy(0);//position(0)/0.06; // -1.0 to 1.0
		jmsg.axes[1] = joy(1);//position(1)/0.06;
		jmsg.axes[2] = joy(2);//(2.0*(position(2)-0.073))/0.1035 - 1.0;//32767
		joy_pub.publish(jmsg);

		loop_rate.sleep();
	}

	return 0;
}


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#define _USE_MATH_DEFINES
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include "specialtypes.hpp"
#include "ublas_cholesky.hpp"
#include "posterior_mcmc.hpp"

namespace bnu = boost::numeric::ublas;

int determinant_sign(const bnu::permutation_matrix<std::size_t>& pm)
{
  int pm_sign=1;
  std::size_t size = pm.size();
  for (std::size_t i = 0; i < size; ++i)
    if (i != pm(i))
      pm_sign *= -1.0; // swap_rows would swap a pair of rows here, so we change sign
  return pm_sign;
}

double determinant(bnu::matrix<double>& m ) {
  bnu::permutation_matrix<std::size_t> pm(m.size1());
  double det = 1.0;
  if( bnu::lu_factorize(m,pm) ) {
    det = 0.0;
  } else {
    for(int i = 0; i < m.size1(); i++)
      det *= m(i,i); // multiply by elements on diagonal
    det = det * determinant_sign( pm );
  }
  return det;
}

double gauss(const vectord& x, const vectord& mu, const matrixd& sigma)
{
  const double tpi = boost::math::constants::two_pi<double>();
  double n = static_cast<double>(x.size());
  const vectord vd = x-mu;
  matrixd invS = sigma;
  bayesopt::utils::inverse_cholesky(sigma,invS);
  matrixd sig = sigma;

  return pow(tpi,n/2)*pow(determinant(sig),0.5)*exp(-0.5*inner_prod(vd,prod(invS,vd)));
}

class Posterior: public bayesopt::RBOptimizable
{
  double evaluate(const vectord& x)
  {
    vectord mu1(2), mu2(2), mu3(2);
    matrixd s1(2,2), s2(2,2), s3(2,2);

    mu1 <<= 0,0;
    mu2 <<= 1,1;
    mu3 <<= -4,2;

    s1 <<= 1,0,
      0,1;

    s2 <<= 4,0,
      0,0.6;

    s3 <<= 4,0,
      0,0.6;

    return gauss(x,mu1,s1) + gauss(x,mu2,s2) + gauss(x,mu3,s3);
  }
};

int main()
{
  randEngine reng;
  Posterior post;
  bayesopt::MCMCSampler sampler(&post,2,reng);
  vectord x = zvectord(2);
  sampler.run(x);
  sampler.printParticles();

  return 0;
}


/**
 * @file gravity_filter.hpp
 * @author Fujii Naomichi
 * @copyright (c) 2021 Fujii Naomichi
 * SPDX-License-Identifier: MIT
 */

#pragma once

#include <math.h>
#include <fpu.hpp>
#include <Eigen/Core>
#include "board.hpp"

/**
 * @brief IMUの測定値から重力の影響を取り除くフィルタ
 */
class GravityFilter {
public:
    /**
     * @brief 内部状態をリセットする
     */
    void reset(void) {
        _initialized = false;
    }

    /**
     * @brief フィルタに新たな入力を与えて出力を更新する
     * @param accel 加速度センサーの測定値
     * @param gyro ジャイロスコープの測定値
     */
    void update(const Eigen::Vector3f& accel, const Eigen::Vector3f& gyro) {
        using namespace Eigen;

        static constexpr float GYRO_GAIN_P = 1.0;
        static constexpr float GYRO_GAIN_I = 0.001;
        static constexpr float GRAVITY_LOW_THRESHOLD = 0.0625f;
        static constexpr float GRAVITY_COMPENSATION = 0.001f;

        if (!_initialized) {
            // 重力加速度ベクトルを初期化する
            _initialized = true;
            _gravity = accel;
            _gyro_error_integ = Vector3f::Zero();
        }

        // 加速度ベクトルと重力加速度ベクトルの成す角度を求める
        Vector3f gyro_error;
        float gravity_scale = fpu::sqrt(_gravity.squaredNorm());
        if (GRAVITY_LOW_THRESHOLD < gravity_scale) {
            gyro_error = accel.cross(_gravity) / (gravity_scale * gravity_scale);
            _gyro_error_integ += gyro_error;
        }
        else {
            // 重力が異様に小さいときは加速度センサーによる角速度の補正を減らす
            gyro_error = Vector3f::Zero();
            _gyro_error_integ *= fpu::max(1.0f - 1.0f / IMU_OUTPUT_RATE, 0.0f);
        }

        // 角速度を加速度センサーから得た角度誤差で補正する
        Vector3f delta_omega = GYRO_GAIN_P * gyro_error + GYRO_GAIN_I * _gyro_error_integ;
        _compensated_gyro = gyro + delta_omega;

        // 重力ベクトルを回転する
        Matrix3f Rt = rotationMatrixTransposed(_compensated_gyro * (1.0f / IMU_OUTPUT_RATE));
        _gravity = Rt * _gravity;

        // 重力加速度ベクトルの大きさを徐々に加速度の大きさに近づける
        // 重力が小さいときは大きさではなくベクトルそのものを使って補正する
        float accel_scale = fpu::sqrt(accel.squaredNorm());
        if (GRAVITY_LOW_THRESHOLD < fpu::min(accel_scale, gravity_scale)) {
            _gravity *= ((1.0f - GRAVITY_COMPENSATION) + GRAVITY_COMPENSATION * accel_scale / gravity_scale);
        }
        else {
            _gravity = (1.0f - GRAVITY_COMPENSATION) * _gravity + GRAVITY_COMPENSATION * accel;
        }

        // 加速度ベクトルから重力の影響を除去する
        _compensated_accel = accel - _gravity;
    }

    /**
     * @brief 重力加速度ベクトル(の反力)を取得する
     * @return 重力加速度ベクトル(の反力) X, Y, Z [m/s^2]
     */
    const Eigen::Vector3f& gravity(void) const {
        return _gravity;
    }

    /**
     * @brief 重力を除去済みの加速度を取得する
     * @return 加速度 X, Y, Z [m/s^2]
     */
    const Eigen::Vector3f& acceleration(void) const {
        return _compensated_accel;
    }

    /**
     * @brief 重力で補正済みの角速度を取得する
     * @return 角速度 X, Y, Z [rad/s]
     */
    const Eigen::Vector3f& angularVelocity(void) const {
        return _compensated_gyro;
    }

private:
    static Eigen::Matrix3f rotationMatrixTransposed(const Eigen::Vector3f& gyro) {
        float z = gyro.z();
        float y = gyro.y();
        float x = gyro.x();
        return Eigen::Matrix3f{
            {1.0f, x * y + z, x * z - y},
            {-z, 1.0f - x * y * z, x + y * z},
            {y, -x, 1.0f},
        };
    }

    bool _initialized = false;
    Eigen::Vector3f _gravity;
    Eigen::Vector3f _compensated_accel;
    Eigen::Vector3f _compensated_gyro;
    Eigen::Vector3f _gyro_error_integ;
};


/**
  This file creates a one-layer neural network to calculate the beat from 16
  inputs.

  We want range of tempo: 35 - 250 bpm //TODO
*/
#include "exception.hh"
#include "network.hh"
#include "timer.hh"

#include <Eigen/Dense>
#include <iostream>
#include <random>
#include <utility>

#include <sys/resource.h>
#include <sys/time.h>

using namespace std;
using namespace Eigen;

constexpr size_t batch_size = 1;
constexpr size_t input_size = 16;

/* use squared error as loss function */
float loss_function( const float target, const float actual )
{
  return ( target - actual ) * ( target - actual );
}

/* partial derivative of loss with respect to neural network output */
float compute_pd_loss_wrt_output( const float target, const float actual )
{
  return -2 * ( target - actual );
}

/* compute input */
Matrix<float, batch_size, input_size> gen_time( float tempo, float offset )
{
  Matrix<float, batch_size, input_size> ret_mat;
  for ( auto i = 0; i < 16; i++ ) {
    ret_mat( i ) = tempo * i + offset;
  }
  return ret_mat;
}

float learning_rate = 0.00001;

void program_body()
{
  /* remove limit on stack size */
  const rlimit limits { RLIM_INFINITY, RLIM_INFINITY };
  CheckSystemCall( "setrlimit", setrlimit( RLIMIT_STACK, &limits ) );

  /* seed C RNG for Eigen random weight initialization */
  srand( Timer::timestamp_ns() );

  /* construct neural network on heap */
  auto nn = make_unique<Network<float, batch_size, input_size, 1>>();
  nn->layer0.initializeWeightsRandomly();

  int tempo = 50;
  float offset = 0;
  for ( tempo = 70; tempo > 50; tempo-- ) {
    /* test true function */
    int i = 0;
    while ( true ) {
      if ( i == 5 )
        break;
      i += 1;
      /* step 1: construct a unique problem instance */
      Matrix<float, batch_size, input_size> input = gen_time( tempo, offset );

      /* step 2: forward propagate and calculate loss functiom */
      nn->apply( input );
      cout << "nn maps input: tempo: " << tempo << " offset " << offset << " => " << nn->output()( 0, 0 ) << endl;

      /* step 3: backpropagate error */
      nn->computeDeltas();
      nn->evaluateGradients( input );

      const float pd_loss_wrt_output = compute_pd_loss_wrt_output( tempo, nn->output()( 0, 0 ) );
      cout << "Original loss: " << pd_loss_wrt_output << "\n";

      // TODO: static eta -> dynamic eta
      auto four_third_lr = 4.0 / 3 * learning_rate;
      auto two_third_lr = 2.0 / 3 * learning_rate;

      /* calculate three loss */
      float current_loss = loss_function( nn->output()( 0, 0 ), tempo );
      Matrix<float, input_size, 1> current_weights;
      for ( int j = 0; j < 16; j++ ) {
        current_weights( j ) = nn->layer0.weights()( j );
      }
      auto current_biase = nn->layer0.biases()( 0 );

      /* loss for 4/3 eta */
      for ( int j = 0; j < 16; j++ ) {
        nn->layer0.weights()( j ) -= four_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, j );
      }
      nn->layer0.biases()( 0 ) -= four_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 16 );
      nn->apply( input );
      auto loss_four_third_lr = loss_function( nn->output()( 0, 0 ), tempo );

      /* loss for 2/3 eta */
      for ( int j = 0; j < 16; j++ ) {
        nn->layer0.weights()( j )
          = current_weights( j ) - two_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, j );
      }
      nn->layer0.biases()( 0 )
        = current_biase - two_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 16 );
      nn->apply( input );
      auto loss_two_third_lr = loss_function( nn->output()( 0, 0 ), tempo );

      cout << current_loss << " " << loss_four_third_lr << " " << loss_two_third_lr << endl;
      auto min_loss = min( min( current_loss, loss_four_third_lr ), loss_two_third_lr );
      if ( min_loss == current_loss ) {
        learning_rate *= 2.0 / 3;
        for ( int j = 0; j < 16; j++ ) {
          nn->layer0.weights()( j ) = current_weights( j );
        }
        nn->layer0.biases()( 0 ) = current_biase;
      } else if ( min_loss == loss_four_third_lr ) {
        learning_rate *= 4.0 / 3;
        for ( int j = 0; j < 16; j++ ) {
          nn->layer0.weights()( j )
            = current_weights( j ) - four_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, j );
        }
        nn->layer0.biases()( 0 )
          = current_biase - four_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 16 );
      } else {
        for ( int j = 0; j < 16; j++ ) {
          nn->layer0.weights()( j )
            = current_weights( j ) - two_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, j );
        }
        nn->layer0.biases()( 0 )
          = current_biase - two_third_lr * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 16 );
      }
      cout << "weights: " << nn->layer0.weights() << endl;
      cout << "biase: " << nn->layer0.biases()( 0 ) << endl;
    }
  }
  for ( int i = 40; i < 80; i++ ) {
    Matrix<float, batch_size, input_size> input = gen_time( i, 0 );
    nn->apply( input );
    cout << "input: " << i << " output: " << nn->output()( 0, 0 ) << endl;
    // cout << nn->output()( 0, 0 ) << endl;
    //  cout << i << endl;
  }
  cout << "yay!" << endl;
}

int main( int argc, char*[] )
{
  try {
    if ( argc <= 0 ) {
      abort();
    }

    program_body();

    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


#include "problemes.h"
#include "chiffres.h"
#include "utilitaires.h"

#include <boost/rational.hpp>
#include <fstream>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;
typedef boost::rational<nombre> fraction;

ENREGISTRER_PROBLEME(112, "Bouncy numbers") {
    // Working from left-to-right if no digit is exceeded by the digit to its left it is called an
    // increasing number; for example, 134468.
    //
    // Similarly if no digit is exceeded by the digit to its right it is called a decreasing number;
    // for example, 66420.
    //
    // We shall call a positive integer that is neither increasing nor decreasing a "bouncy" number;
    // for example, 155349.
    //
    // Clearly there cannot be any bouncy numbers below one-hundred, but just over half of the numbers
    // below one-thousand (525) are bouncy. In fact, the least number for which the proportion of
    // bouncy numbers first reaches 50% is 538.
    //
    // Surprisingly, bouncy numbers become more and more common and by the time we reach 21780 the
    // proportion of bouncy numbers is equal to 90%.
    //
    // Find the least number for which the proportion of bouncy numbers is exactly 99%.
    fraction limite(99, 100);
    nombre ratio_numerateur = 0;
    nombre ratio_denominateur = 0;

    nombre resultat = 0;
    for (nombre n = 1;; ++n) {
        const auto chiffres = chiffres::extraire_chiffres(n);
        ++ratio_denominateur;
        if (!std::is_sorted(chiffres.begin(), chiffres.end())
            && !std::is_sorted(chiffres.rbegin(), chiffres.rend()))
            ++ratio_numerateur;
        if (ratio_numerateur >= limite * ratio_denominateur) {
            resultat = n;
            break;
        }
    }

    return std::to_string(resultat);
}


#include <fstream>

#include <boost/config.hpp>
#include <boost/version.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Snap_rounding_traits_2.h>
#include <CGAL/Snap_rounding_2.h>
#include <CGAL/Snap_rounding_traits_2.h>

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/RegularGridGraphicsItem.h>
#include <CGAL/Qt/SegmentsGraphicsItem.h>
#include <CGAL/Qt/PolylinesGraphicsItem.h>
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif

// for viewportsBbox
#include <CGAL/Qt/utility.h>

// the two base classes
#include "ui_Snap_rounding_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

typedef CGAL::Exact_predicates_exact_constructions_kernel K;
typedef CGAL::Snap_rounding_traits_2<K>     Traits;

typedef K::Point_2 Point_2;
typedef K::Segment_2 Segment_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Snap_rounding_2
{
  Q_OBJECT

private:

  QGraphicsScene scene;

  CGAL::Qt::RegularGridGraphicsItem<K> * rgi;

  CGAL::Qt::GraphicsViewPolylineInput<K> * pi;

  std::list<Segment_2> input;
  std::list<std::list<Point_2> > output;

  typedef CGAL::Qt::SegmentsGraphicsItem<std::list<Segment_2> > InputSegmentsGraphicsItem;
  typedef CGAL::Qt::PolylinesGraphicsItem<std::list<std::list<Point_2> > > OutputPolylinesGraphicsItem;
  InputSegmentsGraphicsItem * isgi;
  OutputPolylinesGraphicsItem *plgi;
  double delta;

public:
  MainWindow();

  void resize(){
  this->graphicsView->setSceneRect(QRectF(0,0,20, 20));
  this->graphicsView->fitInView(0,0, 20, 20, Qt::KeepAspectRatio);
  }

public Q_SLOTS:

  void processInput(CGAL::Object o);

  void on_actionLoadSegments_triggered();

  void on_actionClear_triggered();

  void on_actionSaveSegments_triggered();

  void on_actionRecenter_triggered();

  void on_actionShowGrid_toggled(bool checked);
  void on_actionShowInput_toggled(bool checked);
  void on_actionShowSnappedSegments_toggled(bool checked);

  void deltaChanged(double);

  virtual void open(QString fileName);

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow(), delta(1.0)
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);

  isgi = new InputSegmentsGraphicsItem(&input);
  scene.addItem(isgi);

  plgi = new OutputPolylinesGraphicsItem(&output);
  scene.addItem(plgi);

 // inputs polylines with 2 points
  pi = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &scene, 2, false);
  QObject::connect(pi, SIGNAL(generate(CGAL::Object)),
		   this, SLOT(processInput(CGAL::Object)));

  scene.installEventFilter(pi);

  // Manual handling of actions
  //

  QObject::connect(this->doubleSpinBox, SIGNAL(valueChanged(double)),
		   this, SLOT(deltaChanged(double)));

  QObject::connect(this->actionQuit, SIGNAL(triggered()),
		   this, SLOT(close()));

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  this->graphicsView->setScene(&scene);
  // Turn the vertical axis upside down
  this->graphicsView->matrix().scale(1, -1);
  this->graphicsView->setMouseTracking(true);

  rgi = new CGAL::Qt::RegularGridGraphicsItem<K>(delta, delta);

    QObject::connect(this, SIGNAL(changed()),
                     rgi, SLOT(modelChanged()));

    QObject::connect(this, SIGNAL(changed()),
                     isgi, SLOT(modelChanged()));

    QObject::connect(this, SIGNAL(changed()),
                     plgi, SLOT(modelChanged()));

  rgi->setVerticesPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  rgi->setEdgesPen(QPen(Qt::gray, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(rgi);

  plgi->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Snap_rounding_2.html");
  this->addAboutCGAL();

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
	  this, SLOT(open(QString)));
}

void
MainWindow::deltaChanged(double d)
{
  if(delta == d){
    return;
  }
  delta = d;
  output.clear();
  CGAL::snap_rounding_2<Traits,std::list<Segment_2>::const_iterator,std::list<std::list<Point_2> > >(input.begin(), input.end(), output, delta, true, false);
  rgi->setDelta(delta, delta);
  Q_EMIT( changed());
}

void
MainWindow::processInput(CGAL::Object o)
{

  std::list<Point_2> points;
  if(CGAL::assign(points, o)){
    if(points.size() == 2) {
      input.push_back(Segment_2(points.front(), points.back()));
      output.clear();
      CGAL::snap_rounding_2<Traits,std::list<Segment_2>::const_iterator,std::list<std::list<Point_2> > >(input.begin(), input.end(), output, delta, true, false);
    }
    else {
      std::cerr << points.size() << std::endl;
    }
  }
  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */

void
MainWindow::on_actionClear_triggered()
{
  input.clear();
  output.clear();
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowGrid_toggled(bool checked)
{
  rgi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowInput_toggled(bool checked)
{
  isgi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionShowSnappedSegments_toggled(bool checked)
{
  plgi->setVisible(checked);
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadSegments_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
						  tr("Open segment file"),
                                                  ".",
                                                  tr("Edge files (*.edg);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.wkt *.WKT);;"
                                                   #endif
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::ifstream ifs(qPrintable(fileName));
  if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
  {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    std::vector<std::vector<Point_2> > mls;
    CGAL::read_multi_linestring_WKT(ifs, mls);
    for(const std::vector<Point_2>& ls : mls)
    {
      if(ls.size() > 2)
        continue;
      Segment_2 seg(ls[0], ls[1]);
      input.push_back(seg);
    }
#endif
  }
  else {
    std::copy(std::istream_iterator<Segment_2>(ifs),
              std::istream_iterator<Segment_2>(),
              std::back_inserter(input));
  }
  output.clear();
  CGAL::snap_rounding_2<Traits,std::list<Segment_2>::const_iterator,std::list<std::list<Point_2> > >(input.begin(), input.end(), output, delta, true, false);
  ifs.close();
  // default cursor
  QApplication::restoreOverrideCursor();
  this->addToRecentFiles(fileName);
  on_actionRecenter_triggered();
  Q_EMIT( changed());
}

void
MainWindow::on_actionSaveSegments_triggered()
{
  QString fileName = QFileDialog::getSaveFileName(this,
						  tr("Save points"),
                                                  ".",
                                                  tr("Edge files (*.edg);;"
                                                   #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                     "WKT files (*.wkt *.WKT);;"
                                                   #endif
                                                     "All files (*)"));
  if(! fileName.isEmpty()){
    std::ofstream ofs(qPrintable(fileName));
    ofs.precision(12);
    if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
    {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      std::vector<std::vector<Point_2> >mls;
      for(const Segment_2& seg : input)
      {
        std::vector<Point_2> ls(2);
        ls[0] = seg.source();
        ls[1] = seg.target();
        mls.push_back(ls);
      }
      CGAL::write_multi_linestring_WKT(ofs, mls);
#endif
    }
    else
      std::copy(input.begin(), input.end(),  std::ostream_iterator<Segment_2>(ofs, "\n"));
  }

}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(isgi->boundingRect());
  this->graphicsView->fitInView(isgi->boundingRect(), Qt::KeepAspectRatio);
}

#include "Snap_rounding_2.moc"
#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Snap_rounding_2 demo");

  // Import resources from libCGAL (Qt5).
  // See https://doc.qt.io/qt-5/qdir.html#Q_INIT_RESOURCE
  CGAL_QT_INIT_RESOURCES;
  Q_INIT_RESOURCE(Snap_rounding_2);

  MainWindow mainWindow;
  mainWindow.show();
  mainWindow.resize();
  return app.exec();
}


/*
 ___ ___ __     __ ____________
|   |   |  |   |__|__|__   ___/  Ubiquitous Internet @ IIT-CNR
|   |   |  |  /__/  /  /  /      Stateful FaaS Model Latency Simulator
|   |   |  |/__/  /   /  /       https://github.com/ccicconetti/markovsim/
|_______|__|__/__/   /__/

Licensed under the MIT License <http://opensource.org/licenses/MIT>.
Copyright (c) 2021 Claudio Cicconetti <https://ccicconetti.github.io/>

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.
*/

/*
Determine the min number of containers required for a given number of clients,
which alternate between having a stateful (= they prefer having a dedicated
container) vs. stateless nature (= they are OK with being assigned to a pool
of shared stateless containers), so that the system is stable and the
probability that a stateful container is assigned to a shared pool of
stateless containers is below a given threshold (epsilon).
*/

#include "Support/chrono.h"
#include "Support/glograii.h"

#include <boost/program_options.hpp>

#include <glog/logging.h>

#include <cassert>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <vector>

namespace po = boost::program_options;

double compute_C_F_max(const size_t C_k, const size_t N_k,
                       const double lambda_k, const double mu_L) {
  return C_k * (mu_L - lambda_k * N_k / C_k) / (mu_L - lambda_k);
}

double binom(const size_t n, const size_t k) {
  // prepare input
  std::vector<double> myVec1(k);
  std::vector<double> myVec2(k);
  for (size_t i = n - k + 1, j = 0; i <= n; i++, j++) {
    myVec1[j] = i;
  }
  for (size_t i = k, j = 0; i >= 1; i--, j++) {
    myVec2[j] = i;
  }

  // compute return value
  double ret = 1.0;
  for (size_t i = 0; i < k; i++) {
    ret *= myVec1[i] / myVec2[i];
  }
  return ret;
}

double P_0(const size_t N_k, const double q_L, const double q_F) {
  return std::pow(q_L / (q_F + q_L), N_k);
}

double P_i(const size_t N_k, const size_t i, const double q_L,
           const double q_F) {
  return P_0(N_k, q_L, q_F) * binom(N_k, i) * std::pow(q_F / q_L, i);
}

/**
 * \return the probability that a function requiring a dedicated container
 * is assigned instead to a pool of shared stateless containers.
 */
double compute_P_v(const size_t C_F_max, const size_t N_k, const double q_L,
                   const double q_F) {
  assert(C_F_max > 0);
  assert(N_k > 0);

  double ret = 0;

  VLOG(2) << "q_L = " << q_L << ", q_F = " << q_F << " N_k = " << N_k << ", P0 "
          << P_0(N_k, q_L, q_F);

  for (size_t i = C_F_max; i <= N_k; i++) {
    ret += P_i(N_k, i, q_L, q_F);
  }

  return ret / (1 - P_0(N_k, q_L, q_F));
}

int main(int argc, char *argv[]) {
  uiiit::support::GlogRaii myGlogRaii(argv[0]);

  size_t N_k; // number of clients
  double inv_mu_F;
  double inv_mu_L;
  double lambda_k;
  double q_F;
  double q_L;
  double epsilon;

#ifndef NDEBUG
  assert(std::abs(binom(10, 4)) - 210.0 < 0.1);
  assert(std::abs(binom(20, 10)) - 184756.0 < 0.1);
  assert(std::abs(binom(30, 10)) - 30045015.0 < 0.1);
#endif

  po::options_description myDesc("Allowed options");
  // clang-format off
  myDesc.add_options()
    ("help,h", "produce help message")
    ("arrival-rate",
     po::value<double>(&lambda_k)->default_value(0.075),
     "Arrival rate, in Hz.")
    ("clients",
     po::value<size_t>(&N_k)->default_value(70),
     "Number of clients")
    ("service-time-full",
     po::value<double>(&inv_mu_F)->default_value(1.0),
     "Service time for clients assigned a dedicated container, in s.")
    ("service-time-less",
     po::value<double>(&inv_mu_L)->default_value(3.0),
     "Service time for clients sharing a pool of non-dedicated containers, in s.")
    ("q-full",
     po::value<double>(&q_F)->default_value(20),
     "Transition rate of clients in a stateless state.")
    ("q-less",
     po::value<double>(&q_L)->default_value(80),
     "Transition rate of clients in a stateful state.")
    ("epsilon",
     po::value<double>(&epsilon)->default_value(0.01),
     "Maximum accepted probability that a client in stateful state is served by a shared container.")
    ;
  // clang-format on

  try {
    po::variables_map myVarMap;
    po::store(po::parse_command_line(argc, argv, myDesc), myVarMap);
    po::notify(myVarMap);

    if (myVarMap.count("help")) {
      std::cout << myDesc << std::endl;
      return EXIT_FAILURE;
    }

    if (inv_mu_F <= 0) {
      throw std::runtime_error("Invalid service time (full): " +
                               std::to_string(inv_mu_F));
    }
    double mu_F = 1.0 / inv_mu_F;
    if (inv_mu_L <= 0) {
      throw std::runtime_error("Invalid service time (less): " +
                               std::to_string(inv_mu_L));
    }
    double mu_L = 1.0 / inv_mu_L;

    if (q_F <= 0) {
      throw std::runtime_error("Invalid transition rate (full): " +
                               std::to_string(q_F));
    }
    if (q_L <= 0) {
      throw std::runtime_error("Invalid transition rate (less): " +
                               std::to_string(q_L));
    }
    if (epsilon <= 0 or epsilon >= 1) {
      throw std::runtime_error("Invalid epsilon: " + std::to_string(epsilon));
    }

    for (size_t C_k = 1; C_k <= N_k; C_k++) {
      // maximum number of containers that can be dedicated to stateful use
      // while allowing the stateless clients to remain stable
      // note: the number can be negative, in which case the system will not be
      // stable for stateless clients even though they are left with _all_ the
      /// containers
      auto C_F_max_real = compute_C_F_max(C_k, N_k, lambda_k, mu_L);
      auto C_F_max_int =
          static_cast<size_t>(C_F_max_real < 0 ? 0 : C_F_max_real);

      if (C_F_max_int == 0) {
        VLOG(1) << "C_F_max = " << C_F_max_real << ": system unstable";
        continue;
      }

      // probability that a stateful container "overflows" to the pool
      // of shared stateless containers
      auto P_v = compute_P_v(C_F_max_int, N_k, q_L, q_F);

      VLOG(1) << "mu_F = " << mu_F << ", mu_L = " << mu_L << ", C_k = " << C_k
              << ", C_F_max = " << C_F_max_real << ", P_v = " << P_v;

      // if the probability is below threshold, quit
      if (P_v <= epsilon) {
        std::cout << C_k << ' ' << (static_cast<double>(C_k) / N_k) << ' '
                  << (C_k / (N_k * q_F / (q_F + q_L))) << std::endl;
        break;
      }
    }

    return EXIT_SUCCESS;

  } catch (const std::exception &aErr) {
    LOG(ERROR) << "Exception caught: " << aErr.what();

  } catch (...) {
    LOG(ERROR) << "Unknown exception caught";
  }

  return EXIT_FAILURE;
}


#include <boost/lexical_cast.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <cmath>
#include <iostream>
#include <map>
#include <string>

using namespace std;
using boost::lexical_cast;
using boost::multiprecision::cpp_int;

// Generate the n'th term in the Fibonacci sequence.
cpp_int fib(cpp_int n) {
	static map<cpp_int, cpp_int> memory;
	if(n <= 1) {
		return n;
	}
	if(memory.count(n) > 0) {
		return memory[n];
	}
	cpp_int ret = fib(n - 1) + fib(n - 2);
	memory[n] = ret;
	return ret;
}

int main(int argc, char *argv[]) {
	int index = 1;
	int num_digits = 0;
	while(num_digits != 1000) {
		string s = lexical_cast<string>(fib(index++));
		num_digits = s.size();
	}
	cout << (index - 1) << endl;
	return 0;
}


#include <math.h>
#include <EigenUnsupported/Eigen/KroneckerProduct>
#include "Core/Utilities/QProgInfo/QCircuitInfo.h"
#include "Core/Utilities/Tools/MatrixDecomposition.h"
#include <chrono>
#include "Core/Utilities/QProgInfo/Visualization/QVisualization.h"
#include "QAlg/Base_QCircuit/AmplitudeEncode.h"

USING_QPANDA
using namespace std;
using namespace chrono;

#define PRINT_TRACE 0
#if PRINT_TRACE
#define PTrace printf
#define PTraceMat(mat) (std::cout << (mat) << endl)
#define PTraceCircuit(cir) (std::cout << cir << endl)
#else
#define PTrace
#define PTraceMat(mat)
#define PTraceCircuit(cir)
#endif

#define MAX_MATRIX_PRECISION 1e-10

using MatrixSequence = std::vector<MatrixUnit>;
using DecomposeEntry = std::pair<int, MatrixSequence>;

using ColumnOperator = std::vector<DecomposeEntry>;
using MatrixOperator = std::vector<ColumnOperator>;

using SingleGateUnit = std::pair<MatrixSequence, QStat>;

static void upper_partition(int order, MatrixOperator &entries)
{
	auto index = (int)std::log2(entries.size() + 1) - (int)std::log2(order) - 1;

	for (auto cdx = 0; cdx < order - 1; ++cdx)
	{
		for (auto rdx = 0; rdx < order - cdx - 1; ++rdx)
		{
			auto entry = entries[cdx][rdx];

			entry.first += order;
			entry.second[index] = MatrixUnit::SINGLE_P1;

			entries[cdx + order].emplace_back(entry);
		}
	}

    return;
}

static bool entry_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int lj = ((cdx - 1) >> (udx - 1)) & 1;

	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if 1 ≤ j ≤ m and cj = lj' = 1 , return true
	auto mat = units[units.size() - udx];
	return udx >= 1
		&& udx <= M
		&& lj
		&& mat == MatrixUnit::SINGLE_P1;
}

static bool steps_requirement(const MatrixSequence& units, int udx, int cdx)
{
	int M = 1;
	while (cdx)
	{
		cdx >>= 1;
		M += cdx ? 1 : 0;
	}

	//if j = n and none of cn...cm+1 is 1 , return true
	if (units.size() != udx)
	{
		return false;
	}
	else
	{
		auto iter = std::find(units.begin(), units.end() - M, MatrixUnit::SINGLE_P1);
		return (units.end() - M) == iter;
	}
}

static void under_partition(int order, MatrixOperator& entries)
{
	auto qubits = (int)std::log2(entries.size() + 1);

	for (auto cdx = 1; cdx < order; ++cdx)
	{
		if (cdx & 1)
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto value = entries[0][rdx + order - 1].first ^ cdx;
				auto entry = make_pair(value, entries[cdx - 1][rdx + order - cdx].second);

				entries[cdx].emplace_back(entry);
			}

			auto &units = entries[cdx].back().second;
			for (auto idx = 0; idx < (int)std::log2(order); ++idx)
			{
				units[qubits - idx - 1] = ((cdx >> idx) & 1) ?
					MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;
			}
		}
		else
		{
			for (auto rdx = 0; rdx < order; ++rdx)
			{
				auto range = (int)std::log2(order) + 1;
				auto refer = entries[0][rdx + order - 1].second;
				auto entry = entries[0][rdx + order - 1].first ^ cdx;

				MatrixSequence units(refer.begin() + qubits - range, refer.end());

				for (auto udx = 1; udx <= range; ++udx)  /*udx = j , cdx = L*/
				{
					bool steps_accord = steps_requirement(units, udx, cdx + 1);
					bool entry_accord = entry_requirement(units, udx, cdx + 1);

					units[range - udx] = steps_accord ? MatrixUnit::SINGLE_P1 :
						entry_accord ? MatrixUnit::SINGLE_P0 : units[range - udx];
				}

				for (auto idx = 0; idx < qubits - range; ++idx)
				{
					units.insert(units.begin(), MatrixUnit::SINGLE_I2);
				}

				entries[cdx].emplace_back(make_pair(entry, units));
			}

			auto refer_opt = entries[0][2 * order - 2].second;
			for (auto idx = 0; idx < qubits; ++idx)
			{
				if ((cdx >> idx) & 1)
				{
					refer_opt[qubits - idx - 1] = MatrixUnit::SINGLE_P1;
				}
			}

			entries[cdx].back().second = refer_opt;
		}
	}

    return;
}

static void controller(MatrixSequence &sequence, const EigenMatrix2c U2, EigenMatrixXc &matrix)
{
	EigenMatrix2c P0;
	EigenMatrix2c P1;
	EigenMatrix2c I2;

	P0 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0);
	P1 << Eigen::dcomplex(0, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);
	I2 << Eigen::dcomplex(1, 0), Eigen::dcomplex(0, 0),
		  Eigen::dcomplex(0, 0), Eigen::dcomplex(1, 0);

	std::map<MatrixUnit, std::function<EigenMatrix2c()>> mapping =
	{
		{ MatrixUnit::SINGLE_P0, [&]() {return P0; } },
		{ MatrixUnit::SINGLE_P1, [&]() {return P1; } },
		{ MatrixUnit::SINGLE_I2, [&]() {return I2; } },
		{ MatrixUnit::SINGLE_V2, [&]() {return U2 - I2; } }
	};

	auto order = sequence.size();
	EigenMatrixXc Un = EigenMatrixXc::Identity(1, 1);
	EigenMatrixXc In = EigenMatrixXc::Identity(1ull << order, 1ull << order);

	for (const auto &val : sequence)
	{
		EigenMatrix2c M2 = mapping.find(val)->second();
		Un = Eigen::kroneckerProduct(Un, M2).eval();
	}

	matrix = In + Un;
    return;
}

static void recursive_partition(const EigenMatrixXc& sub_matrix, MatrixOperator &entries)
{
    Eigen::Index order = sub_matrix.rows();
    if (1 == order)
    {
        return;
    }
    else
    {
        EigenMatrixXc corner = sub_matrix.topLeftCorner(order / 2, order / 2);

        recursive_partition(corner, entries);

        upper_partition(order / 2, entries);
        under_partition(order / 2, entries);
    }

    return;
}

static void decomposition(EigenMatrixXc& matrix, MatrixOperator& entries, std::vector<SingleGateUnit>& cir_units)
{
	for (auto cdx = 0; cdx < entries.size(); ++cdx)
	{
		auto opts = entries[cdx].size();
		for (auto idx = 0; idx < opts; ++idx)
		{
			auto rdx = entries[cdx][idx].first;
			auto opt = entries[cdx][idx].second;

			if ((EigenComplexT(0, 0) == matrix(rdx, cdx) && (idx != opts - 1)) ||
				(EigenComplexT(1, 0) == matrix(cdx + 1, cdx) && (idx == opts - 1)))
			{
				continue;
			}
			else
			{
				EigenMatrix2c C2; /*placeholder*/
				C2 << EigenComplexT(0, 1), EigenComplexT(0, 1),
					EigenComplexT(0, 1), EigenComplexT(0, 1);

				EigenMatrixXc Cn;
				controller(opt, C2, Cn);

				Qnum indices(2);
				for (Eigen::Index index = 0; index < (1ull << opt.size()); ++index)
				{
					if (Cn(rdx, index) != EigenComplexT(0, 0))
					{
						indices[index == rdx] = index;
					}
				}

				EigenComplexT C0 = matrix(indices[0], cdx);  /*The entry to be eliminated */
				EigenComplexT C1 = matrix(indices[1], cdx);  /*The corresponding entry */

				EigenComplexT V11, V12, V21, V22;

				if (indices[0] < indices[1])
				{
					V11 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
				}
				else
				{
					V11 = -C0 / std::sqrt(std::norm(C0) + std::norm(C1));
					V12 = C1 / std::sqrt(std::norm(C0) + std::norm(C1));
					V21 = std::conj(C1) / std::sqrt(std::norm(C0) + std::norm(C1));
					V22 = std::conj(C0) / std::sqrt(std::norm(C0) + std::norm(C1));
				}

				EigenMatrix2c V2;
				V2 << V11, V12, V21, V22;

				EigenMatrixXc Un;
				controller(opt, V2, Un);

				matrix = Un * matrix;

				QStat M2 = { (qcomplex_t)V11 ,(qcomplex_t)V12 ,(qcomplex_t)V21 ,(qcomplex_t)V22 };
				cir_units.insert(cir_units.begin(), std::make_pair(opt, M2));
			}
		}
	}

	EigenMatrix2c V2 = matrix.bottomRightCorner(2, 2);
	if (EigenMatrixXc::Identity(2, 2) != V2)
	{
		QStat M2 = { (qcomplex_t)((EigenComplexT)1.0 / V2(0,0)), (qcomplex_t)(V2(0,1)),
					 (qcomplex_t)(V2(1,0)) , (qcomplex_t)((EigenComplexT)1.0 / V2(1,1))};

		auto entry = entries.back().back().second;
		cir_units.insert(cir_units.begin(), std::make_pair(entry, M2));
	}
}

static void initialize(EigenMatrixXc& matrix, MatrixOperator& entries)
{
    auto qubits = (int)std::log2(matrix.rows());

    MatrixSequence Cns(qubits, MatrixUnit::SINGLE_I2);
    Cns.back() = MatrixUnit::SINGLE_V2;
    entries.front().emplace_back(make_pair(1, Cns));

    ColumnOperator& column = entries.front();
    for (auto idx = 1; idx < qubits; ++idx)
    {
        size_t path = 1ull << idx;
        for (auto opt = 0; opt < (1 << idx) - 1; ++opt)
        {
            auto entry = column[opt].first;
            auto units = column[opt].second;

            // 1 : none of cn−1, . . . , c1 equals 1
            // * : otherwise
            auto iter = std::find(units.end() - idx, units.end(), MatrixUnit::SINGLE_P1);

            units[units.size() - 1 - idx] = (units.end() == iter) ?
                MatrixUnit::SINGLE_P1 : MatrixUnit::SINGLE_I2;

            column.emplace_back(make_pair(entry + path, units));
        }

        MatrixSequence Lns(qubits, MatrixUnit::SINGLE_I2);
        Lns[qubits - idx - 1] = MatrixUnit::SINGLE_V2;

        column.emplace_back(make_pair((1ull << idx), Lns));
    }

    return;
}

static void general_scheme(EigenMatrixXc& matrix, std::vector<SingleGateUnit>& cir_units)
{
	MatrixOperator entries;
	for (auto idx = 1; idx < matrix.cols(); ++idx)
	{
		ColumnOperator Co;
		entries.emplace_back(Co);
	}

	initialize(matrix, entries);
 	recursive_partition(matrix, entries);
	decomposition(matrix, entries, cir_units);

    return;
}

static void circuit_insert(QVec& qubits, std::vector<SingleGateUnit>& cir_units, QCircuit &circuit)
{
	std::sort(qubits.begin(), qubits.end(), [&](Qubit *a, Qubit *b)
	{
		return a->getPhysicalQubitPtr()->getQubitAddr()
			 < b->getPhysicalQubitPtr()->getQubitAddr();
	});

	auto rank = qubits.size();
	for (auto &val : cir_units)
	{
		QVec control;
		QCircuit cir;

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_P0 == val.first[qdx])
			{
				cir << X(qubits[qdx]);
				control.emplace_back(qubits[qdx]);
			}
			else if (MatrixUnit::SINGLE_P1 == val.first[qdx])
			{
				control.emplace_back(qubits[qdx]);
			}
			else
			{}
		}

		for (auto qdx = 0; qdx < rank; qdx++)
		{
			if (MatrixUnit::SINGLE_V2 == val.first[qdx])
			{
				circuit << cir
					    << U4(val.second, qubits[qdx]).control(control).dagger()
						<< cir;

				break;
			}
		}
	}
}

/*******************************************************************
*                      class DiagonalMatrixDecompose
********************************************************************/
class DiagonalMatrixDecompose
{
public:
	DiagonalMatrixDecompose() {}
	~DiagonalMatrixDecompose() {}

	QCircuit decompose(const QVec& qubits, const QStat& src_mat)
	{
		//check param
		if (!is_unitary_matrix(src_mat))
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, the input matrix is not a unitary-matrix.");
		}

		const auto mat_dimension = sqrt(src_mat.size());
		const auto need_qubits_num = ceil(log2(mat_dimension));
		if (need_qubits_num > qubits.size())
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed on HQRDecompose, no enough qubits.");
		}

		QCircuit decompose_result_cir;
		m_qubits = qubits;
		QVec controlqvec = qubits;
		controlqvec.pop_back();
		QStat tmp_mat22; //2*2 unitary matrix
		const size_t tmp_base_unitary_cnt = mat_dimension / 2;
		long pre_index = -1;
		for (size_t i = 0; i < tmp_base_unitary_cnt; ++i)
		{
			if (0 == i)
			{
				QCircuit index_cir_zero = index_to_circuit(0, controlqvec);
				decompose_result_cir << index_cir_zero;
			}
			else
			{
				QCircuit index_cir = index_to_merge_circuit(i, pre_index, controlqvec);
				decompose_result_cir << index_cir;
			}

			tmp_mat22.clear();
			const size_t tmp_row = (2 * i * mat_dimension) + (2 * i);
			tmp_mat22.push_back(src_mat[tmp_row]);
			tmp_mat22.push_back(src_mat[tmp_row + 1]);
			tmp_mat22.push_back(src_mat[tmp_row + mat_dimension]);
			tmp_mat22.push_back(src_mat[tmp_row + mat_dimension + 1]);
			QGate tmp_u4 = U4(tmp_mat22, qubits.back()).control(controlqvec);
			QGATE_SPACE::U4* p_gate = dynamic_cast<QGATE_SPACE::U4*>(tmp_u4.getQGate());
			if ((abs(p_gate->getAlpha()) > MAX_MATRIX_PRECISION)
				|| (abs(p_gate->getBeta()) > MAX_MATRIX_PRECISION)
				|| (abs(p_gate->getGamma()) > MAX_MATRIX_PRECISION)
				|| (abs(p_gate->getDelta()) > MAX_MATRIX_PRECISION))
			{
				decompose_result_cir << tmp_u4;
			}

			pre_index = i;
		}

		return decompose_result_cir;
	}

protected:
	QCircuit index_to_circuit(size_t index, QVec& controlqvec)
	{
		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if (0 == index % 2)
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;
		}

		return ret_cir;
	}

	QCircuit index_to_merge_circuit(size_t index, QVec& controlqvec)
	{
		if (0 == index)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed to build merge-index-circuit, the index must be >0.");
		}

		size_t pre_index = index - 1;
		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if ((index % 2) != (pre_index % 2))
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;
			pre_index /= 2;
		}

		return ret_cir;
	}

	QCircuit index_to_merge_circuit(size_t index, long pre_index, QVec& controlqvec)
	{
		if (0 == index)
		{
			QCERR_AND_THROW_ERRSTR(run_fail, "Error: failed to build merge-index-circuit, the index must be >0.");
		}

		size_t tmp_pre_index = pre_index;
		if (pre_index < 0)
		{
			tmp_pre_index = 1;
		}

		QCircuit ret_cir;
		size_t data_qubits_cnt = controlqvec.size();
		for (size_t i = 0; i < data_qubits_cnt; ++i)
		{
			if ((index % 2) != (tmp_pre_index % 2))
			{
				ret_cir << X(controlqvec[data_qubits_cnt - i - 1]);
			}

			index /= 2;

			if (pre_index > 0)
			{
				tmp_pre_index /= 2;
			}
		}

		return ret_cir;
	}

private:
	QVec m_qubits;
};

/*******************************************************************
*                      public interface
********************************************************************/
QCircuit QPanda::matrix_decompose_qr(QVec qubits, const QStat& src_mat)
{
	auto order = std::sqrt(src_mat.size());
	EigenMatrixXc tmp_mat = EigenMatrixXc::Map(&src_mat[0], order, order);

    return matrix_decompose_qr(qubits, tmp_mat);
}

QCircuit QPanda::matrix_decompose_qr(QVec qubits, EigenMatrixXc& src_mat)
{
	if (!src_mat.isUnitary(MAX_MATRIX_PRECISION))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "Non-unitary matrix.");
	}

	if (qubits.size() != log2(src_mat.cols()))
	{
		QCERR_AND_THROW_ERRSTR(invalid_argument, "The qubits number is error or the input matrix is not a 2^n-dimensional matrix.");
	}

	QCircuit output_circuit;
    //QR decompose
    std::vector<SingleGateUnit> cir_units;
    general_scheme(src_mat, cir_units);
    circuit_insert(qubits, cir_units, output_circuit);

	return output_circuit;
}

QCircuit QPanda::diagonal_matrix_decompose(const QVec& qubits, const QStat& src_mat)
{
	return DiagonalMatrixDecompose().decompose(qubits, src_mat);
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

// Jonathan Driedger, Thomas Prätzlich, and Meinard Müller
// Let It Bee — Towards NMF-Inspired Audio Mosaicing
// Proceedings of ISMIR 2015

#pragma once

#include "STFT.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <vector>

namespace fluid {
namespace algorithm {

using _impl::asEigen;
using _impl::asFluid;
using Eigen::Array;
using Eigen::ArrayXd;
using Eigen::ArrayXXd;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::VectorXd;

class NMFCross
{

public:
  // pass iteration number; returns true if able to continue (i.e. not
  // cancelled)
  using ProgressCallback = std::function<bool(index)>;

  NMFCross(index nIterations) : mIterations(nIterations) {}

  static void synthesize(const RealMatrixView h, const ComplexMatrixView w,
                         ComplexMatrixView out)
  {
    using namespace Eigen;
    using namespace _impl;
    MatrixXd  H = asEigen<Matrix>(h);
    MatrixXcd W = asEigen<Matrix>(w);
    MatrixXcd V = H * W;
    out <<= asFluid(V);
  }

  void process(const RealMatrixView X, RealMatrixView H1, RealMatrixView W0,
               index r, index p, index c) const
  {
    index nFrames = X.extent(0);
    index nBins = X.extent(1);
    index rank = W0.extent(0);
    nBins = W0.extent(1);
    MatrixXd W = asEigen<Matrix>(W0).transpose();
    MatrixXd H;
    H = MatrixXd::Random(rank, nFrames) * 0.5 +
        MatrixXd::Constant(rank, nFrames, 0.5);
    MatrixXd V = asEigen<Matrix>(X).transpose();
    multiplicativeUpdates(V, W, H, r, p, c);
    MatrixXd HT = H.transpose();
    H1 <<= asFluid(HT);
  }

  void addProgressCallback(ProgressCallback&& callback)
  {
    mCallbacks.emplace_back(std::move(callback));
  }

private:
  index                         mIterations;
  std::vector<ProgressCallback> mCallbacks;

  std::vector<index> topC(Eigen::VectorXd vec, index c) const
  {
    using namespace std;
    vector<double> stdVec(vec.data(), vec.data() + vec.size());
    sort(stdVec.begin(), stdVec.end());
    vector<index> idx(asUnsigned(vec.size()));
    iota(idx.begin(), idx.end(), 0);
    sort(idx.begin(), idx.end(),
         [&vec](index i1, index i2) { return vec[i1] > vec[i2]; });
    auto result = std::vector<index>(idx.begin(), idx.begin() + c);
    return result;
  }

  Eigen::MatrixXd promoteContinuity(MatrixXd& H, index size) const
  {
    index    halfSize = (size - 1) / 2;
    MatrixXd kernel = MatrixXd::Identity(size, size);
    MatrixXd padded = MatrixXd::Zero(H.rows() + size, H.cols() + size);
    MatrixXd output = MatrixXd::Zero(H.rows(), H.cols());
    padded.block(halfSize, halfSize, H.rows(), H.cols()) = H;
    for (index i = 0; i < H.rows(); i++)
    {
      for (index j = 0; j < H.cols(); j++)
      {
        output(i, j) =
            padded.block(i, j, size, size).cwiseProduct(kernel).sum();
      }
    }
    return output;
  }

  Eigen::MatrixXd enforceTemporalSparseness(MatrixXd& H, index size,
                                            index iteration) const
  {
    index    halfSize = (size - 1) / 2;
    MatrixXd padded = MatrixXd::Zero(H.rows(), H.cols() + size);
    MatrixXd output = MatrixXd::Zero(H.rows(), H.cols());
    padded.block(0, halfSize, H.rows(), H.cols()) = H;
    for (index i = 0; i < H.rows(); i++)
    {
      for (index j = 0; j < H.cols(); j++)
      {
        VectorXd        neighborhood = padded.row(i).segment(j, size);
        VectorXd::Index maxIndex{0};
        neighborhood.maxCoeff(&maxIndex);
        if (int(maxIndex) != halfSize)
        { output(i, j) = H(i, j) * (1 - ((iteration + 1) / mIterations)); }
        else
        {
          output(i, j) = H(i, j);
        }
      }
    }
    return output;
  }

  Eigen::MatrixXd restrictPolyphony(MatrixXd& H, ArrayXd& energyInW, index size,
                                    index iteration) const
  {
    MatrixXd output = MatrixXd::Zero(H.rows(), H.cols());
    for (index k = 0; k < H.cols(); k++)
    {
      ArrayXd wCol = H.col(k).array() * energyInW.array();
      output.col(k) = H.col(k) * (1 - ((iteration + 1) / mIterations));
      auto top = topC(wCol, size);
      for (auto t : top) { output(t, k) = H(t, k); }
    }
    return output;
  }
  void multiplicativeUpdates(MatrixXd& V, MatrixXd& W, MatrixXd& H, index r,
                             index p, index c) const
  {
    using namespace std;
    using namespace Eigen;
    double const epsilon = std::numeric_limits<double>::epsilon();
    MatrixXd     ones = MatrixXd::Ones(V.rows(), V.cols());
    W = W.array().max(epsilon).matrix();
    // ArrayXd wNorm = W.colwise().sum();
    // W.array().rowwise() /= wNorm.transpose());
    ArrayXd energyInW = W.array().square().colwise().sum();
    for (index i = 0; i < mIterations; i++)
    {
      if ((i % 1) == 0)
      { // TODO: original version seems to work better with one in 5 iterations
        H = enforceTemporalSparseness(H, r, i);
        H = restrictPolyphony(H, energyInW, p, i);
        H = promoteContinuity(H, c);
      }
      ArrayXXd V2 = (W * H).array().max(epsilon);
      ArrayXXd hnum = (W.transpose() * (V.array() / V2).matrix()).array();
      ArrayXXd hden = (W.transpose() * ones).array();
      H = (H.array() * hnum / hden.max(epsilon)).matrix();
      // MatrixXd R = W * H;
      // R = R.cwiseMax(epsilon);
      // double divergence = (V.cwiseProduct(V.cwiseQuotient(R)) - V + R).sum();
      for (auto& cb : mCallbacks)
        if (!cb(i + 1)) return;
    }
    V = W * H;
  }
};
} // namespace algorithm
} // namespace fluid


#ifndef HAMILTONIANS_XXXXMG_HPP
#define HAMILTONIANS_XXXXMG_HPP
#include <Eigen/Eigen>
#include <nlohmann/json.hpp>

class XXXMG
{
private:
	int n_;

public:

	XXXMG(int n)
		: n_(n)
	{
	}

	nlohmann::json params() const
	{
		return nlohmann::json
		{
			{"name", "XXXXMG"},
			{"n", n_},
		};
	}

	template<class State>
	typename State::Scalar operator()(const State& smp) const
	{
		constexpr double J1 = 1.0;
		constexpr double J2 = 0.5;
		typename State::Scalar s = 0.0;

		{
			double yysign = -smp.sigmaAt(0)*smp.sigmaAt(1);
			s += -J1/2*yysign; //zz
			s += J1/2*(1.0+yysign)*smp.ratio(0, 1); //xx+yy
		}
		{
			double yysign = -smp.sigmaAt(n_-2)*smp.sigmaAt(n_-1);
			s += -J1/2*yysign; //zz
			s += J1/2*(1.0+yysign)*smp.ratio(n_-2, n_-1); //xx+yy
		}
		//Nearest-neighbor
		for(int i = 1; i < (n_-3); i++)
		{
			double yysign = -smp.sigmaAt(i)*smp.sigmaAt(i+1);
			s += -J1*yysign; //zz
			s += J1*(1.0+yysign)*smp.ratio(i, i+1); //xx+yy
		}
		//Next-nearest-neighbor
		for(int i = 0; i < n_-3; i++)
		{
			double yysign = -smp.sigmaAt(i)*smp.sigmaAt(i+2);
			s += -J2*yysign; //zz
			s += J2*(1.0+yysign)*smp.ratio(i, i+2); //xx+yy
		}
		return s;
	}

	std::map<uint32_t, double> operator()(uint32_t col) const
	{
		constexpr double J1 = 1.0;
		constexpr double J2 = 0.5;
		std::map<uint32_t, double> m;
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+1)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+1)%(n_)));
			m[col ^ x] += J1*(1.0 - sgn*1.0);
			m[col] += J1*sgn;
		}
		for(int i = 0; i < n_; i++)
		{
			int b1 = (col >> i) & 1;
			int b2 = (col >> ((i+2)%n_)) & 1;
			int sgn = (1-2*b1)*(1-2*b2);
			long long int x = (1 << i) | (1 << ((i+2)%(n_)));
			m[col ^ x] += J2*(1.0 - sgn*1.0);
			m[col] += J2*sgn;
		}
		return m;
	}
};
#endif//HAMILTONIANS_XXXXMG_HPP


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

/*
 Copyright (C) 2014 Jose Aparicio

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

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

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef quantlib_gaussian_copula_policy_hpp
#define quantlib_gaussian_copula_policy_hpp

#include <vector>
#include <numeric>
#include <algorithm>

#include <boost/bind.hpp>

#include <ql/utilities/disposable.hpp>
#include <ql/math/distributions/normaldistribution.hpp>

namespace QuantLib {

    /*! Gaussian Latent Model's copula policy. Its simplicity is a result of
      the convolution stability of the Gaussian distribution.
    */
    /* This is the only case that would have allowed the policy to be static,
    but other copulas will need parameters and initialization.*/
    struct GaussianCopulaPolicy {

        typedef int initTraits;

        explicit GaussianCopulaPolicy(
            const std::vector<std::vector<Real> >& factorWeights =
                std::vector<std::vector<Real> >(),
            const initTraits& dummy = int())
        : numFactors_(factorWeights.size() + factorWeights[0].size())
        {
            /* check factors in LM are normalized. */
            for(Size iLVar=0; iLVar<factorWeights.size(); iLVar++) {
                Real factorsNorm =
                    std::inner_product(factorWeights[iLVar].begin(),
                        factorWeights[iLVar].end(),
                        factorWeights[iLVar].begin(), 0.);
                QL_REQUIRE(factorsNorm < 1.,
                    "Non normal random factor combination.");
            }
            /* check factor matrix is squared .......... */
        }

        /*! Number of independent random factors.
        This is the only methos that ould stop the class from being static, it
        is needed for the MC generator construction.
        */
        Size numFactors() const {
            return numFactors_;
        }

        //! returns a copy of the initialization arguments
        initTraits getInitTraits() const {
            return initTraits();
        }

        /*! Cumulative probability of the indexed latent variable
            @param iVariable The index of the latent variable requested.
        */
        Probability cumulativeY(Real val, Size iVariable) const {
            return cumulative_(val);
        }
        //! Cumulative probability of the idiosyncratic factors (all the same)
        Probability cumulativeZ(Real z) const {
            return cumulative_(z);
        }
        /*! Probability density of a given realization of values of the systemic
          factors (remember they are independent). In the normal case, since
          they all follow the same law it is just a trivial product of the same
          density.
          Intended to be used in numerical integration of an arbitrary function
          depending on those values.
        */
        Probability density(const std::vector<Real>& m) const {
            return std::accumulate(m.begin(), m.end(), 1.,
                boost::bind(std::multiplies<Real>(), _1,
                    boost::bind(density_, _2)));
        }
        /*! Returns the inverse of the cumulative distribution of the (modelled)
          latent variable (as indexed by iVariable). The normal stability avoids
          the convolution of the factors' distributions
        */
        Real inverseCumulativeY(Probability p, Size iVariable) const {
            return InverseCumulativeNormal::standard_value(p);
        }
        /*! Returns the inverse of the cumulative distribution of the
        idiosyncratic factor (identically distributed for all latent variables)
        */
        Real inverseCumulativeZ(Probability p) const {
            return InverseCumulativeNormal::standard_value(p);
        }
        /*! Returns the inverse of the cumulative distribution of the
          systemic factor iFactor.
        */
        Real inverseCumulativeDensity(Probability p, Size iFactor) const {
            return InverseCumulativeNormal::standard_value(p);
        }
        //!
        //to use this (by default) version, the generator must be a uniform one.
        Disposable<std::vector<Real> >
            allFactorCumulInverter(const std::vector<Real>& probs) const {
            std::vector<Real> result;
            result.resize(probs.size());
            std::transform(probs.begin(), probs.end(), result.begin(),
                boost::bind(&InverseCumulativeNormal::standard_value, _1));
            return result;
        }
    private:
        mutable Size numFactors_;
        // no op =
        static const NormalDistribution density_;
        static const CumulativeNormalDistribution cumulative_;
    };

}

#endif


#ifndef STAN_MATH_PRIM_SCAL_FUN_OWENS_T_HPP
#define STAN_MATH_PRIM_SCAL_FUN_OWENS_T_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/special_functions/owens_t.hpp>

namespace stan {
namespace math {

/**
 * Return the result of applying Owen's T function to the
 * specified arguments.
 *
 * Used to compute the cumulative density function for the skew normal
 * distribution.
 *
   \f[
   \mbox{owens\_t}(h, a) =
   \begin{cases}
     \mbox{owens\_t}(h, a) & \mbox{if } -\infty\leq h, a \leq \infty \\[6pt]
     \textrm{NaN} & \mbox{if } h = \textrm{NaN or } a = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{owens\_t}(h, a)}{\partial h} =
   \begin{cases}
     \frac{\partial\, \mbox{owens\_t}(h, a)}{\partial h} & \mbox{if }
 -\infty\leq h, a\leq \infty \\[6pt] \textrm{NaN} & \mbox{if } h = \textrm{NaN
 or } a = \textrm{NaN} \end{cases} \f]

   \f[
   \frac{\partial\, \mbox{owens\_t}(h, a)}{\partial a} =
   \begin{cases}
     \frac{\partial\, \mbox{owens\_t}(h, a)}{\partial a} & \mbox{if }
 -\infty\leq h, a\leq \infty \\[6pt] \textrm{NaN} & \mbox{if } h = \textrm{NaN
 or } a = \textrm{NaN} \end{cases} \f]

   \f[
   \mbox{owens\_t}(h, a) = \frac{1}{2\pi} \int_0^a
 \frac{\exp(-\frac{1}{2}h^2(1+x^2))}{1+x^2}dx \f]

   \f[
   \frac{\partial \, \mbox{owens\_t}(h, a)}{\partial h} =
 -\frac{1}{2\sqrt{2\pi}} \operatorname{erf}\left(\frac{ha}{\sqrt{2}}\right)
   \exp\left(-\frac{h^2}{2}\right)
   \f]

   \f[
   \frac{\partial \, \mbox{owens\_t}(h, a)}{\partial a} =
 \frac{\exp\left(-\frac{1}{2}h^2(1+a^2)\right)}{2\pi (1+a^2)} \f]
 *
 * @param h First argument
 * @param a Second argument
 * @return Owen's T function applied to the arguments.
 */
inline double owens_t(double h, double a) { return boost::math::owens_t(h, a); }
}  // namespace math
}  // namespace stan

#endif


////////////////////////////////////////////////////////////////////////////////
// BendingEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Implements the bending energy from [Grinspun et al. 2003: Discrete Shells].
//  We provide analytical gradients for the energy, but resort to automatic
//  differentiation for Hessians.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  05/25/2019 15:43:17
////////////////////////////////////////////////////////////////////////////////
#ifndef BENDINGENERGY_HH
#define BENDINGENERGY_HH

#include <cmath>
#include <Eigen/Dense>
#include <array>
#include <MeshFEM/AutomaticDifferentiation.hh>

// Bending energy contributed by a single "hinge" (mesh edge) between two
// triangles.
// The triangle points are indexed as follows:
//            p0-----p1            n1   n2
//              \ 1 / \             ^   ^
//               \ /`.2\             \ /
//               p2   `.\             o
//                      p3
// We call the complement of the dihedral angle "theta". Then the bending energy is given by
//      0.5 (theta - theta_bar)^2 ||e_bar||/h_bar
// (we expect the code using this class to scale everything by the bending stiffness).

// Get the indices corresponding to the four vertices in the hinge stencil for halfedge "he".
//  p0-he->p1
//    \ 1 /2|
//     \ /`.|
//     p2  p3
// Note: "he" lies in triangle 2.
template<class HalfEdge>
std::array<int, 4> bendingHingeStencil(const HalfEdge &he) {
    assert(he.isPrimary() && !he.isBoundary());
    return {{ he.tail().index(),
              he.tip ().index(),
              he.opposite().next().tip().index(),
              he           .next().tip().index() }};
}

// Templated by real number type "_Real" for autodiff.
template<class _Real>
struct HingeEnergy {
    using Real = _Real;
    using Pt = Eigen::Matrix<_Real, 3, 1>;
    using Vec = Pt;

    // Reference configuration quantities
    // (We don't need autodiff types for these...)
    double e_bar_len, h_bar;
    double theta_bar;

    // Deformed configuration quantities
    Eigen::Matrix<Real, 3, 4> deformed_pts;
    Real theta, e_len;
    Real squared_dbl_A1, squared_dbl_A2;
    Vec N1, N2; // un-normalized triangle normals (cross products of edge vectors)
    Real e_01_dot_ehat, e_02_dot_ehat;
    Real e_11_dot_ehat, e_12_dot_ehat;

    // Copy the reference configuration quantities from an existing class of a different type
    template<class _Real2>
    HingeEnergy(HingeEnergy<_Real2> h2) : e_bar_len(h2.e_bar_len), h_bar(h2.h_bar), theta_bar(h2.theta_bar) {
        e_bar_len      = h2.e_bar_len;
        h_bar          = h2.h_bar;
        theta_bar      = h2.theta_bar;

        theta          = h2.theta;
        e_len          = h2.e_len;
        squared_dbl_A1 = h2.squared_dbl_A1;
        squared_dbl_A2 = h2.squared_dbl_A2;
        N1             = h2.N1;
        N2             = h2.N2;
        e_01_dot_ehat  = h2.e_01_dot_ehat;
        e_02_dot_ehat  = h2.e_02_dot_ehat;

        e_11_dot_ehat  = h2.e_11_dot_ehat;
        e_12_dot_ehat  = h2.e_12_dot_ehat;
    }

    HingeEnergy(Eigen::Ref<const Pt> ref_p0,
                Eigen::Ref<const Pt> ref_p1,
                Eigen::Ref<const Pt> ref_p2,
                Eigen::Ref<const Pt> ref_p3) {
        Vec e = ref_p1 - ref_p0;
        e_bar_len = e.norm();
        e /= e_bar_len;

        Vec ref_n1 = (ref_p2 - ref_p0).cross(ref_p1 - ref_p0),
            ref_n2 = (ref_p1 - ref_p0).cross(ref_p3 - ref_p0);
        double dbl_A1 = ref_n1.norm(),
               dbl_A2 = ref_n2.norm();
        h_bar = (dbl_A1 + dbl_A2) / (6.0 * e_bar_len); // 1/6 (h1 + h2) = 1/6 (b * h1 + b * h2) / b = 1/6(2 A1 + 2 A2) / b

        // Note: n1, n2 needn't be normalized since atan2 is invariant to uniform scaling of its arguments.
        theta_bar = atan2(ref_n2.cross(ref_n1).dot(e), ref_n1.dot(ref_n2)); // Note: can't use std::atan2 since this breaks ADL for autodiff types

        setDeformedConfiguration(ref_p0, ref_p1, ref_p2, ref_p3);
    }

    void setDeformedConfiguration(Eigen::Ref<const Pt> p0,
                                  Eigen::Ref<const Pt> p1,
                                  Eigen::Ref<const Pt> p2,
                                  Eigen::Ref<const Pt> p3) {
        deformed_pts.col(0) = p0;
        deformed_pts.col(1) = p1;
        deformed_pts.col(2) = p2;
        deformed_pts.col(3) = p3;

        Vec e = p1 - p0;
        e_len = e.norm();
        e /= e_len;

        N1 = (p2 - p0).cross(p1 - p0),
        N2 = (p1 - p0).cross(p3 - p0);

        squared_dbl_A1 = N1.squaredNorm();
        squared_dbl_A2 = N2.squaredNorm();

        // Note: n1, n2 needn't be normalized since atan2 is invariant to uniform scaling of its arguments.
        theta = atan2(N2.cross(N1).dot(e), N1.dot(N2)); // Note: can't use std::atan2 since this breaks ADL for autodiff types

        e_01_dot_ehat = e.dot(p1 - p2);
        e_02_dot_ehat = e.dot(p1 - p3); // really the negation of e_02 based on the labeling in the derivation figure...

        e_11_dot_ehat = e.dot(p2 - p0);
        e_12_dot_ehat = e.dot(p3 - p0);

        // Effectively disable this hinge's energy in degenerate configurations since
        // these will introduce large and pseudorandom values into the gradient and Hessian,
        // breaking the optimization.
        if ((e_len < 1e-9) || (squared_dbl_A1 < 1e-16) || (squared_dbl_A2 < 1e-16)) {
            e.setZero();
            e[0] = 1.0;
            theta = theta_bar;
            squared_dbl_A1 = 1.0;
            squared_dbl_A2 = 1.0;
            N1.setZero();
            N2.setZero();
            e_01_dot_ehat = e_02_dot_ehat = e_11_dot_ehat = e_12_dot_ehat = 0.0;
        }
    }

    // Gradient of theta with respect to the matrix [p0 | p1 | p2 | p3].
    Eigen::Matrix<Real, 3, 4> gradTheta() const {
        Eigen::Matrix<Real, 3, 4> result;
        result.col(0) = (e_01_dot_ehat / squared_dbl_A1) * N1 + (e_02_dot_ehat / squared_dbl_A2) * N2;
        result.col(1) = (e_11_dot_ehat / squared_dbl_A1) * N1 + (e_12_dot_ehat / squared_dbl_A2) * N2;
        result.col(2) = (-e_len / squared_dbl_A1) * N1;
        result.col(3) = (-e_len / squared_dbl_A2) * N2;
        return result;
    }

    using HessType = Eigen::Matrix<Real, 12, 12>;
    HessType hessTheta() const {
        HessType result;
        using ADType = Eigen::AutoDiffScalar<Eigen::Matrix<Real, 12, 1>>;
        HingeEnergy<ADType> diff_he(*this);

        Eigen::Matrix<ADType, 3, 4> ad_deformed_pts = deformed_pts;

        for (size_t j = 0; j < 12; ++j) {
            ad_deformed_pts.data()[j].derivatives().setZero();
            ad_deformed_pts.data()[j].derivatives()[j] = 1.0;
        }

        diff_he.setDeformedConfiguration(ad_deformed_pts.col(0),
                                         ad_deformed_pts.col(1),
                                         ad_deformed_pts.col(2),
                                         ad_deformed_pts.col(3));
        auto diff_g = diff_he.gradTheta();

        for (size_t i = 0; i < 12; ++i)
            result.row(i) = diff_g.data()[i].derivatives().transpose();

        return result;
    }

    Real energy() const {
        // Note: this is 1/2 the energy in [Grinspun 2003]
        return 0.5 * (theta - theta_bar) * (theta - theta_bar) * e_bar_len / h_bar;
    }

    Eigen::Matrix<Real, 3, 4> gradient() const {
        return ((theta - theta_bar) * e_bar_len / h_bar) * gradTheta();
    }

    HessType hessian() const {
        auto g = gradTheta();
        auto gFlattened = Eigen::Map<Eigen::Matrix<Real, 12, 1>>(g.data());
        return (e_bar_len / h_bar) * (
                gFlattened * gFlattened.transpose() +
                (theta - theta_bar) * hessTheta());
    }
};

#endif /* end of include guard: BENDINGENERGY_HH */


#pragma once

#include "calotypes/KernelDensityEstimation.hpp"
#include "calotypes/WeightedSamplers.hpp"
#include "calotypes/DataSelector.hpp"

#include <boost/random/random_device.hpp>
#include <boost/random/mersenne_twister.hpp>

namespace calotypes
{

// TODO Possibly expose ability to seed the engine directly, or give an engine reference?
/*! \brief Performs importance resampling to redraw samples from a set drawn according
 * to a proposal distribution to approximate a target distribution. */
template < class Data,
		   class Engine = boost::random::mt19937,
		   class Resampler = LowVarianceWeightedSampling<Engine> >
void ImportanceResample( const std::vector<Data>& samples,
						 const typename ProbabilityDensityFunction<Data>::Ptr& proposal,
						 const typename ProbabilityDensityFunction<Data>::Ptr& target,
						 unsigned int numSamples, std::vector<Data>& resampled )
{
	// 1. Calculate resampling weights according to target(x)/proposal(x)
	std::vector<double> weights( samples.size() );
// 	std::cout << "weights: " << std::endl;
	for( unsigned int i = 0; i < samples.size(); i++ )
	{
		weights[i] = (*target)( samples[i] ) / (*proposal)( samples[i] );
// 		std::cout << "\t " << samples[i].name << " (" << weights[i] << ")" << std::endl;
	}

	// 2. Resample based on the weights
	Engine engine;
	boost::random::random_device rng;
	engine.seed( rng );
	std::vector<unsigned int> resampleIndices;
	Resampler::Sample( weights, numSamples, resampleIndices, engine );

	// 3. Return the samples
	// TODO Verify that numSamples = resampleIndices.size()?
	resampled.resize( numSamples );
	for( unsigned int i = 0; i < numSamples; i++ )
	{
		resampled[i] = samples[ resampleIndices[i] ];
	}
}

template <class Data>
class ImportanceDataSelector
: public DataSelector<Data>
{
public:

	typedef std::shared_ptr<ImportanceDataSelector> Ptr;
	typedef std::vector<Data> Dataset;

	ImportanceDataSelector( const typename ProbabilityDensityFunction<Data>::Ptr& prop,
							const typename ProbabilityDensityFunction<Data>::Ptr& tar )
	: proposal( prop ), target( tar ) {}

	virtual void SelectData( const Dataset& data, unsigned int subsetSize, Dataset& subset )
	{
		ImportanceResample( data, proposal, target, subsetSize, subset );
	}

private:

	typename ProbabilityDensityFunction<Data>::Ptr proposal;
	typename ProbabilityDensityFunction<Data>::Ptr target;
};

} // end namespace calotypes


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/camera/projection_matrix_utils.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <glog/logging.h>

#include "theia/math/matrix/rq_decomposition.h"
#include "theia/sfm/pose/util.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

void IntrinsicsToCalibrationMatrix(const double focal_length,
                                   const double skew,
                                   const double aspect_ratio,
                                   const double principal_point_x,
                                   const double principal_point_y,
                                   Matrix3d* calibration_matrix) {
  *calibration_matrix <<
      focal_length, skew, principal_point_x,
      0, focal_length * aspect_ratio, principal_point_y,
      0, 0, 1.0;
}

void CalibrationMatrixToIntrinsics(const Matrix3d& calibration_matrix,
                                   double* focal_length,
                                   double* skew,
                                   double* aspect_ratio,
                                   double* principal_point_x,
                                   double* principal_point_y) {
  CHECK_NE(calibration_matrix(2, 2), 0);
  *focal_length = calibration_matrix(0, 0) / calibration_matrix(2, 2);
  *skew = calibration_matrix(0, 1) / calibration_matrix(2, 2);
  *aspect_ratio = calibration_matrix(1, 1) / calibration_matrix(0, 0);
  *principal_point_x = calibration_matrix(0, 2) / calibration_matrix(2, 2);
  *principal_point_y = calibration_matrix(1, 2) / calibration_matrix(2, 2);
}

bool DecomposeProjectionMatrix(const Matrix3x4d pmatrix,
                               Matrix3d* calibration_matrix,
                               Vector3d* rotation,
                               Vector3d* position) {
  RQDecomposition<Matrix3d> rq(pmatrix.block<3, 3>(0, 0));

  Matrix3d rotation_matrix = ProjectToRotationMatrix(rq.matrixQ());

  const double k_det = rq.matrixR().determinant();
  if (k_det == 0) {
    return false;
  }

  Matrix3d& kmatrix = *calibration_matrix;
  if (k_det > 0) {
    kmatrix = rq.matrixR();
  } else {
    kmatrix = -rq.matrixR();
  }

  // Fix the matrix such that all internal parameters are greater than 0.
  for (int i = 0; i < 3; ++i) {
    if (kmatrix(i, i) < 0) {
      kmatrix.col(i) *= -1.0;
      rotation_matrix.row(i) *= -1.0;
    }
  }

  // Solve for t.
  const Vector3d t =
      kmatrix.triangularView<Eigen::Upper>().solve(pmatrix.col(3));

  // c = - R' * t, and flip the sign according to k_det;
  if (k_det > 0) {
    *position = - rotation_matrix.transpose() * t;
  } else {
    *position = rotation_matrix.transpose() * t;
  }

  const Eigen::AngleAxisd rotation_aa(rotation_matrix);
  *rotation = rotation_aa.angle() * rotation_aa.axis();

  return true;
}

bool ComposeProjectionMatrix(const Matrix3d& calibration_matrix,
                             const Vector3d& rotation,
                             const Vector3d& position,
                             Matrix3x4d* pmatrix) {
  const double rotation_angle = rotation.norm();
  if (rotation_angle == 0) {
    pmatrix->block<3, 3>(0, 0) = Matrix3d::Identity();
  } else {
    pmatrix->block<3, 3>(0, 0) = Eigen::AngleAxisd(
        rotation_angle, rotation / rotation_angle).toRotationMatrix();
  }

  pmatrix->col(3) = - (pmatrix->block<3, 3>(0, 0) *  position);
  *pmatrix = calibration_matrix * (*pmatrix);
  return true;
}

}  // namespace theia


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_LOG_2_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_LOG_2_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant Log_2 : \f$\log(2)\f$.

    @par Semantic:

    @code
    T r = Log_2<T>();
    @endcode

    is similar to:

    @code
      r =  T(0.6931471805599453094172321214581765680755001343602553);
    @endcode

**/
  template<typename T> T Log_2();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant Log_2. (\f$\log(2)\f$)

      Generate the  constant log_2.

      @return The Log_2 constant for the proper type
    **/
    const boost::dispatch::functor<tag::log_2_> log_2 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/log_2.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


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

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

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

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

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

//
// *** System
//
#include <iostream>

//
// *** Boost
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>

//
// *** ViennaCL
//

// #define VIENNACL_DEBUG_ALL
// #define VIENNACL_DEBUG_BUILD
// #define VIENNACL_HAVE_UBLAS 1
// #define VIENNACL_DEBUG_CUSTOM_OPERATION
#include "viennacl/vector.hpp"
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/linalg/norm_1.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/norm_inf.hpp"
#include "viennacl/generator/custom_operation.hpp"

using namespace boost::numeric;

template <class TYPE>
bool readVectorFromFile ( const std::string & filename, boost::numeric::ublas::vector<TYPE> & vec ) {
    std::ifstream file ( filename.c_str() );

    if ( !file ) return false;

    unsigned int size;
    file >> size;

    if ( size > 20000 )  //keep execution times short
        size = 20000;
    vec.resize ( size );
    for ( unsigned int i = 0; i < size; ++i ) {
        TYPE element;
        file >> element;
        vec[i] = element;
    }

    return true;
}

template <typename ScalarType>
ScalarType diff ( ScalarType & s1, viennacl::scalar<ScalarType> & s2 )
{
    viennacl::backend::finish();  //workaround for a bug in APP SDK 2.7 on Trinity APUs (with Catalyst 12.8)
    if ( s1 != s2 )
        return ( s1 - s2 ) / std::max ( fabs ( s1 ), fabs ( s2 ) );
    return 0;
}

template< typename NumericT,unsigned int Alignment, typename Epsilon >
int test ( Epsilon const& epsilon, std::string vecfile ) {
    int retval = EXIT_SUCCESS;

    viennacl::scalar<NumericT>  vcl_res ( 0 );
    ublas::vector<NumericT> vec;
    ublas::vector<NumericT> vec2;

    NumericT res;

    viennacl::generator::gpu_symbolic_scalar<0,NumericT> symres;
    viennacl::generator::symbolic_vector<1,NumericT,Alignment> symv;
    viennacl::generator::symbolic_vector<2,NumericT,Alignment> symv2;
    viennacl::generator::cpu_symbolic_scalar<3,NumericT> symscal;
    viennacl::generator::cpu_symbolic_scalar<2,NumericT> symscal2;

    if ( !readVectorFromFile<NumericT> ( vecfile, vec ) ) {
        std::cout << "Error reading vec file" << std::endl;
        retval = EXIT_FAILURE;
    }
//
    std::cout << "Running tests for vector of size " << vec.size() << std::endl;
	std::cout << "----- Alignment " << Alignment << " -----" << std::endl;
//
    viennacl::vector<NumericT,Alignment> vcl_vec ( vec.size() );
    viennacl::vector<NumericT,Alignment> vcl_vec2 ( vec.size() );
//
    vec2 = vec;
    viennacl::copy ( vec.begin(), vec.end(), vcl_vec.begin() );
    viennacl::copy ( vec2.begin(), vec2.end(), vcl_vec2.begin() );

//     --------------------------------------------------------------------------

    std::cout << "testing inner product..." << std::endl;

    res = ublas::inner_prod ( vec, vec2 );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation(symres = inner_prod ( symv, symv2 ), "inner_prod") ( vcl_res, vcl_vec, vcl_vec2 ) );
    //std::cout << viennacl::generator::custom_operation(symres = inner_prod ( symv, symv2 ), "inner_prod") .kernels_source_code() << std::endl;
    if ( fabs ( diff ( res, vcl_res ) ) > epsilon ) {
        std::cout << "# Error at operation: inner product" << std::endl;
        std::cout << "  Diff " << fabs ( diff ( res, vcl_res ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    std::cout << "testing inner product division..." << std::endl;
    res = ublas::inner_prod ( vec, vec2 ) /ublas::inner_prod ( vec, vec );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symres = inner_prod ( symv, symv2 ) /inner_prod ( symv,symv ), "inner_prod_division" ) ( vcl_res, vcl_vec, vcl_vec2 ) );
    if ( fabs ( diff ( res, vcl_res ) ) > epsilon ) {
        std::cout << "# Error at operation: inner_prod_division" << std::endl;
        std::cout << "  diff: " << fabs ( diff ( res, vcl_res ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    std::cout << "testing scalar / inner product..." << std::endl;
    res = 4/ublas::inner_prod ( vec, vec );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symres = symscal2/inner_prod ( symv,symv ),"scalar_division" ) ( vcl_res, vcl_vec, 4.0f ) );
    //std::cout << viennacl::generator::custom_operation ( symres = symscal2/inner_prod ( symv,symv ), "scalar_division" ).kernels_source_code() << std::endl;
    if ( fabs ( diff ( res, vcl_res ) ) > epsilon ) {
        std::cout << "# Error at operation: scalar over inner product" << std::endl;
        std::cout << "  diff: " << fabs ( diff ( res, vcl_res ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    std::cout << "testing inner_prod - ( scal - inner_prod ) " << std::endl;
    res = ublas::inner_prod ( vec, vec2 ) - ( 5.0f - inner_prod ( vec,vec2 ) );
    viennacl::ocl::enqueue ( viennacl::generator::custom_operation ( symres = inner_prod ( symv, symv2 ) - ( symscal - inner_prod ( symv,symv2 ) ), "inner_prod_minus_scal_minus_inprod" ) ( vcl_res, vcl_vec, vcl_vec2, 5.0f ) );
    if ( fabs ( diff ( res, vcl_res ) ) > epsilon ) {
        std::cout << "# Error at operation: inner_prod minus ( scal minus inner_prod ) " << std::endl;
        std::cout << "  diff: " << fabs ( diff ( res, vcl_res ) ) << std::endl;
        retval = EXIT_FAILURE;
    }

    return retval;
}

int main() {
    std::cout << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "## Test :: Inner Product" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << std::endl;

    int retval = EXIT_SUCCESS;

    std::string vecfile ( "../examples/testdata/rhs65025.txt" );

    std::cout << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << std::endl;
    {
        typedef float NumericT;
        NumericT epsilon = 1.0E-4;
        std::cout << "# Testing setup:" << std::endl;
        std::cout << "  eps:     " << epsilon << std::endl;
        std::cout << "  numeric: float" << std::endl;
        retval = test<NumericT,1> ( epsilon, vecfile );
//  		retval = test<NumericT,4> ( epsilon, vecfile, resultfile );
//        retval = test<NumericT,16> ( epsilon, vecfile, resultfile );
        if ( retval == EXIT_SUCCESS )
            std::cout << "# Test passed" << std::endl;
        else
            return retval;
    }
}