/*
 * Copyright (c) 2013-2019 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_MAFFINE2_HPP
#define ODE_MAFFINE2_HPP

// ODE using Affine and Mean Value Form (fast)

#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/psa.hpp>
#include <kv/affine.hpp>
#include <kv/ode.hpp>
#include <kv/ode-autodif.hpp>
#include <kv/ode-param.hpp>
#include <kv/ode-callback.hpp>

#ifndef ODE_FAST
#define ODE_FAST 1
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
void
ode_onlytype1(F f, ub::vector< interval<T> >& init, const interval<T>& start, const interval<T>& end, int order) {
	int n = init.size();
	int i, j;

	ub::vector< psa< interval<T> > > x, y;
	psa< interval<T> > torg;
	psa< interval<T> > t;

	ub::vector< interval<T> > result;

	interval<T> deltat;

	bool save_mode, save_uh, save_rh;

	x = init;
	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa< interval<T> >::mode();
	save_uh = psa< interval<T> >::use_history();
	save_rh = psa< interval<T> >::record_history();
	psa< interval<T> >::mode() = 1;
	psa< interval<T> >::use_history() = false;
	psa< interval<T> >::record_history() = false;
	#if ODE_FAST == 1
	psa< interval<T> >::record_history() = true;
	psa< interval<T> >::history().clear();
	#endif
	for (j=0; j<order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa< interval<T> >::use_history() = true;
		if (j == order - 1) psa< interval<T> >::record_history() = false;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) y(i) = integrate(y(i));
		x = init + y;
	}

	deltat = end - start;

	result.resize(n);
	for (i=0; i<n; i++) {
		result(i) = eval(x(i), deltat);
	}

	init = result;

	psa< interval<T> >::mode() = save_mode;
	psa< interval<T> >::use_history() = save_uh;
	psa< interval<T> >::record_history() = save_rh;
}

template <class T, class F>
void
ode_onlytype1(F f, ub::vector< autodif< interval<T> > >& init, const interval<T>& start, const interval<T>& end, int order) {
	int n = init.size();
	int i, j, k;

	ub::vector< psa< autodif< interval<T> > > > x, y;
	psa< autodif< interval<T> > > torg;
	psa< autodif< interval<T> > > t;

	ub::vector< autodif< interval<T> > > result;

	interval<T> deltat;

	bool save_mode, save_uh, save_rh;

	x = init;

	torg.v.resize(2);
	torg.v(0) = start; torg.v(1) = 1.;

	save_mode = psa< autodif< interval<T> > >::mode();
	save_uh = psa< autodif< interval<T> > >::use_history();
	save_rh = psa< autodif< interval<T> > >::record_history();
	psa< autodif< interval<T> > >::mode() = 1;
	psa< autodif< interval<T> > >::use_history() = false;
	psa< autodif< interval<T> > >::record_history() = false;
	#if ODE_FAST == 1
	psa< autodif< interval<T> > >::record_history() = true;
	psa< autodif< interval<T> > >::history().clear();
	#endif
	for (j=0; j<order; j++) {
		#if ODE_FAST == 1
		if (j == 1) psa< autodif< interval<T> > >::use_history() = true;
		if (j == order - 1) psa< autodif< interval<T> > >::record_history() = false;
		#endif
		t = setorder(torg, j);
		y = f(x, t);
		for (i=0; i<n; i++) y(i) = integrate(y(i));
		x = init + y;
	}

	deltat = end - start;

	result.resize(n);
	for (i=0; i<n; i++) {
		result(i) = eval(x(i), (autodif< interval<T> >)deltat);
	}

	init = result;

	psa< autodif< interval<T> > >::mode() = save_mode;
	psa< autodif< interval<T> > >::use_history() = save_uh;
	psa< autodif< interval<T> > >::record_history() = save_rh;
}

template <class T, class F>
int
ode_maffine2(F f, ub::vector< affine<T> >& init, const interval<T>& start, interval<T>& end, ode_param<T> p = ode_param<T>(), ub::vector< psa< interval<T> > >* result_psa = NULL)
{
	int n = init.size();
	int i, j;

	ub::vector< interval<T> > c;
	ub::vector< interval<T> > fc;
	ub::vector< interval<T> > I, Idummy, I2;
	ub::vector< autodif< interval<T> > > Iad;

	ub::vector< interval<T> > result_i;
	ub::matrix< interval<T> > result_d;

	ub::vector< affine<T> > result;

	int maxnum_save;

	interval<T> deltat_n;
	ub::vector< psa< interval<T> > > psa_result;

	int r;

	interval<T> end2 = end;

	I.resize(n);
	c.resize(n);
	for (i=0; i<n; i++) {
		I(i) = to_interval(init(i));
		c(i) = mid(I(i));
	}

	Idummy = I;
	r = ode(f, Idummy, start, end2, p, &psa_result);
	if (r == 0) return 0;

	if (result_psa != NULL) {
		*result_psa = psa_result;
	}

	deltat_n = pow(end2 - start, p.order);

	I2.resize(n);
	for (i=0; i<n; i++) {
		I2(i) = psa_result(i).v(p.order) * deltat_n;
	}

	Iad = autodif< interval<T> >::init(I);
	// NOTICE: below must be autodif version
	ode_onlytype1(f, Iad, start, end2, p.order-1);

	fc = c;
	ode_onlytype1(f, fc, start, end2, p.order-1);

	autodif< interval<T> >::split(Iad, result_i, result_d);

	if (p.ep_reduce == 0) {
		maxnum_save = affine<T>::maxnum();
	}

	result = I2 + fc + prod(result_d, init - c);

	if (p.ep_reduce == 0) {
		epsilon_reduce2(result, maxnum_save);
	} else {
		epsilon_reduce(result, p.ep_reduce, p.ep_reduce_limit);
	}

	init = result;
	if (r == 1) end = end2;

	return r;
}

template <class T, class F>
int
odelong_maffine2(
	F f,
	ub::vector< affine<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	ub::vector< affine<T> > x, x1;
	interval<T> t, t1;
	int ret_ode;
	int ret_val = 0;
	bool ret_callback;

	ub::vector< psa< interval<T> > > result_tmp;

	x = init;
	t = start;
	p.set_autostep(true);

	while (1) {
		x1 = x;
		t1 = end;

		ret_ode = ode_maffine2(f, x1, t, t1, p, &result_tmp);
		if (ret_ode == 0) {
			if (ret_val == 1) {
				init = x1;
				end = t;
			}
			return ret_val;
		}
		ret_val = 1;
		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << to_interval(x1) << "\n";
		}

		ret_callback = callback(t, t1, to_interval(x), to_interval(x1), result_tmp);

		if (ret_callback == false) {
			init = x1;
			end = t1;
			return 3;
		}

		if (ret_ode == 2) {
			init = x1;
			return 2;
		}

		t = t1;
		x = x1;
	}
}

template <class T, class F>
int
odelong_maffine2(
	F f,
	ub::vector< interval<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	int i;
	ub::vector< affine<T> > x;
	int maxnum_save;
	int r;

	maxnum_save = affine<T>::maxnum();
	affine<T>::maxnum() = 0;
	x = init;

	r = odelong_maffine2(f, x, start, end, p, callback);

	affine<T>::maxnum() = maxnum_save;

	if (r == 0) return 0;

	for (i=0; i<s; i++) init(i) = to_interval(x(i));

	return r;
}

} // namespace kv

#endif // ODE_MAFFINE2_HPP


/*******************************************************************************
 * Copyright (c) 2014, 2015  IBM Corporation and others
 *
 * 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.
 *******************************************************************************/

#ifndef GaussianProcess_hpp
#define GaussianProcess_hpp

#include <stdio.h>
#include <iostream>
#include <vector>
#include <memory>
#include <complex>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>

#include "KernelFunction.hpp"
#include "MathUtils.hpp"

namespace loc{

    class GaussianProcessParameterSet{
    public:
        std::vector<double> sigmaFs{1,2,3,5};
        std::vector<double> lengthes{1,2,3,4,5,7,9};
        std::vector<double> lengthFloors{0.01};
        std::vector<double> sigmaNs{1};
    };

    class GaussianProcessParameters{
    public:
        GaussianKernel::Parameters gaussianKernelParameters;
        double sigmaN;
    };

    class GaussianProcess{

    private:
        // variables to be serialized
        ////std::shared_ptr<KernelFunction> mKernel;
        GaussianKernel mGaussianKernel;
        Eigen::MatrixXd X_;
        Eigen::MatrixXd Weights_;
        double sigmaN_ = 1.0;

        // variables not to be serialized
        Eigen::MatrixXd Y_;
        Eigen::MatrixXd K_;
        Eigen::MatrixXd Ky_;
        Eigen::MatrixXd invKy_;
        Eigen::MatrixXd Actives_;
        GaussianProcessParameterSet mParameterSet;

    public:
        // A function for serealization
        template<class Archive>
        void serialize(Archive& ar);

        virtual GaussianProcess& sigmaN(double sigmaN);
        virtual double sigmaN() const;
        /*
        GaussianProcess& kernel(std::shared_ptr<KernelFunction> kernel){
            mKernel = kernel;
            return *this;
        }
        */
        virtual GaussianProcess& gaussianProcessParameterSet(const GaussianProcessParameterSet&);
        virtual GaussianProcess& gaussianKernel(GaussianKernel gaussianKernel);
        virtual GaussianKernel gaussianKernel() const;

        virtual Eigen::MatrixXd X() const;
        virtual Eigen::MatrixXd Y() const;
        virtual GaussianProcess& fit(const Eigen::MatrixXd & X, const Eigen::MatrixXd& Y);
        virtual GaussianProcess& fit(const Eigen::MatrixXd & X, const Eigen::MatrixXd& Y, const Eigen::MatrixXd& Actives);
        virtual GaussianProcess& actives(const Eigen::MatrixXd& Actives);

        virtual Eigen::MatrixXd computeKernelMatrix(const Eigen::MatrixXd& X);
        virtual Eigen::VectorXd computeKstar(double x[]) const;

        virtual Eigen::VectorXd predict(double x[]) const;
        virtual Eigen::VectorXd predict(const Eigen::VectorXd& kstar) const;

        virtual double predict(double x[], int index);
        virtual std::vector<double> predict(double x[], const std::vector<int>& indices) const;
        virtual std::vector<double> predict(const Eigen::VectorXd& kstar, const std::vector<int>& indices) const;
        virtual Eigen::VectorXd predictVarianceF(double x[]) const;
        virtual Eigen::VectorXd predictVarianceF(const Eigen::VectorXd& kstar) const;

        virtual double computeLogLikelihood(double x[], const Eigen::VectorXd& y) const;
        virtual double marginalLogLikelihood();
        virtual double predictiveLogLikelihood();
        virtual double leaveOneOutMSE();

        virtual std::vector<GaussianProcessParameters> createParameterMatrix(const GaussianProcessParameterSet&) const;
        virtual void fitCV(const Eigen::MatrixXd & X, const Eigen::MatrixXd& Y, const Eigen::MatrixXd& Actives);
    };
}

#endif /* GaussianProcess_hpp */


/*************************************************************************
	> File Name: main.cpp
	> Author: TAI Lei
	> Mail: ltai@ust.hk
	> Created Time: Thu Mar  7 19:39:14 2019
 ************************************************************************/

#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <array>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Eigen>
#include "csv_reader.h"
#include "motion_model.h"
#include "trajectory_optimizer.h"

#define L 1.0
#define DS 0.1
#define CONST_V 3.0  // use a const linear velocity here
typedef std::vector<std::vector<float>> Table;

using namespace cpprobotics;

StateList sample_states(std::vector<float> angle_samples,
                        float a_min, float a_max,
                        int d, float p_max, float p_min, int nh){
  StateList states;
  for(float item:angle_samples){
    float a = a_min + ( a_max - a_min ) * item;
    for(int j=0; j<nh; j++){
      float xf = d * std::cos(a);
      float yf = d * std::sin(a);
      float yawf;
      if(nh == 1) yawf = (p_max - p_min)/2.0 + a;
      else yawf = p_min + (p_max - p_min) * j /(nh-1) + a;
      states.push_back(TrajState(xf, yf, yawf));
    }
  }
  return states;
};

StateList calc_uniform_polar_states(int nxy, int nh, int d,
                                    float a_min, float a_max,
                                    float p_min, float p_max){
  std::vector<float> angle_samples;
  for(int i=0; i<nxy; i++){
    angle_samples.push_back(i*1.0/(nxy-1));
  }
  StateList states = sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh);
  return states;
};

StateList calc_biased_polar_states(float goal_angle, int ns, int nxy,
                                   int nh, int d,
                                   float a_min, float a_max,
                                   float p_min, float p_max){
  std::vector<float> asi;
  std::vector<float> cnav;
  float cnav_max = std::numeric_limits<float>::min();
  float cnav_sum = 0;
  for(int i=0; i<ns-1; i++){
    float asi_sample = a_min + (a_max - a_min)*i/(ns-1);
    asi.push_back(asi_sample);
    float cnav_sample = M_PI - std::abs(asi_sample - goal_angle);
    cnav.push_back(cnav_sample);
    cnav_sum += cnav_sample;
    if (cnav_max < cnav_sample){
      cnav_max = cnav_sample;
    }
  }

  std::vector<float> csumnav;
  float cum_temp = 0;
  for(int i=0; i<ns-1; i++){
    cnav[i] = (cnav_max - cnav[i]) / (cnav_max * ns - cnav_sum);
    cum_temp += cnav[i];
    csumnav.push_back(cum_temp);
  }

  int li = 0;
  std::vector<float> angle_samples;
  for(int i=0; i<nxy; i++){
    for(int j=li; j<ns-1; j++){
      if (j*1.0/ns >= i*1.0/(nxy -1)){
        angle_samples.push_back(csumnav[j]);
        li = j - 1;
        break;
      }
    }
  }

  StateList states = sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh);
  return states;
};

StateList calc_lane_states(float l_center, float l_heading, float l_width, float v_width, float d, int nxy){
  float xc = std::cos(l_heading) * d + std::sin(l_heading) * l_center;
  float yc = std::sin(l_heading) * d + std::cos(l_heading) * l_center;

  StateList states;
  for(int i=0; i<nxy; i++){
    float delta = -0.5 * (l_width - v_width) + (l_width - v_width) * i / (nxy -1);
    float xf = xc - delta * std::sin(l_heading);
    float yf = yc + delta * std::cos(l_heading);
    states.push_back(TrajState(xf, yf, l_heading));
  }
  return states;
}

Parameter search_nearest_one_from_lookuptable(TrajState target, Table csv_file){

    float min_d = std::numeric_limits<float>::max();
    int min_id = -1;

    for(unsigned int i=0; i<csv_file.size(); i++)
    {
      float dx = target.x - csv_file[i][0];
      float dy = target.y - csv_file[i][1];
      float dyaw = target.yaw - csv_file[i][2];
      float d = std::sqrt(dx * dx + dy * dy + dyaw * dyaw);

      if ( d<min_d ){
        min_id = i;
        min_d = d;
      }
    }
    Parameter best_p(std::sqrt(target.x * target.x + target.y * target.y),
        {{0, csv_file[min_id][4], csv_file[min_id][5]}});
    return best_p;
}

std::vector<Traj> generate_path(StateList states, Table csv_file, float k0=0.0){
  std::vector<Traj> traj_list;
  for(TrajState state:states){
    Parameter   p = search_nearest_one_from_lookuptable(state, csv_file);
    p.steering_sequence[0] = k0;

    // default settings for this scenario
    State init_state(0, 0, 0, CONST_V);
    MotionModel m_model(L, DS, init_state);
    float cost_th_ = 0.1;
    std::vector<float> h_step_{0.5, 0.02, 0.02};
    int max_iter = 100;

    TrajectoryOptimizer traj_opti_obj(m_model, p, state);
    Traj traj = traj_opti_obj.optimizer_traj(max_iter, cost_th_, h_step_, true, true);
    traj_list.push_back(traj);
  }
  return traj_list;
};

std::vector<Traj> uniform_terminal_state_sample_test(Table csv_file){
  float k0 = 0.0;
  int nxy = 5;  // number of position sampling
  int nh = 3;  // number of heading sampling
  int d = 20; // distance to target
  float a_min = -45.0/180 * M_PI; // position sampling min angle
  float a_max = +45.0/180 * M_PI; // position sampling max angle
  float p_min = -45.0/180 * M_PI; // heading sampling min angle
  float p_max = +45.0/180 * M_PI; // heading sampling max angle

  StateList states = calc_uniform_polar_states(nxy, nh, d,
                                               a_min, a_max,
                                               p_min, p_max);

  std::vector<Traj> traj_list = generate_path(states, csv_file, k0);
  return traj_list;
};

std::vector<Traj> biased_terminal_state_sample_test(Table csv_file){
  float k0 = 0.0;
  int nxy = 30;  // number of position sampling
  int nh = 2;  // number of heading sampling
  int d = 20; // distance to target
  float a_min = -45.0/180 * M_PI; // position sampling min angle
  float a_max = +45.0/180 * M_PI; // position sampling max angle
  float p_min = -20.0/180 * M_PI; // heading sampling min angle
  float p_max = +20.0/180 * M_PI; // heading sampling max angle

  int ns = 100;
  float goal_angle = 0.0;
  StateList states = calc_biased_polar_states(goal_angle, ns,
                                              nxy, nh, d,
                                              a_min, a_max,
                                              p_min, p_max);

  std::vector<Traj> traj_list = generate_path(states, csv_file, k0);
  return traj_list;
};

std::vector<Traj> lane_state_sample_test(Table csv_file){
  float k0 = 0.0;
  float l_center = 10.0;
  float l_heading = 90.0/180.0 * M_PI;
  float l_width = 3.0;
  float v_width = 1.0;
  int d = 10;
  int nxy = 5;

  StateList states = calc_lane_states(l_center, l_heading, l_width,
                                      v_width, d, nxy);

  std::vector<Traj> traj_list = generate_path(states, csv_file, k0);
  return traj_list;
};

int main(){
  //uniform_terminal_state_sample_test1();
    std::vector<std::vector<float>> lookup_table;

    std::ifstream file("../../lookuptable.csv");
    CSVIterator loop(file);
    loop++;
    for(; loop != CSVIterator(); ++loop)
    {
      std::vector<float> temp;
      for(int i=0; i<6; i++){
        temp.push_back(std::stod((*loop)[i]));
      }
      lookup_table.push_back(temp);
    }
    std::vector<Traj> traj_list1 = uniform_terminal_state_sample_test(lookup_table);
    std::vector<Traj> traj_list2 = biased_terminal_state_sample_test(lookup_table);
    std::vector<Traj> traj_list3 = lane_state_sample_test(lookup_table);

};


//  To use the simple FFT implementation
//  g++ -o demofft -I.. -Wall -O3 FFT.cpp

//  To use the FFTW implementation
//  g++ -o demofft -I.. -DUSE_FFTW -Wall -O3 FFT.cpp -lfftw3 -lfftw3f -lfftw3l

#ifdef USE_FFTW
#include <fftw3.h>
#endif

#include <vector>
#include <complex>
#include <algorithm>
#include <iterator>
#include <iostream>
#include <Eigen/Core>
#include <unsupported/Eigen/FFT>

using namespace std;
using namespace Eigen;

template <typename T> T mag2(T a) { return a * a; }
template <typename T> T mag2(std::complex<T> a) { return norm(a); }

template <typename T> T mag2(const std::vector<T> &vec) {
    T out = 0;
    for (size_t k = 0; k < vec.size(); ++k)
        out += mag2(vec[k]);
    return out;
}

template <typename T> T mag2(const std::vector<std::complex<T>> &vec) {
    T out = 0;
    for (size_t k = 0; k < vec.size(); ++k)
        out += mag2(vec[k]);
    return out;
}

template <typename T>
vector<T> operator-(const vector<T> &a, const vector<T> &b) {
    vector<T> c(a);
    for (size_t k = 0; k < b.size(); ++k)
        c[k] -= b[k];
    return c;
}

template <typename T> void RandomFill(std::vector<T> &vec) {
    for (size_t k = 0; k < vec.size(); ++k)
        vec[k] = T(rand()) / T(RAND_MAX) - .5;
}

template <typename T> void RandomFill(std::vector<std::complex<T>> &vec) {
    for (size_t k = 0; k < vec.size(); ++k)
        vec[k] = std::complex<T>(T(rand()) / T(RAND_MAX) - .5,
                                 T(rand()) / T(RAND_MAX) - .5);
}

template <typename T_time, typename T_freq> void fwd_inv(size_t nfft) {
    typedef typename NumTraits<T_freq>::Real Scalar;
    vector<T_time> timebuf(nfft);
    RandomFill(timebuf);

    vector<T_freq> freqbuf;
    static FFT<Scalar> fft;
    fft.fwd(freqbuf, timebuf);

    vector<T_time> timebuf2;
    fft.inv(timebuf2, freqbuf);

    long double rmse = mag2(timebuf - timebuf2) / mag2(timebuf);
    cout << "roundtrip rmse: " << rmse << endl;
}

template <typename T_scalar> void two_demos(int nfft) {
    cout << "     scalar ";
    fwd_inv<T_scalar, std::complex<T_scalar>>(nfft);
    cout << "    complex ";
    fwd_inv<std::complex<T_scalar>, std::complex<T_scalar>>(nfft);
}

void demo_all_types(int nfft) {
    cout << "nfft=" << nfft << endl;
    cout << "   float" << endl;
    two_demos<float>(nfft);
    cout << "   double" << endl;
    two_demos<double>(nfft);
    cout << "   long double" << endl;
    two_demos<long double>(nfft);
}

int main() {
    demo_all_types(2 * 3 * 4 * 5 * 7);
    demo_all_types(2 * 9 * 16 * 25);
    demo_all_types(1024);
    return 0;
}


// Copyright 2019 Xanadu Quantum Technologies Inc.

// 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.
/**
 * @file
 * Contains functions for computing the Torontonian using the algorithm described in
 * *A faster hafnian formula for complex matrices and its benchmarking
 * on the Titan supercomputer*, [arxiv:1805.12498](https://arxiv.org/abs/1805.12498)
 */
#pragma once
#include <stdafx.h>
#include <numeric>

#ifdef LAPACKE
#define EIGEN_SUPERLU_SUPPORT
#define EIGEN_USE_BLAS
#define EIGEN_USE_LAPACKE

#define LAPACK_COMPLEX_CUSTOM
#define lapack_complex_float std::complex<float>
#define lapack_complex_double std::complex<double>
#endif

#include <Eigen/Eigenvalues>
#include "fsum.hpp"

namespace libwalrus {
/**
 * Given a string of length `len`, finds the positions in which it has a 1
 * and stores its position i, as 2*i and 2*i+1 in consecutive slots
 * of the array pos.
 *
 * It also returns (twice) the number of ones in array dst
 *
 * @param dst character array representing binary digits.
 * @param len length of the array `dst`.
 * @param pos resulting character array of length `2*len` storing
 * the indices at which `dst` contains the values 1.
 * @return returns twice the number of ones in array `dst`.
 */
void find2T (char *dst, Byte len, Byte *pos, char offset)
{
    Byte j = offset - 1;

    for (Byte i = 0; i < len; i++) {
        if (1 == dst[i]) {
            pos[j] = len - i - 1;
            pos[j + offset] = 2 * len - i - 1;
            j--;
        }
    }
}

/**
 * Partial sum of a character array
 *
 * @param dst character array
 * @param m sum the first m characters
 *
 * @return the partial sum
 */
char sum(char *dst, Byte m) {
    char sum_tot = 0;
    for (int i = 0; i < m; i++) {
        sum_tot += (Byte)dst[i];
    }
    return sum_tot;
}

/**
 * Computes the Torontonian of an input matrix.
 *
 * If the output is NaN, that means that the input matrix does not have
 * a Torontonian with physical meaning.
 *
 * This function uses OpenMP (if available) to parallelize the reduction.
 *
 * @param mat flattened vector of size \f$n^2\f$, representing an \f$n\times n\f$
 *       row-ordered symmetric matrix.
 * @return Torontonian of the input matrix
 */
template <typename T>
inline T torontonian(std::vector<T> &mat) {
    int n = std::sqrt(static_cast<double>(mat.size()));
    Byte m = n / 2;
    unsigned long long int x = static_cast<unsigned long long int>(pow(2, m));

    namespace eg = Eigen;
    eg::Matrix<T, eg::Dynamic, eg::Dynamic> A = eg::Map<eg::Matrix<T, eg::Dynamic, eg::Dynamic>, eg::Unaligned>(mat.data(), n, n);

#ifdef _OPENMP
    int nthreads = omp_get_max_threads();
    omp_set_num_threads(nthreads);
#else
    int nthreads = 1;
#endif

    std::vector<unsigned long long int> threadbound_low(nthreads);
    std::vector<unsigned long long int> threadbound_hi(nthreads);

    for (int i = 0; i < nthreads; i++) {

        threadbound_low[i] = i * x / nthreads;
        threadbound_hi[i] = (i + 1) * x / nthreads;
    }

    std::vector<T> localsum(nthreads);

    #pragma omp parallel for shared(localsum)

    for (int ii = 0; ii < nthreads; ii++) {

        T netsum = static_cast<T>(0.0);
        for (unsigned long long int k = threadbound_low[ii]; k < threadbound_hi[ii]; k++) {

            unsigned long long int xx = k;
            char* dst = new char[m];

            dec2bin(dst, xx, m);
            char len = sum(dst, m);

            Byte* short_st = new Byte[2 * len];
            find2T(dst, m, short_st, len);
            delete [] dst;

            eg::Matrix<T, eg::Dynamic, eg::Dynamic> B;
            B.resize(2 * len, 2 * len);

            for (int i = 0; i < 2 * len; i++) {
                for (int j = 0; j < 2 * len; j++) {
                    B(i, j) = -A(short_st[i], short_st[j]);
                }
            }

            delete [] short_st;

            for (int i = 0; i < 2 * len; i++) {
                B(i, i) += static_cast<T>(1);
            }

            T det = std::real(B.determinant());

            if (len % 2 == 0) {
                netsum += static_cast<T>(1.0) / std::sqrt(det);
            }
            else {
                netsum -= static_cast<T>(1.0) / std::sqrt(det);
            }

        }

        localsum[ii] = netsum;

    }

    int n_local = localsum.size();
    T final = 0.0;
    T sign = 1.0;

    if (m % 2 != 0)
        sign = -1.0;

    for (int i = 0; i < n_local; i++) {
        final += localsum[i]    ;
    }

    return sign * final;
}

/**
 * Computes the Torontonian of an input matrix using the
 * [Shewchuck algorithm](https://github.com/achan001/fsum),
 * a significantly more [accurate summation algorithm](https://link.springer.com/article/10.1007%2FPL00009321).
 *
 * Note that the fsum implementation currently only allows for
 * double precision, and precludes use of OpenMP parallelization.
 *
 * Note: if the output is NaN, that means that the input matrix does not have
 * a Torontonian with physical meaning.
 *
 * @param mat flattened vector of size \f$n^2\f$, representing an \f$n\times n\f$
 *       row-ordered symmetric matrix.
 * @return Torontonian of the input matrix
 */
template <typename T>
inline double torontonian_fsum(std::vector<T> &mat) {
    // Here weinput the matrix from python. The variable n is the size of the matrix
    int n = std::sqrt(static_cast<double>(mat.size()));
    Byte m = n / 2;
    unsigned long long int x = static_cast<unsigned long long int>(pow(2, m));

    fsum::sc_partials netsum;

    namespace eg = Eigen;
    eg::Matrix<T, eg::Dynamic, eg::Dynamic> A = eg::Map<eg::Matrix<T, eg::Dynamic, eg::Dynamic>, eg::Unaligned>(mat.data(), n, n);

    for (int k = 0; k < x; k++) {
        unsigned long long int xx = k;
        char* dst = new char[m];

        dec2bin(dst, xx, m);
        char len = sum(dst, m);

        Byte* short_st = new Byte[2 * len];
        find2T(dst, m, short_st, len);
        delete [] dst;

        // eg::Matrix<double,eg::Dynamic,eg::Dynamic> B(2*len, 2*len, 0.);
        eg::Matrix<T, eg::Dynamic, eg::Dynamic> B;
        B.resize(2 * len, 2 * len);

        for (int i = 0; i < 2 * len; i++) {
            for (int j = 0; j < 2 * len; j++) {
                B(i, j) = -A(short_st[i], short_st[j]);
            }
        }

        delete [] short_st;

        for (int i = 0; i < 2 * len; i++) {
            B(i, i) += 1;
        }

        long double det = std::real(B.determinant());

        if (len % 2 == 0) {
            netsum += 1.0 / std::sqrt(det);
        }
        else {
            netsum += -1.0 / std::sqrt(det);
        }
    }

    double sign = 1.0;

    if (m % 2 != 0)
        sign = -1.0;

    return static_cast<double>(netsum) * static_cast<double>(sign);
}

/**
 * Computes the Torontonian of an input matrix.
 *
 * If the output is NaN, that means that the input matrix does not have
 * a Torontonian with physical meaning.
 *
 * This is a wrapper around the templated function `libwalrus::torontonian` for Python
 * integration. It accepts and returns complex double numeric types, and
 * returns sensible values for empty and non-even matrices.
 *
 * In addition, this wrapper function automatically casts all matrices
 * to type `complex<long double>`, allowing for greater precision than supported
 * by Python and NumPy.
 *
 * @param mat flattened vector of size \f$n^2\f$, representing an \f$n\times n\f$
 *       row-ordered symmetric matrix.
 * @return Torontonian of the input matrix
 */
std::complex<double> torontonian_quad(std::vector<std::complex<double>> &mat) {
    std::vector<std::complex<long double>> matq(mat.begin(), mat.end());
    std::complex<long double> tor = torontonian(matq);
    return static_cast<std::complex<double>>(tor);
}

/**
 * Computes the Torontonian of an input matrix.
 *
 * If the output is NaN, that means that the input matrix does not have
 * a Torontonian with physical meaning.
 *
 * This is a wrapper around the templated function `libwalrus::torontonian` for Python
 * integration. It accepts and returns double numeric types, and
 * returns sensible values for empty and non-even matrices.
 *
 * In addition, this wrapper function automatically casts all matrices
 * to type `long double`, allowing for greater precision than supported
 * by Python and NumPy.
 *
 * @param mat flattened vector of size \f$n^2\f$, representing an \f$n\times n\f$
 *       row-ordered symmetric matrix.
 * @return Torontonian of the input matrix
 */
double torontonian_quad(std::vector<double> &mat) {
    std::vector<long double> matq(mat.begin(), mat.end());
    long double tor = torontonian(matq);
    return static_cast<double>(tor);
}

}


#ifndef _LDAPLUSPLUS_OPTIMIZATION_MULTINOMIAL_LOGISTIC_REGRESSION
#define _LDAPLUSPLUS_OPTIMIZATION_MULTINOMIAL_LOGISTIC_REGRESSION

#include <cmath>

#include <Eigen/Core>

namespace ldaplusplus {
namespace optimization {

/**
 * MultinomialLogisticRegression is an implementation of the multinomial
 * logistic loss function (without bias unit).
 *
 * It follows the protocol used by GradientDescent. For the specific function
 * implementations see value() and gradient().
 */
template <typename Scalar>
class MultinomialLogisticRegression
{
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;

    public:
        /**
         * @param X  The documents defining the minimization problem (\f$X \in
         *           \mathbb{R}^{D \times N}\f$)
         * @param y  The class indexes for each document (\f$y \in
         *           \mathbb{N}^N\f$)
         * @param Cy A different weight for each class in the optimization
         *           problem
         * @param L  The L2 regularization penalty for the weights
         */
        MultinomialLogisticRegression(const MatrixX &X, const Eigen::VectorXi &y, VectorX Cy, Scalar L);
        /**
         * @param X  The documents defining the minimization problem (\f$X \in
         *           \mathbb{R}^{D \times N}\f$)
         * @param y  The class indexes for each document (\f$y \in
         *           \mathbb{N}^N\f$)
         * @param L  The L2 regularization penalty for the weights
         */
        MultinomialLogisticRegression(const MatrixX &X, const Eigen::VectorXi &y, Scalar L);

        /**
         * The value of the objective function to be minimized.
         *
         * \f$N\f$ is the number of documents (different vectors), \f$X_n \in
         * \mathbb{R}^D\f$ is the nth document, \f$\eta_y \in \mathbb{R}^D\f$
         * is the weights vector for the class \f$y\f$ defining the hyperplane
         * that separates class \f$y\f$ from all the other, finally \f$y_n\f$
         * is the class of the nth document.
         *
         * \f[
         *     J = -\sum_{n=1}^N C_{y_n}\left(\eta_{y_n}^T X_n - \log\left(
         *         \sum_{\hat{y}=1}^Y \exp\left( \eta_{\hat{y}}^T X_n \right)
         *         \right)\right) +
         *         \frac{L}{2} \left\| \eta \right\|_F^2
         * \f]
         *
         * @param eta The weights of the linear model (\f$\eta \in
         *            \mathbb{R}^{D \times Y}\f$)
         */
        Scalar value(const MatrixX &eta) const;

        /**
         * The gradient of the objective function implemented in value().
         *
         * We use \f$I(y) \in \mathbb{R}^Y\f$ as the indicator vector of
         * \f$y\f$ (a vector with all the values 0 except at the yth position).
         *
         * \f[
         *     \nabla_{\eta} J = -\sum_{n=1}^N C_{y_n} \left(
         *         X_n I(y_n)^T -
         *         \frac{\sum_{\hat{y}=1}^Y X_n I(\hat{y})^T \exp(\eta_{\hat{y}}^T X_n)}
         *              {\sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T X_n)}
         *         \right) +
         *         L \eta
         * \f]
         *
         * @param eta  The weights of the linear model (\f$\eta \in
         *             \mathbb{R}^{D \times Y}\f$)
         * @param grad A matrix of dimensions equal to \f$\eta\f$ that will
         *             hold the result
         */
        void gradient(const MatrixX &eta, Eigen::Ref<MatrixX> grad) const;

    private:
        const MatrixX &X_;
        const Eigen::VectorXi &y_;
        Scalar L_;
        VectorX Cy_;
};

}  // namespace optimization
}  // namespace ldaplusplus
#endif // _LDAPLUSPLUS_OPTIMIZATION_MULTINOMIAL_LOGISTIC_REGRESSION


#ifndef CANNON_PHYSICS_SYSTEMS_KINEMATIC_CAR_H
#define CANNON_PHYSICS_SYSTEMS_KINEMATIC_CAR_H

#include <random>

#include <ompl/control/ODESolver.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/base/spaces/SO2StateSpace.h>
#include <ompl/base/spaces/SE2StateSpace.h>

#include <Eigen/Dense>

#include <cannon/physics/rk4_integrator.hpp>
#include <cannon/physics/systems/system.hpp>
#include <cannon/graphics/geometry/plane.hpp>
#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

namespace oc = ompl::control;
namespace ob = ompl::base;

namespace cannon {
  namespace physics {
    namespace systems {

      struct KinCarSystem : System {
        KinCarSystem(double l = 1.0) : l_(l) {}

        virtual void operator()(const VectorXd& s, VectorXd& dsdt, const double /*t*/) override {
          double th = s[2];
          double uv = s[3];
          double uth = s[4];

          dsdt.resize(5);
          dsdt[0] = uv * std::cos(th);
          dsdt[1] = uv * std::sin(th);
          dsdt[2] = (uv / l_) * std::tan(uth);
          dsdt[3] = 0.0;
          dsdt[4] = 0.0;
        }

        virtual void ompl_ode_adaptor(const oc::ODESolver::StateType& q,
            const oc::Control* control, oc::ODESolver::StateType& qdot) override {

          const double uv = control->as<oc::RealVectorControlSpace::ControlType>()->values[0];
          const double uth = control->as<oc::RealVectorControlSpace::ControlType>()->values[1];

          VectorXd s(5);
          s[0] = q[0];
          s[1] = q[1];
          s[2] = q[2];
          s[3] = uv;
          s[4] = uth;
          VectorXd dsdt(5);

          (*this)(s, dsdt, 0.0);

          qdot.resize(q.size(), 0);
          for (unsigned int i = 0; i < q.size(); i++) {
            qdot[i] = dsdt[i];
          }
        }

        virtual std::tuple<MatrixXd, MatrixXd, VectorXd> get_linearization(const VectorXd& x) override {
          MatrixXd A = MatrixXd::Identity(3, 3);
          VectorXd c = x;

          // TODO Don't hardcode timestep at some point
          double theta = x[2];
          MatrixXd B(3, 2);
          B << std::cos(theta) * 0.01, 0,
               std::sin(theta) * 0.01, 0,
               0, 0;

          return std::make_tuple(A, B, c);
        }

        virtual void
        get_continuous_time_linearization(const oc::ODESolver::StateType &q,
                                          Ref<MatrixXd> A,
                                          Ref<MatrixXd> B) override {
          // TODO
          throw std::runtime_error("Not implemented yet");
        }

        static void ompl_post_integration(const ob::State* /*state*/, const
            oc::Control* /*control*/, const double /*duration*/, ob::State *result) {

          ob::SO2StateSpace SO2;
          SO2.enforceBounds(result->as<ob::SE2StateSpace::StateType>()->as<ob::SO2StateSpace::StateType>(1));
        }

        // Parameters
        double l_;
      };

      class KinematicCar {
        public:
          KinematicCar() = delete;

          KinematicCar(Vector3d s, Vector3d g) : e_(s_, 4, time_step), start_(s), goal_(g) {
            std::random_device rd;
            gen_ = std::mt19937(rd());

            xy_dis_ = std::uniform_real_distribution<double>(-1.0, 1.0);
            th_dis_ = std::uniform_real_distribution<double>(-M_PI, M_PI);

            state_ = VectorXd::Zero(5);
            reset();
          }

          std::pair<VectorXd, double> step(double uv, double uth) {
            double clipped_uv = std::max(-1.0, std::min(uv, 1.0));
            //double clipped_uth = std::max(-M_PI, std::min(uth, M_PI));

            state_[3] = clipped_uv;
            state_[4] = uth;

            double goal_r = -std::pow((state_.head(2) - goal_.head(2)).norm(), 2.0);
            double control_r = -std::pow((std::abs(clipped_uv) + std::abs(uth)), 2.0);
            double reward = goal_r + 0.001*control_r;

            e_.set_state(state_);
            state_ = e_.step();

            return std::make_pair(state_.head(3), reward);
          }

          VectorXd reset() {
            //state_.head(3) = start_ + Vector3d::Random() * 0.1;
            state_[0] = xy_dis_(gen_);
            state_[1] = xy_dis_(gen_);
            state_[2] = th_dis_(gen_);

            state_[3] = 0.0;
            state_[4] = 0.0;

            return state_.head(3);
          }

          VectorXd reset(const VectorXd& s) {
            state_[0] = s[0];
            state_[1] = s[1];
            state_[2] = s[2];

            state_[3] = 0.0;
            state_[4] = 0.0;

            return state_.head(3);

          }

          // In seconds
          const double time_step = 0.01;

          KinCarSystem s_;

        private:
          RK4Integrator e_;

          VectorXd state_;

          Vector3d start_;
          Vector3d goal_;

          std::mt19937 gen_;
          std::uniform_real_distribution<double> xy_dis_;
          std::uniform_real_distribution<double> th_dis_;
      };

    } // namespace physics
  } // namespace physics
} // namespace cannon

#endif /* ifndef CANNON_PHYSICS_SYSTEMS_KINEMATIC_CAR_H */


/** @file
*****************************************************************************

Implementation of a secret-key lattice-based additively homomorphic
vector encryption scheme.

See lwe.hpp

*****************************************************************************
* @author     Samir Menon, Brennan Shacklett, and David J. Wu
* @copyright  MIT license (see LICENSE file)
*****************************************************************************/

#include <cstdlib>
#include <iostream>
#include <cassert>
#include <random>
#include <cstdint>
#include <fstream>
#include <NTL/ZZ.h>

#include "lwe.hpp"
#include <libsnark/common/libsnark_serialization.hpp>

using namespace std;
namespace LWE {

static NTL::ZZ_p random(const NTL::ZZ &mod) {
    // Choose a random value from a space that is 128-bits
    // longer than the target space, and then round down.

    long num_bytes = NTL::NumBytes(mod) + 16;
    unsigned char bytes[num_bytes];
    static ifstream urandom("/dev/urandom", ios::binary);
    urandom.read(reinterpret_cast<char *>(bytes), num_bytes);

    NTL::ZZ randZZ = NTL::ZZFromBytes(bytes, num_bytes);

    return NTL::to_ZZ_p(randZZ % mod);
}

// Sample a discrete Gaussian variable using the Box-Muller
// transform.
static int32_t sample_discrete_gaussian(double stddev) {
    static const double PI = 4.0*atan(1.0);

    double r1 = ((double) rand()) / RAND_MAX;
    double r2 = ((double) rand()) / RAND_MAX;
    double theta = 2*PI*r1;

    return (int32_t) floor(stddev * sqrt(-2.0*log(r2)) * cos(theta) + 0.5);
}

ciphertext& ciphertext::operator=(const ciphertext& other) {
    NTL::ZZ_p::init(LWE::q);

    this->ctxt = other.ctxt;

    return *this;
}

ciphertext ciphertext::operator+(const ciphertext &other) const {
    ciphertext sum = *this;
    sum += other;

    return sum;
}

ciphertext& ciphertext::operator+=(const ciphertext &other) {
    NTL::ZZ_p::init(LWE::q);

    this->ctxt += other.ctxt;
    return *this;
}

ciphertext ciphertext::operator*(uint64_t val) const {
    return operator*(NTL::ZZ_p(val));
}

ciphertext ciphertext::operator*(const NTL::ZZ_p &val) const {
    ciphertext prod = *this;
    prod *= val;

    return prod;
}

ciphertext& ciphertext::operator*=(uint64_t val) {
    return operator*=(NTL::ZZ_p(val));
}

ciphertext& ciphertext::operator*=(const NTL::ZZ_p &val) {
    NTL::ZZ_p::init(LWE::q);

    this->ctxt *= val;
    return *this;
}

ciphertext operator*(uint64_t val, const ciphertext& ct) {
    return operator*(NTL::ZZ_p(val), ct);
}

ciphertext operator*(const NTL::ZZ_p &val, const ciphertext& ct) {
    ciphertext prod = ct;
    prod *= val;

    return prod;
}

secret_key keygen() {
    NTL::ZZ_p::init(LWE::q);
    secret_key sk;

    // Sampled uniformly random matrix A
    matrix A_hat(NTL::INIT_SIZE, n, n);
    for (size_t i = 1; i <= n; i++) {
        for (size_t j = 1; j <= n; j++) {
            A_hat(i, j) = random(q);
        }
    }

    // Sample secret keys from error distribution
    matrix S_hat(NTL::INIT_SIZE, n, pt_dim);
    for (size_t i = 1; i <= n; i++) {
        for (size_t j = 1; j <= pt_dim; j++) {
            S_hat(i, j) = sample_discrete_gaussian(stddev);
        }
    }

    // Sample errors from error distribution
    matrix E_hat(NTL::INIT_SIZE, pt_dim, n);
    for (size_t i = 1; i <= pt_dim; i++) {
        for (size_t j = 1; j <= n; j++) {
            E_hat(i, j) = sample_discrete_gaussian(stddev);
        }
    }

    // Construct A = [ A_hat ; S_hat^T * A_hat + p * E_hat ]
    matrix A_bottom = NTL::transpose(S_hat)*A_hat + p_int*E_hat;

    for (size_t i = 1; i <= n; i++) {
        for (size_t j = 1; j <= n; j++) {
            sk.A(i, j) = A_hat(i, j);
        }
    }

    for (size_t i = 1; i <= pt_dim; i++) {
        for (size_t j = 1; j <= n; j++) {
            sk.A(i + n, j) = A_bottom(i, j);
        }
    }

    // Construct S = [ -S_hat ; I ]
    for (size_t i = 1; i <= n; i++) {
        for (size_t j = 1; j <= pt_dim; j++) {
            sk.S(i, j) = -S_hat(i, j);
        }
    }

    matrix ident = NTL::ident_mat_ZZ_p(pt_dim);
    for (size_t i = 1; i <= pt_dim; i++) {
        for (size_t j = 1; j <= pt_dim; j++) {
            sk.S(i + n, j) = ident(i, j);
        }
    }

    return sk;
}

ciphertext encrypt(const secret_key &sk, const plaintext &pt) {
    NTL::ZZ_p::init(LWE::q);

    // Sample an LWE error vector for the randomness (n x 1)
    vector r(NTL::INIT_SIZE, n);
    for (size_t i = 1; i <= n; i++) {
        r(i) = sample_discrete_gaussian(stddev);
    }

    vector v_padded(NTL::INIT_SIZE, n + pt_dim);
    for (size_t i = 1; i <= n; i++) {
        v_padded(i) = 0;
    }

    for (size_t i = 1; i <= pt_dim; i++) {
        v_padded(i + n) = pt(i);
    }

    ciphertext ctxt;
    ctxt.ctxt = sk.A*r + v_padded;

    // Add error to each component of ciphertext
    for (size_t i = 1; i <= n + pt_dim; i++) {
        ctxt.ctxt(i) += sample_discrete_gaussian(stddev) * LWE::p_int;
    }

    return ctxt;
}

plaintext decrypt(const secret_key &sk, const ciphertext& ct) {
    NTL::ZZ_p::init(LWE::q);
    vector modqvec = NTL::transpose(sk.S)*ct.ctxt;

    NTL::ZZ_p::init(LWE::p);
    plaintext pt(NTL::INIT_SIZE, pt_dim);
    for (size_t i = 1; i <= pt_dim; i++) {
        NTL::ZZ modq = NTL::rep(modqvec(i));
        if (modq > q/2) {
            modq -= q;
        } else if (modq < -q/2) {
            modq += q;
        }
        pt(i) = ((modq % p_int) + p_int) % p_int;
    }

    return pt;
}

}


#include <algorithm>
#include <fstream>
#include <iterator>
#include <map>
#include <set>
#include <sstream>
#include <string>
#include <utility>

#include "utils/profiling.h"

#include <armadillo>

using namespace util;
using namespace arma;

bool checkMat(int row, int col, mat const &mat) {
    if(mat.n_rows == row && mat.n_cols == col) return true;
    return false;
}
bool checkVec(int dim, vec const &vec) {
    if(vec.size() == dim) return true;
    return false;
}

void exit_with_error(std::string error_message) {
    std::cout << error_message << std::endl;
    exit(1);
}

int main() {

    auto timer = Timer{};
    //timer.here_then_reset("");

    int r_dim = 3; // Reduced rank r

    // TF matrix for the test.
    // n_dim by m_dim .
    int n_dim = 50;
    int m_dim = 50;

    sp_mat inM(n_dim,m_dim);
    for(int i = 0; i < n_dim; i++) {
        for(int j = 0; j < m_dim; j++) {
            inM(i,j) = n_dim*i + j + 1;
        }
    }

    mat inU;
    vec ins;
    mat inV;

    svds(inU,ins,inV,inM,r_dim);

    timer.here_then_reset("Truncated SVD is done for original data matrix.\n");

    if(checkMat(n_dim,r_dim,inU) == false) exit_with_error("U matrix is incorrect.");
    if(checkVec(r_dim,ins) == false) exit_with_error("s diagonal matrix is incorrect.");
    if(checkMat(m_dim,r_dim,inV) == false) exit_with_error("V matrix is incorrect.");

    // incremental n_dim by c_dim matrix
    int c_dim = 1;
    mat inC(n_dim,c_dim);
    for(int i = 0; i < n_dim; i++) {
        for(int j = 0; j < c_dim; j++) {
            inC(i,j) = i*i+j*(j+1)+1;
        }
    }

    if(checkMat(n_dim,c_dim,inC) == false) exit_with_error("C matrix is incorrect.");

    mat inL = trans(inU)*inC;
    if(checkMat(r_dim,c_dim,inL) == false) exit_with_error("L matrix is incorrect.");

    mat inH;
    inH = inC - inU*inL;
    if(checkMat(n_dim,c_dim,inH) == false) exit_with_error("H matrix is incorrect.");

    mat inJ;
    mat inK;

    qr(inJ,inK,inH);

    timer.here_then_reset("QR decomposition for H matrix.\n");

    if(checkMat(n_dim,n_dim,inJ) == false) exit_with_error("J matrix is incorrect.");
    if(checkMat(n_dim,c_dim,inK) == false) exit_with_error("K matrix is incorrect.");

    mat inQ(r_dim+n_dim,r_dim+c_dim,fill::zeros);

    for(int r=0;r<r_dim;r++)
        inQ(r,r) = ins(r);

    for(int i=0;i<r_dim;i++) {
        for(int j=0;j<c_dim;j++) {
            inQ(i,r_dim+j) = inL(i,j);
        }
    }

    for(int i=0;i<n_dim;i++) {
        for(int j=0;j<c_dim;j++) {
            inQ(r_dim+i,r_dim+j)=inK(i,j);
        }
    }

    if(checkMat(r_dim+n_dim,r_dim+c_dim,inQ) == false) exit_with_error("Q matrix is incorrect.");

    mat inUp;
    vec insp;
    mat inVp;

    svd(inUp,insp,inVp,inQ);
    timer.here_then_reset("SVD is done for extended Q matrix.\n");

    if(checkMat(r_dim+n_dim,r_dim+n_dim,inUp) == false) exit_with_error("Up matrix is incorrect.");
    if(checkVec(r_dim+c_dim,insp) == false) exit_with_error("sp diagonal matrix is incorrect.");
    // Assuming r_dim+c_dim <= r_dim+n_dim
    if(checkMat(r_dim+c_dim,r_dim+c_dim,inVp) == false) exit_with_error("Vp matrix is incorrect.");

    mat inUpp;
    vec inspp;
    mat inVpp;

    mat mapU;
    mapU = join_rows(inU,inJ);
    if(checkMat(n_dim,r_dim+n_dim,mapU) == false) exit_with_error("mapU matrix is incorrect.");

    mat mapV(m_dim+c_dim,r_dim+c_dim,fill::zeros);
    for(int i=0;i<m_dim;i++) {
        for(int j=0;j<r_dim;j++) {
            mapV(i,j) = inV(i,j);
        }
    }

    for(int i=0;i<c_dim;i++) {
        mapV(m_dim+i,r_dim+i) = 1;
    }
    if(checkMat(m_dim+c_dim,r_dim+c_dim,mapV) == false) exit_with_error("mapV matrix is incorrect.");

    inUpp = mapU * inUp;
    inspp = insp;
    inVpp = mapV * inVp;

    mat inspp_diag(r_dim+n_dim,r_dim+c_dim,fill::zeros);

    for(int i=0;i<r_dim+c_dim;i++) {
        inspp_diag(i,i) = insp(i);
    }

    if(checkMat(n_dim,n_dim+r_dim,inUpp) == false) exit_with_error("Upp matrix is incorrect.");
    if(checkMat(n_dim+r_dim,r_dim+c_dim,inspp_diag) == false) exit_with_error("inspp_diag matrix is incorrect.");
    if(checkMat(m_dim+c_dim,r_dim+c_dim,inVpp) == false) exit_with_error("Vpp matrix is incorrect.");

    mat resMat = inUpp*inspp_diag*trans(inVpp);
    timer.here_then_reset("Updated SVD is done.\n");

    //resMat.print("resMat = ");
    //End of updating SVD (slow way)

    //Begin of brute SVD

    timer.here_then_reset("Test for brute SVD begins. First I make the matrix.\n");

    mat inMb;

    mat inMd(inM.n_rows, inM.n_cols);
    for(int i=0;i<inMd.n_rows;i++) {
        for(int j=0;j<inMd.n_cols;j++) {
            inMd(i,j) = inM(i,j);
        }
    }

    inMb = join_rows(inMd,inC);

    sp_mat inMbb(inMb.n_rows,inMb.n_cols);
    for(int i=0;i< inMbb.n_rows;i++) {
        for(int j=0;j<inMbb.n_cols;j++) {
            inMbb(i,j) = inMb(i,j);
        }
    }

    timer.here_then_reset("Now begins brute SVD calculation.\n");

    mat inUb;
    vec insb;
    mat inVb;

    svds(inUb,insb,inVb,inMbb,r_dim+1); // For the fair comparison, I added 1 to r_dim.

    timer.here_then_reset("Brute SVD is done.\n");

    //End of brute SVD
    return 0;
}


#include <iostream>
#include <vector>
#include <random>
#include <boost/concept_check.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>

using namespace std;
using namespace g2o;

class g2o_vertex: public BaseVertex<3, Eigen::Matrix<double, 1, 3>> {
	void oplusImpl(const double* v) {
		_estimate += Eigen::Matrix<double, 1, 3>(v);
	}
	void setToOriginImpl() {
		_estimate.setZero();
	}

	bool read(istream& is) {}
	bool write(ostream& os) const {}
};

class g2o_edge: public BaseUnaryEdge<1, double, g2o_vertex> {
public:
	explicit g2o_edge(double x): _x(x) {}
	void computeError() {
		g2o_vertex* v = static_cast<g2o_vertex*>(_vertices[0]);
		const Eigen::Matrix<double, 1, 3> est = v->estimate();
		_error(0, 0) = _measurement - exp(est(0,0)*_x*_x + est(0,1)*_x + est(0,2));
		//cout << "_error = " << _error(0,0) << endl;
	}

	bool read(istream& is) {}
	bool write(ostream& os) const {}
private:
	double _x;
};

//y = exp(3*x^2 + 2*x + 1)
int main(int argc, char **argv)
{
	typedef BlockSolver<BlockSolverTraits<3, 1>> block_solver;
	block_solver::LinearSolverType* linear_solver = new LinearSolverDense<block_solver::PoseMatrixType>;
	block_solver* blk_slv = new block_solver(linear_solver);

	OptimizationAlgorithmLevenberg* algorithm = new OptimizationAlgorithmLevenberg(blk_slv);
	SparseOptimizer optimizer;
	optimizer.setAlgorithm(algorithm);
	optimizer.setVerbose(true);

	//加顶点
	g2o_vertex* vertex = new g2o_vertex;
	vertex->setEstimate(Eigen::Matrix<double, 1, 3>(0,0,0));
	vertex->setId(0);
	optimizer.addVertex(vertex);

	//生成观测值
	vector<double> _x,_y;
	double x_temp;
	default_random_engine generator;
	normal_distribution<double> distribution(0.0,0.5);
	for(int i=0;i<100;i++) {
		//100 * 0.005 = 0.5,此值不能太大,过大时exp(3*x^2 + 2*x + 1)就溢出了
		x_temp = i*0.005;
		_x.push_back(x_temp);
		_y.push_back(exp(3*x_temp*x_temp + 2*x_temp + 1) + distribution(generator));
	}
	//加边
	for(int i=0;i<100;i++) {
		g2o_edge* edge = new g2o_edge(_x[i]);
		edge->setId(i);
		edge->setVertex(0, vertex);
		edge->setInformation(Eigen::Matrix<double,1,1>(1/0.25));
		edge->setMeasurement(_y[i]);
		optimizer.addEdge(edge);
	}
	//开始进行优化估计
	optimizer.initializeOptimization();
	optimizer.optimize(100);

	cout << "optimized variable: " << vertex->estimate().transpose() << endl;

	return 0;
}


// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <vinecopulib/misc/tools_stats.hpp>
#include <boost/math/constants/constants.hpp>

namespace vinecopulib {
inline StudentBicop::StudentBicop()
{
    family_ = BicopFamily::student;
    parameters_ = Eigen::VectorXd(2);
    parameters_lower_bounds_ = Eigen::VectorXd(2);
    parameters_upper_bounds_ = Eigen::VectorXd(2);
    parameters_ << 0, 50;
    parameters_lower_bounds_ << -1, 2;
    parameters_upper_bounds_ << 1, 50;
}

inline Eigen::VectorXd StudentBicop::pdf_raw(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double rho = double(this->parameters_(0));
    double nu = double(this->parameters_(1));
    Eigen::VectorXd f = Eigen::VectorXd::Ones(u.rows());
    Eigen::Matrix<double, Eigen::Dynamic, 2> tmp = tools_stats::qt(u, nu);

    f = tmp.col(0).cwiseAbs2() + tmp.col(1).cwiseAbs2() -
        (2 * rho) * tmp.rowwise().prod();
    f /= nu * (1.0 - pow(rho, 2.0));
    f = f + Eigen::VectorXd::Ones(u.rows());
    f = f.array().pow(-(nu + 2.0) / 2.0);
    f = f.cwiseQuotient(tools_stats::dt(tmp, nu).rowwise().prod());
    f *= boost::math::tgamma_ratio((nu + 2.0) / 2.0, nu / 2.0);
    f /= (nu * constant::pi * sqrt(1.0 - pow(rho, 2.0)));

    return f;
}

inline Eigen::VectorXd StudentBicop::cdf(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    using namespace tools_stats;

    double rho = double(this->parameters_(0));
    double nu = double(this->parameters_(1));

    // for integer nu, just use pbvt
    // otherwise, interpolate linearly between floor(nu) and ceil(nu)
    if (nu == round(nu)) {
        int inu = static_cast<int>(nu);
        return pbvt(qt(u, inu), inu, rho);
    } else {
        int nu1 = static_cast<int>(std::floor(nu));
        int nu2 = static_cast<int>(std::ceil(nu));
        double weight = (nu - static_cast<double>(nu1)) /
            (static_cast<double>(nu2) - static_cast<double>(nu1));
        return pbvt(qt(u, nu1), nu1, rho) * (1 - weight) +
            pbvt(qt(u, nu2), nu2, rho) * weight;
    }
}

inline Eigen::VectorXd StudentBicop::hfunc1(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double rho = double(this->parameters_(0));
    double nu = double(this->parameters_(1));
    Eigen::VectorXd h = Eigen::VectorXd::Ones(u.rows());
    Eigen::Matrix<double, Eigen::Dynamic, 2> tmp = tools_stats::qt(u, nu);
    h = nu * h + tmp.col(0).cwiseAbs2();
    h *= (1.0 - pow(rho, 2)) / (nu + 1.0);
    h = h.cwiseSqrt().cwiseInverse().cwiseProduct(
        tmp.col(1) - rho * tmp.col(0));
    h = tools_stats::pt(h, nu + 1.0);

    return h;
}

inline Eigen::VectorXd StudentBicop::hinv1(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double rho = double(this->parameters_(0));
    double nu = double(this->parameters_(1));
    Eigen::VectorXd hinv = Eigen::VectorXd::Ones(u.rows());
    Eigen::VectorXd tmp = u.col(1);
    Eigen::VectorXd tmp2 = u.col(0);
    tmp = tools_stats::qt(tmp, nu + 1.0);
    tmp2 = tools_stats::qt(tmp2, nu);

    hinv = nu * hinv + tmp2.cwiseAbs2();
    hinv *= (1.0 - pow(rho, 2)) / (nu + 1.0);
    hinv = hinv.cwiseSqrt().cwiseProduct(tmp) + rho * tmp2;
    hinv = tools_stats::pt(hinv, nu);

    return hinv;
}

inline Eigen::VectorXd StudentBicop::get_start_parameters(const double tau)
{
    Eigen::VectorXd parameters = get_parameters();
    parameters(0) = std::sin(tau * constant::pi / 2);;
    parameters(1) = 5;
    return parameters;
}

inline Eigen::MatrixXd StudentBicop::tau_to_parameters(const double &tau)
{
    return no_tau_to_parameters(tau);
}
}


#include <iostream>
#include <time.h>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <LEDA/graph/graph.h>
#include <LEDA/graph/shortest_path.h>

using namespace std;
using namespace boost;
using namespace leda;

// Define the boost edge weight property
typedef property<edge_weight_t, int> EdgeWeightProperty;

// Define the boost directed graph: std::vector, std::vector, directed, no vertex property, int edge property, no graph property, std::list
typedef adjacency_list<vecS, vecS, directedS, no_property, EdgeWeightProperty, no_property, listS> DirectedGraph;

// Define the vertex class as vertex_desciptor
typedef graph_traits<DirectedGraph>::vertex_descriptor Vertex;

// Define the edge class as edge_desciptor
typedef graph_traits<DirectedGraph>::edge_descriptor Edge;

// Define the edge iterator as edge_iterator
typedef graph_traits<DirectedGraph>::edge_iterator EdgeIterator;

// Define the edge iterator as edge_iterator
typedef graph_traits<DirectedGraph>::vertex_iterator VertexIterator;

// Define the edge weight map as a property map
typedef property_map<DirectedGraph, edge_weight_t>::type EdgeWeightMap;

/**
 * Copies @param LedaGraph to @param BoostDirectedGraph, using the @param LedaEdgeWeightMap
 * @param BoostDirectedGraph The boost directed graph
 * @param LedaGraph The leda directed graph
 * @param LedaEdgeWeightMap The edge array that contain the leda directed graph edges weights
*/
void CopyLedaGraphToBoostGraph(DirectedGraph& BoostDirectedGraph, leda::graph& LedaGraph, edge_array<int>& LedaEdgeWeightMap)
{
	// Create a new boost directed graph containing the smae number of nodes as the leda directed graph
	DirectedGraph boostGraph(LedaGraph.number_of_nodes());

	// Leda edge that will be used for iteration
	leda::edge tempEdge;

	// For all edges in the leda directed graph
	forall_edges(tempEdge, LedaGraph)
	{
		// Get the source node of the edge
		node source = LedaGraph.source(tempEdge);

		// Get the target node of the edge
		node target = LedaGraph.target(tempEdge);

		// Get the weight of the edge
		int edgeWeight = LedaEdgeWeightMap[tempEdge];

		// Add the edge in the boost directed graph
		add_edge(LedaGraph.index(source), LedaGraph.index(target), edgeWeight, boostGraph);
	}

	// Update the boost directed graph
	BoostDirectedGraph = boostGraph;
}

/**
 * Applies the Bellman Ford algorithm to the @param directedGraph, using @param startingnode
 * @param directedGraph The inserted boost directed graph
 * @param startingNode The node that will be used as the minimum path's starting node
 * @return False if the graph contains a negative weight circle or true otherwise
 */
bool BellmanFord(DirectedGraph& directedGraph, Vertex startingVertex)
{
	// Initialize a node map containg the node minimum path cost
	std::map<Vertex, int> nodeCostMap;

	// Initialize the property map that contain the edges's weights
	EdgeWeightMap boostEdgeWeightMap = get(edge_weight, directedGraph);

	// Initialise the boost vertex iterators
	VertexIterator vertexIteratorBegin, vertexIteratorEnd;

	// For every vertex in the boost directed graph...
	for(tie(vertexIteratorBegin, vertexIteratorEnd) = vertices(directedGraph); vertexIteratorBegin != vertexIteratorEnd; vertexIteratorBegin++)
	{
		// Set the vertex initial cost to INT_MAX
		nodeCostMap.insert(pair<Vertex,int>(*vertexIteratorBegin, INT_MAX));
	}

	// Set the starting node cost to 0
	nodeCostMap[startingVertex] = 0;

	// Initialise the boost edge iterators
	EdgeIterator edgeIteratorBegin, edgeIteratorEnd;

	// For every vertex in the boost directed graph...
	for(tie(vertexIteratorBegin, vertexIteratorEnd) = vertices(directedGraph); vertexIteratorBegin != vertexIteratorEnd; vertexIteratorBegin++)
	{
		// For every out edge of the current vertex...
		for(tie(edgeIteratorBegin, edgeIteratorEnd) = edges(directedGraph); edgeIteratorBegin != edgeIteratorEnd; edgeIteratorBegin++)
		{
			// Get the current edge's source node
			int sourceNodeCost = nodeCostMap[source(*edgeIteratorBegin, directedGraph)];

			// Get the current edge's target node
			int targetNodeCost = nodeCostMap[target(*edgeIteratorBegin, directedGraph)];

			// Get the current edge's weight
			int edgeWeight = boostEdgeWeightMap[*edgeIteratorBegin];

			// If the current edge verifies the triangular inequality and has already been accessed...
			if(sourceNodeCost != INT_MAX && (sourceNodeCost + edgeWeight < targetNodeCost))
			{
				nodeCostMap[target(*edgeIteratorBegin, directedGraph)] = sourceNodeCost + edgeWeight;
			}
	 	}
	}

	// For all edges in the boost directed graph...
	for(tie(edgeIteratorBegin, edgeIteratorEnd) = edges(directedGraph); edgeIteratorBegin != edgeIteratorEnd; edgeIteratorBegin++)
	{
		// Get the current edge's source node
		int sourceNodeCost = nodeCostMap[source(*edgeIteratorBegin, directedGraph)];

		// Get the current edge's target node
		int targetNodeCost = nodeCostMap[target(*edgeIteratorBegin, directedGraph)];

		// Get the current edge's weight
		int edgeWeight = boostEdgeWeightMap[*edgeIteratorBegin];

		// If negative cycle is detected...
		if(sourceNodeCost != INT_MAX && (sourceNodeCost + edgeWeight < targetNodeCost))
		{
			// Return false if a negative weight cycle is present in the directed graph
			return false;
		}
	}

	// Return true since the directed graph doesn't contain a negative weight cycle
	return true;
}

// Main function
int main()
{
	#pragma region Initialization

	// Create an empty boost directed graph
	DirectedGraph boostDirectedGraph;

	// Create an empty leda directed graph
	leda::graph ledaDirectedGraph;

	//Create an empty edge array
	edge_array<int> ledaEdgeWeightArray;

	// User graph option
	std::string graphOption;

	// Number of nodes
	int numberOfNodes;

	cout << "Choose the testing graph between grid or random." << endl;

	// Read the graph type
	cin >> graphOption;

	cout << "Enter the number of nodes." << endl;

	// Read the number of nodes
	cin >> numberOfNodes;

	// If the grid graph is selected...
	if(graphOption == "grid")
	{
		// Create a grid graph
		grid_graph(ledaDirectedGraph, numberOfNodes);

		// Intialise an edge array that will contain the leda graph edges weights
		edge_array<int> edgeWeightArray(ledaDirectedGraph);

		// Copy the edge array
		ledaEdgeWeightArray = edgeWeightArray;

		// Initialise a random seed
		srand(time(NULL));

		// Edge that will be used for the iteration
		leda::edge tempEdge;

		// For every edge in the undirected graph...
		forall_edges(tempEdge, ledaDirectedGraph)
		{
			// Get the current edge source node index
			int tempEdgeSourceNodeIndex = ledaDirectedGraph.index(ledaDirectedGraph.source(tempEdge));

			// Get the current edge target node index
			int tempEdgeTargetNodeIndex = ledaDirectedGraph.index(ledaDirectedGraph.target(tempEdge));

			// Get the point representation of the edge's source node index
			div_t tempEdgeSourceNodeIndexDivResult = div(tempEdgeSourceNodeIndex, numberOfNodes);

			// Get the point representation of the edge's target node index
			div_t tempEdgeTargetNodeIndexDivResult = div(tempEdgeTargetNodeIndex, numberOfNodes);

			// Check if the edge is a vertical edge that belong in the third quarter
			bool verticalEdgeThirdQuarterPresence = (tempEdgeSourceNodeIndexDivResult.quot >= (numberOfNodes/2)) && (tempEdgeSourceNodeIndexDivResult.rem <= (numberOfNodes/2)) && (tempEdgeTargetNodeIndexDivResult.quot > (numberOfNodes/2)) && (tempEdgeTargetNodeIndexDivResult.rem < (numberOfNodes/2));

			// Check if the edge is a horizontal edge that belong in the third quarter
			bool horizontalEdgeThirdQuarterPresence = (tempEdgeSourceNodeIndexDivResult.quot > (numberOfNodes/2)) && (tempEdgeSourceNodeIndexDivResult.rem < (numberOfNodes/2)) && (tempEdgeTargetNodeIndexDivResult.quot >= (numberOfNodes/2)) && (tempEdgeTargetNodeIndexDivResult.rem <= (numberOfNodes/2));

			// If the edge belongs into the third quarter...
			if(verticalEdgeThirdQuarterPresence || horizontalEdgeThirdQuarterPresence)
			{
				// If the current edge, was randomly chosen to reversed...
				if((rand() % 2) == 0)
				{
					// Reverse the current edge
					ledaDirectedGraph.rev_edge(tempEdge);
				}

				// Check if the edge is the special horizontal edge
				bool specialThirdQuarterHorizontalEdge = (tempEdgeSourceNodeIndexDivResult.quot == (numberOfNodes/2 + 1)) && (tempEdgeSourceNodeIndexDivResult.rem == (numberOfNodes/2 - 1)) && (tempEdgeTargetNodeIndexDivResult.quot == (numberOfNodes/2 + 1)) && (tempEdgeTargetNodeIndexDivResult.rem == (numberOfNodes/2));

				// Check if the edge is the special vertical edge
				bool specialThirdQuarterVerticalEdge = (tempEdgeSourceNodeIndexDivResult.quot == (numberOfNodes/2)) && (tempEdgeSourceNodeIndexDivResult.rem == (numberOfNodes/2 - 1)) && (tempEdgeTargetNodeIndexDivResult.quot == (numberOfNodes/2 + 1)) && (tempEdgeTargetNodeIndexDivResult.rem == (numberOfNodes/2 - 1));

				// If the edge is either the special negative weight vertical edge or the special negative weight horizontal edge...
				if(specialThirdQuarterVerticalEdge || specialThirdQuarterHorizontalEdge)
				{
					// Assign -100000 as weight to the current special edge
					ledaEdgeWeightArray[tempEdge] = -100000;
				}
			}
			else
			{
				// Assign random integer values as costs between 0 and 10000
				ledaEdgeWeightArray[tempEdge] = (rand() % 10000);
			}
		}
		// Get the grid graph number of nodes
		numberOfNodes = ledaDirectedGraph.number_of_nodes();

		// Copy the leda directed graph to the boost directed graph
		CopyLedaGraphToBoostGraph(boostDirectedGraph, ledaDirectedGraph, ledaEdgeWeightArray);

		// Initialise a property map that contain the boost graph edges weights
		EdgeWeightMap boostEdgeWeightMap = get(edge_weight, boostDirectedGraph);
	}
	else
	{
		// If the random graph is selected...
		if(graphOption == "random")
		{
			// Calculate the number of edges
			int numberOfEdges = ceil(20 * numberOfNodes * log2(numberOfNodes));

			// Generate a random directed graph
			random_graph(ledaDirectedGraph, numberOfNodes, numberOfEdges, false, true, true);

			// Make the graph cohesive
			Make_Connected(ledaDirectedGraph);

			// Intialise an edge array that will contain the leda graph edges weights
			edge_array<int> edgeWeightArray(ledaDirectedGraph);

			// Copy the edge array
			ledaEdgeWeightArray = edgeWeightArray;

			// Initialise a random seed
			srand(time(NULL));

			// Edge that will be used for the iteration
			leda::edge tempEdge;

			// For every edge in the undirected graph...
			forall_edges(tempEdge, ledaDirectedGraph)
			{
				// Assign random integer values as costs between 10 and 10000
				ledaEdgeWeightArray[tempEdge] = (rand() % 10100) - 100;
			}

			// Copy the leda directed graph to the boost directed graph
			CopyLedaGraphToBoostGraph(boostDirectedGraph, ledaDirectedGraph, ledaEdgeWeightArray);

			// Initialise a property map that contain the boost graph edges weights
			EdgeWeightMap boostEdgeWeightMap = get(edge_weight, boostDirectedGraph);
		}
		else
		{
			cout << "Choose between grid or random." << endl;

			// Exit if another option is selected
			exit(0);
		}
	}

	// Choose a random node from the Leda directed graph
	node randomLedaNode = ledaDirectedGraph.choose_node();

	// Get the index of the Boost directed graph vertex that correspond to the appropriate Leda random chosen node
	Vertex randomBoostVertex = vertex(ledaDirectedGraph.index(randomLedaNode), boostDirectedGraph);

	// Initialise a node array the will contain the last edge on a shortest path from the starting node to a node
	node_array<leda::edge> ledaPredecessorNodeArray(ledaDirectedGraph);

	// Initialise a node array the will contain the shortest path langth from the starting node to a node
	node_array<int> ledaDistanceNodeArray(ledaDirectedGraph);

	// Initialize a property map that contain the edges's weights
	EdgeWeightMap boostEdgeWeightMap = get(edge_weight, boostDirectedGraph);

	// Initialize a vector that will contain the distance to each node and set the initial distance to INT_MAX
	std::vector<int> boostDistanceVector(numberOfNodes,INT_MAX);

	// Set the distance of the chosen vertex to 0
	boostDistanceVector[randomBoostVertex] = 0;

	// Initialize a vector that will contain the predeccesor of each node
	std::vector<std::size_t> boostPredeccesorVector(numberOfNodes);

	// For every vertex in the boost directed graph...
	for(int index = 0; index < numberOfNodes; index++)
	{
		// Set the current vertex predeccesor to itself
		boostPredeccesorVector[index] = index;
	}

	#pragma endregion Initialization

	#pragma region Simulation

	// Initialise the starting CPU time
	float CPUTime = used_time();

	// Execute the user defined Bellman Ford algorithm for the boost directed graph using the random vertex
	bool negativeWeightCircleNotFound = BellmanFord(boostDirectedGraph, randomBoostVertex);

	// Print the user defined Bellman Ford function execution time
	cout << "User defined Bellman Ford function execution time: " << used_time(CPUTime) << " seconds."<< endl;

	// Execute the Leda Bellman Ford algorithm for the Leda directed graph using the random node and the defined arrays
	negativeWeightCircleNotFound = BELLMAN_FORD(ledaDirectedGraph, randomLedaNode, ledaEdgeWeightArray, ledaDistanceNodeArray, ledaPredecessorNodeArray);

	// Print the Leda Bellman Ford function execution time
	cout << "Leda Bellman Ford function execution time: " << used_time(CPUTime) << " seconds."<< endl;

	// If the directed graph doesn't contain a negative weight circle...
	if(negativeWeightCircleNotFound)
	{
		// Execute the Boost Bellman Ford algorithm for the boost directed graph using the defined maps
		bellman_ford_shortest_paths(boostDirectedGraph, numberOfNodes, weight_map(boostEdgeWeightMap).distance_map(&boostDistanceVector[0]).distance_map(&boostPredeccesorVector[0]));

		// Print the Boost Bellman Ford function execution time
		cout << "Boost Bellman Ford function execution time: " << used_time(CPUTime) << " seconds."<< endl;

		cout << "The directed graph doesn't contain a negative weight cycle." << endl;
	}
	else
	{
		cout << "The directed graph contains a negative weight cycle." << endl;
	}

	#pragma endregion Simulation

	// Return 0
	return 0;
}

Project Τεχνολογίες Υλοποίησης Αλγορίθμων/2η Άσκηση/Ergasia_2.cpp


/*****************************************************************************
 * fixed.cpp        Blitz++ array using a custom type
 * $Id$
 * This example illustrates how simple it is to create Blitz++ arrays
 * using a custom type.
 *****************************************************************************/

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

// A simple fixed point arithmetic class which represents a point
// in the interval [0,1].
class FixedPoint {

public:
    typedef unsigned int T_mantissa;

    FixedPoint() { }

    explicit FixedPoint(T_mantissa mantissa)
    {
        mantissa_ = mantissa;
    }

    FixedPoint(double value)
    {
        assert((value >= 0.0) && (value <= 1.0));
        mantissa_ = T_mantissa(value * huge(T_mantissa()));
    }

    FixedPoint operator+(FixedPoint x)
    { return FixedPoint(mantissa_ + x.mantissa_); }

    double value() const
    { return mantissa_ / double(huge(T_mantissa())); }

private:
    T_mantissa mantissa_;
};

ostream& operator<<(ostream& os, const FixedPoint& a)
{
    os << a.value();
    return os;
}

int main()
{
    // Now create an array using the FixedPoint class:

    Array<FixedPoint, 2> A(4,4), B(4,4);

    A = 0.5, 0.3, 0.8, 0.2,
        0.1, 0.3, 0.2, 0.9,
        0.0, 1.0, 0.7, 0.4,
        0.2, 0.3, 0.8, 0.4;

    B = A + 0.05;

    cout << "B = " << B << endl;

    return 0;
}

// Program output:
// B = 4 x 4
//      0.55      0.35      0.85      0.25
//      0.15      0.35      0.25      0.95
//      0.05      0.05      0.75      0.45
//      0.25      0.35      0.85      0.45

/*
 * Note: Just because Array<T,N> supports all possible operators doesn't
 * mean that a user-defined class has to.  You only need to define the
 * operators you actually use on the array.  This works because the ISO/ANSI
 * draft standard forbids instantiation of unused member functions:
 *
 * [temp.inst, paragraph 7]
 * An implementation shall not implicitly instantiate  a  function,  non-
 * virtual  member  function,  class  or  member  template  that does not
 * require instantiation.  It is unspecified whether or not an  implemen-
 * tation implicitly instantiates a virtual member function that does not
 * require specialization.
 */


// GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
// Distributed under the GNU Lesser General Public License 2.1 with an
// addendum covering inlined code. (See accompanying files LICENSE and
// LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)

// This file was originally part of PyJuggler.

// PyJuggler is (C) Copyright 2002, 2003 by Patrick Hartling
// Distributed under the GNU Lesser General Public License 2.1.  (See
// accompanying file COPYING.txt or http://www.gnu.org/copyleft/lesser.txt)

// Includes ====================================================================
#include <boost/python.hpp>
#include <gmtl/Math.h>

// Using =======================================================================
using namespace boost::python;

// Declarations ================================================================

namespace gmtlWrappers
{
   template<typename T, typename U>
   T lerp(const U& lerp, const T& a, const T& b)
   {
      T result;
      gmtl::Math::lerp(result, lerp, a, b);
      return result;
   }

   template<typename T>
   tuple quadraticFormula(const T& a, const T& b, const T& c)
   {
      T r1, r2;
      bool result = gmtl::Math::quadraticFormula(r1, r2, a, b, c);
      return make_tuple(result, r1, r2);
   }
}

namespace
{

class Fake : public boost::noncopyable
{
};

}

// Module ======================================================================
void _Export_gmtl_Math_h()
{
    // Retained (temporarily) for backwards compatibility.
    def("deg2Rad", (double (*)(double))&gmtl::Math::deg2Rad);
    def("deg2Rad", (float (*)(float))&gmtl::Math::deg2Rad);
    def("rad2Deg", (float (*)(float))&gmtl::Math::rad2Deg);
    def("rad2Deg", (double (*)(double))&gmtl::Math::rad2Deg);

    scope* gmtl_Math_scope = new scope(
    class_< Fake, boost::noncopyable >("Math", no_init)
        .def("sign", (int (*)(int)) &gmtl::Math::sign)
        .def("sign", (int (*)(float)) &gmtl::Math::sign)
        .def("sign", (int (*)(double)) &gmtl::Math::sign)
        .def("fastInvSqrt", &gmtl::Math::fastInvSqrt)
        .def("fastInvSqrt2", &gmtl::Math::fastInvSqrt2)
        .def("fastInvSqrt3", &gmtl::Math::fastInvSqrt3)
        .def("deg2Rad", (double (*)(double))&gmtl::Math::deg2Rad)
        .def("deg2Rad", (float (*)(float))&gmtl::Math::deg2Rad)
        .def("rad2Deg", (float (*)(float))&gmtl::Math::rad2Deg)
        .def("rad2Deg", (double (*)(double))&gmtl::Math::rad2Deg)
        .def("factorial", (double (*)(double)) &gmtl::Math::factorial)
        .def("factorial", (float (*)(float)) &gmtl::Math::factorial)
        .def("factorial", (int (*)(int)) &gmtl::Math::factorial)
        .def("lerp",
            (double (*)(const double&, const double&, const double&)) &gmtlWrappers::lerp)
        .def("lerp",
            (float (*)(const float&, const float&, const float&)) &gmtlWrappers::lerp)
        .def("quadraticFormula",
            (tuple (*)(const double&, const double&, const double&)) &gmtlWrappers::quadraticFormula)
        .def("quadraticFormula",
            (tuple (*)(const float&, const float&, const float&)) &gmtlWrappers::quadraticFormula)
        .staticmethod("sign")
        .staticmethod("fastInvSqrt")
        .staticmethod("fastInvSqrt2")
        .staticmethod("fastInvSqrt3")
        .staticmethod("deg2Rad")
        .staticmethod("rad2Deg")
        .staticmethod("factorial")
        .staticmethod("lerp")
        .staticmethod("quadraticFormula")
    );

    delete gmtl_Math_scope;
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2007, 2010.

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

#ifdef _MSC_VER
# pragma warning (disable : 4305) // 'initializing' : truncation from 'long double' to 'const eval_type'
# pragma warning (disable : 4244) //  conversion from 'long double' to 'const eval_type'
#endif

#include <iostream>
using std::cout; using std::endl;

//[policy_eg_3

#include <boost/math/distributions/binomial.hpp>
using boost::math::binomial_distribution;

// Begin by defining a policy type, that gives the behaviour we want:

//using namespace boost::math::policies; or explicitly
using boost::math::policies::policy;

using boost::math::policies::promote_float;
using boost::math::policies::discrete_quantile;
using boost::math::policies::integer_round_nearest;

typedef policy<
   promote_float<false>, // Do not promote to double.
   discrete_quantile<integer_round_nearest> // Round result to nearest integer.
> mypolicy;
//
// Then define a new distribution that uses it:
typedef boost::math::binomial_distribution<float, mypolicy> mybinom;

//  And now use it to get the quantile:

int main()
{
   cout << "quantile(mybinom(200, 0.25), 0.05) is: " <<
      quantile(mybinom(200, 0.25), 0.05) << endl;
}

//]

/*

Output:

  quantile(mybinom(200, 0.25), 0.05) is: 40

*/