/*
 *  carpack.cpp
 *  carpack
 *
 *  Created by Brandon Kelly on 12/19/12.
 *
 *  Method definitions of classes for MCMC sampling from continuous time
 *  autoregressive (CAR) models.
 *
 */

// Standard includes
#include <complex>
#include <iostream>

// Armadillo includes
#include <armadillo>

// Boost includes
#include <boost/math/special_functions/binomial.hpp>

// Local includes
#include "include/carpack.hpp"

// Global random number generator object, instantiated in random.cpp
extern boost::random::mt19937 rng;

// Object containing some common random number generators.
extern RandomGenerator RandGen;

/********************************************************************
						METHODS OF CAR1 CLASS
 *******************************************************************/

// Method of CAR1 class to generate the starting values of the
// parameters theta = (mu, sigma, measerr_scale, log(omega)).

arma::vec CAR1::StartingValue()
{
	double log_omega_start, car1_stdev_start, sigma;

	// Initialize the standard deviation of the CAR(1) process
	// by drawing from its prior
	car1_stdev_start = RandGen.scaled_inverse_chisqr(y_.n_elem-1, arma::var(y_));
	car1_stdev_start = sqrt(car1_stdev_start);

    // Get initial value of the time series mean
    double mu = RandGen.normal(arma::mean(y_), car1_stdev_start / y_.n_elem);

	// Initialize log(omega) to log( 1 / (a * median(dt)) ), where
	// a ~ Uniform(1,50) , under the constraint that
	// tau = 1 / omega < max(time)

    arma::vec dt = time_(arma::span(1,time_.n_elem-1)) - time_(arma::span(0,time_.n_elem-2));
	log_omega_start = -1.0 * log(arma::median(dt) * RandGen.uniform( 1.0, 50.0 ));
	log_omega_start = std::min(log_omega_start, max_freq_);

	sigma = car1_stdev_start * sqrt(2.0 * exp(log_omega_start));

	// Get initial value of the measurement error scaling parameter by
	// drawing from its prior.

	double measerr_scale = RandGen.scaled_inverse_chisqr(measerr_dof_, 1.0);
    measerr_scale = std::min(measerr_scale, 1.99);
    measerr_scale = std::max(measerr_scale, 0.51);

	arma::vec theta(4);

	theta << car1_stdev_start << measerr_scale << mu << log_omega_start << arma::endr;

	// Initialize the Kalman filter
    pKFilter_->SetOmega(exp(log_omega_start));
    pKFilter_->SetSigsqr(sigma * sigma);
    arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
    pKFilter_->SetTimeSeriesErr(proposed_yerr);
    arma::vec ycent = y_ - mu;
    pKFilter_->SetTimeSeries(ycent);
    pKFilter_->Filter();

	return theta;
}

arma::vec CAR1::SetStartingValue(arma::vec init)
{
   if (init.n_elem != 4) {
      std::cout << "WARNING: initial guess not length 4, initializing with prior" << std::endl;
      return StartingValue();
   }

   double logpost = LogDensity(init);
   bool good_initials = arma::is_finite(logpost);
   if (good_initials == false) {
      std::cout << "WARNING: initial guess yields non-finite likelihood, initializing with prior" << std::endl;
      return StartingValue();
   }

   double mu, log_omega_start, car1_stdev_start, sigma, measerr_scale;
   car1_stdev_start = init[0];
   measerr_scale    = init[1];
   mu               = init[2];
   log_omega_start  = init[3];
   sigma            = car1_stdev_start * sqrt(2.0 * exp(log_omega_start));

   pKFilter_->SetOmega(exp(log_omega_start));
   pKFilter_->SetSigsqr(sigma * sigma);
   arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
   pKFilter_->SetTimeSeriesErr(proposed_yerr);
   arma::vec ycent = y_ - mu;
   pKFilter_->SetTimeSeries(ycent);
   pKFilter_->Filter();

   return init;
}

bool CAR1::CheckPriorBounds(arma::vec theta)
{
    double ysigma = theta(0);
    double measerr_scale = theta(1);
    double omega = exp(theta(3));

    bool prior_satisfied = true;
    if ( (omega > max_freq_) || (omega < min_freq_) ||
        (ysigma > max_stdev_) || (ysigma < 0) ||
        (measerr_scale < 0.5) || (measerr_scale > 2.0) ) {
		// prior bounds not satisfied
		prior_satisfied = false;
	}
    return prior_satisfied;
}

/********************************************************************
                        METHODS OF CARp CLASS
 *******************************************************************/

// Calculate the roots of the AR(p) polynomial from the parameters
arma::cx_vec CARp::ARRoots(arma::vec theta)
{
    arma::cx_vec ar_roots(p_);

    // Construct the complex vector of roots of the characteristic polynomial:
    // alpha(s) = s^p + alpha_1 s^{p-1} + ... + alpha_{p-1} s + alpha_p
    for (int i=0; i<p_/2; i++) {
        // alpha(s) decomposed into its quadratic terms:
        //   alpha(s) = (quad_term1 + quad_term2 * s + s^2) * ...
        double quad_term1 = exp(theta(3+2*i));
        double quad_term2 = exp(theta(3+2*i+1));

        double discriminant = quad_term2 * quad_term2 - 4.0 * quad_term1;

        if (discriminant > 0) {
            // two real roots
            double root1 = -0.5 * (quad_term2 + sqrt(discriminant));
            double root2 = -0.5 * (quad_term2 - sqrt(discriminant));
            ar_roots(2*i) = std::complex<double> (root1, 0.0);
            ar_roots(2*i+1) = std::complex<double> (root2, 0.0);
        } else {
            double real_part = -0.5 * quad_term2;
            double imag_part = -0.5 * sqrt(-discriminant);
            ar_roots(2*i) = std::complex<double> (real_part, imag_part);
            ar_roots(2*i+1) = std::complex<double> (real_part, -imag_part);
        }
    }

    if ((p_ % 2) == 1) {
        // p is odd, so add in additional low-frequency component
        double real_root = -exp(theta(3+p_-1));
        ar_roots(p_-1) = std::complex<double> (real_root, 0.0);
    }

    return ar_roots;
}

// Return the starting value and set log_posterior_
arma::vec CARp::StartingValue()
{
    // Create the parameter vector, theta
    arma::vec theta(p_+3);

    bool good_initials = false;
    int iguess_count = 0;
    while (!good_initials) {

        arma::vec loga = StartingAR();
        for (int i=0; i<p_; i++) {
            theta(3+i) = loga(i);
        }

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        double yvar = RandGen.scaled_inverse_chisqr(y_.n_elem-1, arma::var(y_));

        // Get initial value of the time series mean
        double mu = RandGen.normal(arma::mean(y_), sqrt(yvar) / y_.n_elem);

        arma::cx_vec alpha_roots = ARRoots(theta);
        double sigsqr = yvar / Variance(alpha_roots, ma_coefs_, 1.0);

        // Get initial value of the measurement error scaling parameter by
        // drawing from its prior.
        double measerr_scale = RandGen.scaled_inverse_chisqr(measerr_dof_, 1.0);
        measerr_scale = std::min(measerr_scale, 1.99);
        measerr_scale = std::max(measerr_scale, 0.51);

        theta(0) = sqrt(yvar);
        theta(1) = measerr_scale;
        theta(2) = mu;

        // set the Kalman filter parameters
        pKFilter_->SetSigsqr(sigsqr);
        pKFilter_->SetOmega(ExtractAR(theta));
        arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
        pKFilter_->SetTimeSeriesErr(proposed_yerr);
        arma::vec ycent = y_ - mu;
        pKFilter_->SetTimeSeries(ycent);

        // run the kalman filter
        pKFilter_->Filter();

        double logpost = LogDensity(theta);
        good_initials = arma::is_finite(logpost);

        iguess_count++;
        if (iguess_count > 200) {
            std::cout << "Tried 200 initial guesses, still trying..." << std::endl;
        }
    } // continue loop until the starting values give us a finite posterior

    return theta;
}

// Return the starting value and set log_posterior_
arma::vec CARp::SetStartingValue(arma::vec init)
{

   if (init.n_elem != (p_+3)) {
      std::cout << "WARNING: initial guess wrong length, initializing with prior" << std::endl;
      return StartingValue();
   }

   double logpost = LogDensity(init);
   bool good_initials = arma::is_finite(logpost);
   if (good_initials == false) {
      std::cout << "WARNING: initial guess yields non-finite likelihood, initializing with prior" << std::endl;
      return StartingValue();
   }

   double yvar = init(0)*init(0);
   double measerr_scale = init(1);
   double mu = init(2);

   arma::cx_vec alpha_roots = ARRoots(init);
   double sigsqr = yvar / Variance(alpha_roots, ma_coefs_, 1.0);

   // set the Kalman filter parameters
   pKFilter_->SetSigsqr(sigsqr);
   pKFilter_->SetOmega(ExtractAR(init));
   arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
   pKFilter_->SetTimeSeriesErr(proposed_yerr);
   arma::vec ycent = y_ - mu;
   pKFilter_->SetTimeSeries(ycent);
   pKFilter_->Filter();

   return init;
}

// return the starting values for the autoregressive polynomial paramters
arma::vec CARp::StartingAR() {
    double min_freq = 1.0 / (time_.max() - time_.min());

    // Obtain initial values for Lorentzian centroids (= system frequencies) and
    // widths (= break frequencies)
    arma::vec lorentz_cent((p_+1)/2);
    lorentz_cent.randu();
    lorentz_cent = log(max_freq_ / min_freq) * lorentz_cent + log(min_freq);
    lorentz_cent = arma::exp(lorentz_cent);

    // Force system frequencies to be in descending order to make the model identifiable
    lorentz_cent = arma::sort(lorentz_cent, 1);

    arma::vec lorentz_width((p_+1)/2);
    lorentz_width.randu();
    lorentz_width = log(max_freq_ / min_freq) * lorentz_width + log(min_freq);
    lorentz_width = arma::exp(lorentz_width);

    if ((p_ % 2) == 1) {
        // p is odd, so add additional low-frequency component
        lorentz_cent(p_/2) = 0.0;
        // make initial break frequency of low-frequency component less than minimum
        // value of the system frequencies
        lorentz_width(p_/2) = exp(RandGen.uniform(log(min_freq), log(lorentz_cent(p_/2-1))));
    }

    arma::vec loga(p_);

    // convert the PSD lorentzian parameters to quadratic terms in the AR polynomial decomposition
    for (int i=0; i<p_/2; i++) {
        double real_part = -2.0 * arma::datum::pi * lorentz_width(i);
        double imag_part = 2.0 * arma::datum::pi * lorentz_cent(i);
        double quad_term1 = real_part * real_part + imag_part * imag_part;
        double quad_term2 = -2.0 * real_part;
        loga(2*i) = log(quad_term1);
        loga(1+2*i) = log(quad_term2);
    }
    if ((p_ % 2) == 1) {
        // p is odd, so add in additional value of lorentz_width
        double real_part = -2.0 * arma::datum::pi * lorentz_width(p_/2);
        loga(p_-1) = log(-real_part);
    }
    return loga;
}

// check prior bounds
bool CARp::CheckPriorBounds(arma::vec theta)
{
    if (ignore_prior_) {return true;}

    double ysigma = theta(0);
    double measerr_scale = theta(1);
    arma::cx_vec ar_roots = ExtractAR(theta);

    arma::vec lorentz_cent = arma::abs(arma::imag(ar_roots)) / 2.0 / arma::datum::pi;
    arma::vec lorentz_width = -arma::real(ar_roots) / 2.0 / arma::datum::pi;

    // Find the set of Frequencies satisfying the prior bounds
    arma::uvec valid_frequencies1 = arma::find(lorentz_cent < max_freq_);
	arma::uvec valid_frequencies2 = arma::find(lorentz_width < max_freq_);
    arma::uvec valid_frequencies3 = arma::find(lorentz_width > min_freq_);

    double tol = 1e-4;
    bool prior_satisfied = unique_roots(ar_roots, tol); // are the roots unique?

    if ( (valid_frequencies1.n_elem != lorentz_cent.n_elem) ||
        (valid_frequencies2.n_elem != lorentz_width.n_elem) ||
        (valid_frequencies3.n_elem != lorentz_width.n_elem) ||
        (ysigma > max_stdev_) || (ysigma < 0) ||
        (measerr_scale < 0.5) || (measerr_scale > 2.0) ) {
        // Value are outside of prior bounds

//        std::cout << "prior bounds violated" << std::endl;
//        std::cout << "# of valid centroids: " << valid_frequencies1.n_elem << std::endl;
//        std::cout << "# of valid widths: " << valid_frequencies2.n_elem << std::endl;
//        std::cout << "max_freq: " << max_freq_ << std::endl;
//        std::cout << "min_freq: " << min_freq_ << std::endl;
//        lorentz_cent.print("centroid");
//        lorentz_width.print("width");
//        std::cout << "ysigma: " << ysigma << ", max_stdev: " << max_stdev_ << std::endl;
//        std::cout << "measerr_scale: " << measerr_scale << std::endl;
//
        prior_satisfied = false;
    }

    if (order_lorentzians_) {
        // Make sure the Lorentzian centroids are still in decreasing order
        for (int i=1; i<lorentz_cent.n_elem; i++) {
            double lorentz_cent_difference = lorentz_cent(i) - lorentz_cent(i-1);
            if (lorentz_cent_difference > 1e-8) {
                // Lorentzians are not in decreasing order, reject this proposal
                prior_satisfied = false;
            }
        }
    }

//    // Make sure Lorentzian widths are greater than minimum frequency for those Lorentzians with centroids
//    // less than the minimum frequency.
//    for (int i=0; i<lorentz_cent.n_elem; i++) {
//        if (lorentz_cent(i) < min_freq_) {
//            if (lorentz_width(i) < min_freq_) {
//                prior_satisfied = false;
//            }
//        }
//    }
    return prior_satisfied;
}

// Calculate the variance of the CAR(p) process
double CARp::Variance(arma::cx_vec alpha_roots, arma::vec ma_coefs, double sigma, double dt)
{
    std::complex<double> car_var(0.0,0.0);
    std::complex<double> denom(0.0,0.0);
    std::complex<double> numer(0.0,0.0);

	// Calculate the variance of a CAR(p) process
	for (int k=0; k<alpha_roots.n_elem; k++) {
		std::complex<double> denom_product(1.0,0.0);

		for (int l=0; l<alpha_roots.n_elem; l++) {
			if (l != k) {
				denom_product *= (alpha_roots(l) - alpha_roots(k)) *
                (std::conj(alpha_roots(l)) + alpha_roots(k));
			}
		}
        denom = -2.0 * std::real(alpha_roots(k)) * denom_product;

        int q = ma_coefs.n_elem;
        std::complex<double> ma_sum1(0.0,0.0);
        std::complex<double> ma_sum2(0.0,0.0);
        for (int l=0; l<q; l++) {
            ma_sum1 += ma_coefs(l) * std::pow(alpha_roots(k),l);
            ma_sum2 += ma_coefs(l) * std::pow(-alpha_roots(k),l);
        }
        numer = ma_sum1 * ma_sum2 * std::exp(alpha_roots(k) * dt);

        car_var += numer / denom;
	}

	// Variance is real-valued, so only return the real part of CARMA_var.
    return sigma * sigma * car_var.real();
}

/*******************************************************************
                        METHODS OF CARMA CLASS
 ******************************************************************/

// Return the starting value and set log_posterior_
arma::vec CARMA::StartingValue()
{
    // Create the parameter vector, theta
    arma::vec theta(p_+q_+3);

    bool good_initials = false;
    while (!good_initials) {

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        arma::vec loga = StartingAR();
        for (int i=0; i<p_; i++) {
            theta(3+i) = loga(i);
        }

        arma::vec log_ma_quad = StartingMA();
        theta(arma::span(p_+3,theta.n_elem-1)) = log_ma_quad;
        arma::vec ma_coefs = ExtractMA(theta);

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        double yvar = RandGen.scaled_inverse_chisqr(y_.n_elem-1, arma::var(y_));

        // Get initial value of the time series mean
        double mu = RandGen.normal(arma::mean(y_), sqrt(yvar) / y_.n_elem);

        arma::cx_vec alpha_roots = ARRoots(theta);
        double sigsqr = yvar / Variance(alpha_roots, ma_coefs, 1.0);
        good_initials = arma::is_finite(sigsqr);
        // Don't run kalman filter if we're already bad_initials

        if (good_initials) {
            // Get initial value of the measurement error scaling parameter by
            // drawing from its prior.

            double measerr_scale = RandGen.scaled_inverse_chisqr(measerr_dof_, 1.0);
            measerr_scale = std::min(measerr_scale, 1.99);
            measerr_scale = std::max(measerr_scale, 0.51);

            theta(0) = sqrt(yvar);
            theta(1) = measerr_scale;
            theta(2) = mu;

            // set the Kalman filter parameters
            pKFilter_->SetSigsqr(sigsqr);
            pKFilter_->SetOmega(ExtractAR(theta));
            pKFilter_->SetMA(ExtractMA(theta));
            arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
            pKFilter_->SetTimeSeriesErr(proposed_yerr);
            arma::vec ycent = y_ - mu;
            pKFilter_->SetTimeSeries(ycent);

            // run the kalman filter
            pKFilter_->Filter();

            double logpost = LogDensity(theta);
            good_initials = arma::is_finite(logpost);
        }
    } // continue loop until the starting values give us a finite posterior

    return theta;
}

arma::vec CARMA::SetStartingValue(arma::vec init)
{
   if (init.n_elem != (p_+q_+3)) {
      std::cout << "WARNING: initial guess wrong length, initializing with prior" << std::endl;
      return StartingValue();
   }

   double logpost = LogDensity(init);
   bool good_initials = arma::is_finite(logpost);
   if (good_initials == false) {
      std::cout << "WARNING: initial guess yields non-finite likelihood, initializing with prior" << std::endl;
      return StartingValue();
   }

   double yvar          = init(0)*init(0);
   double measerr_scale = init(1);
   double mu            = init(2);

   arma::cx_vec alpha_roots = ARRoots(init);
   arma::vec ma_coefs       = ExtractMA(init);
   double sigsqr            = yvar / Variance(alpha_roots, ma_coefs, 1.0);

   // set the Kalman filter parameters
   pKFilter_->SetSigsqr(sigsqr);
   pKFilter_->SetOmega(ExtractAR(init));
   pKFilter_->SetMA(ExtractMA(init));
   arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
   pKFilter_->SetTimeSeriesErr(proposed_yerr);
   arma::vec ycent = y_ - mu;
   pKFilter_->SetTimeSeries(ycent);
   pKFilter_->Filter();

   return init;
}

// get initial guess for the moving average polynomial coefficients
arma::vec CARMA::StartingMA() {
    arma::vec ma_quad(q_);
    ma_quad.randn();
    return arma::abs(ma_quad);
}

// extract the moving-average coefficients from the CARMA parameter vector
arma::vec CARMA::ExtractMA(arma::vec theta)
{
    arma::cx_vec ma_roots(q_);

    // Construct the complex vector of roots of the characteristic polynomial:
    // alpha(s) = s^p + alpha_1 s^{p-1} + ... + alpha_{p-1} s + alpha_p
    for (int i=0; i<q_/2; i++) {
        // alpha(s) decomposed into its quadratic terms:
        //   alpha(s) = (quad_term1 + quad_term2 * s + s^2) * ...
        double quad_term1 = exp(theta(3+p_+2*i));
        double quad_term2 = exp(theta(3+p_+2*i+1));

        double discriminant = quad_term2 * quad_term2 - 4.0 * quad_term1;

        if (discriminant > 0) {
            // two real roots
            double root1 = -0.5 * (quad_term2 + sqrt(discriminant));
            double root2 = -0.5 * (quad_term2 - sqrt(discriminant));
            ma_roots(2*i) = std::complex<double> (root1, 0.0);
            ma_roots(2*i+1) = std::complex<double> (root2, 0.0);
        } else {
            double real_part = -0.5 * quad_term2;
            double imag_part = -0.5 * sqrt(-discriminant);
            ma_roots(2*i) = std::complex<double> (real_part, imag_part);
            ma_roots(2*i+1) = std::complex<double> (real_part, -imag_part);
        }
    }

    if ((q_ % 2) == 1) {
        // p is odd, so add in additional low-frequency component
        double real_root = -exp(theta(3+p_+q_-1));
        ma_roots(q_-1) = std::complex<double> (real_root, 0.0);
    }

    // calculate the coefficients of the polynomial
    //
    //    p(x) = x^q + c_1 * x^{q-1} + ... + c_{q-1} * x + c_q
    //
    // from it roots. note that poly_coefs[0] = 1.0 = c_0.
    arma::vec poly_coefs = polycoefs(ma_roots);

    // convert coefficients to MA polynomial representation:
    //
    //   beta(s) = beta_q * x^q + beta_{q-1} * x^{q-1} + ... + beta_1 x + beta_0,
    //
    // where beta_0 = 1.0.
    //
    poly_coefs = poly_coefs / poly_coefs(q_); // standardize so c_q = 1 instead of c_0;
    arma::vec ma_coefs = arma::zeros(p_);

    // poly_coefs[0]   poly_coefs[1]   ...   poly_coefs[q] = 1.0
    //    ||                ||                     ||
    // ma_coefs[q]    ma_coefs[q-1]    ...    ma_coefs[0]
    for (int i=0; i<q_+1; i++) {
        ma_coefs(i) = poly_coefs(q_-i);
    }

    return ma_coefs;
}

/*******************************************************************
                        METHODS OF ZCARMA CLASS
 *******************************************************************/

arma::vec ZCARMA::StartingValue()
{
    // Create the parameter vector, theta
    arma::vec theta(p_+4);

    bool good_initials = false;
    while (!good_initials) {

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        arma::vec loga = StartingAR();
        for (int i=0; i<p_; i++) {
            theta(3+i) = loga(i);
        }

        theta(3+p_) = logit(StartingKappa());

        // compute the coefficients of the MA polynomial
        arma::vec ma_coefs = ExtractMA(theta);

        // Initial guess for model standard deviation is randomly distributed
        // around measured standard deviation of the time series
        double yvar = RandGen.scaled_inverse_chisqr(y_.n_elem-1, arma::var(y_));

        // Get initial value of the time series mean
        double mu = RandGen.normal(arma::mean(y_), sqrt(yvar) / y_.n_elem);

        arma::cx_vec alpha_roots = ARRoots(theta);
        double sigsqr = yvar / Variance(alpha_roots, ma_coefs, 1.0);

        // Get initial value of the measurement error scaling parameter by
        // drawing from its prior.

        double measerr_scale = RandGen.scaled_inverse_chisqr(measerr_dof_, 1.0);
        measerr_scale = std::min(measerr_scale, 1.99);
        measerr_scale = std::max(measerr_scale, 0.51);

        theta(0) = sqrt(yvar);
        theta(1) = measerr_scale;
        theta(2) = mu;

        // set the Kalman filter parameters
        pKFilter_->SetSigsqr(sigsqr);
        pKFilter_->SetOmega(ExtractAR(theta));
        pKFilter_->SetMA(ma_coefs);
        arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
        pKFilter_->SetTimeSeriesErr(proposed_yerr);
        arma::vec ycent = y_ - mu;
        pKFilter_->SetTimeSeries(ycent);

        // run the kalman filter
        pKFilter_->Filter();

        double logpost = LogDensity(theta);
        good_initials = arma::is_finite(logpost);
    } // continue loop until the starting values give us a finite posterior

    return theta;
}

arma::vec ZCARMA::SetStartingValue(arma::vec init)
{
   if (init.n_elem != (p_+4)) {
      std::cout << "WARNING: initial guess wrong length, initializing with prior" << std::endl;
      return StartingValue();
   }

   double logpost = LogDensity(init);
   bool good_initials = arma::is_finite(logpost);
   if (good_initials == false) {
      std::cout << "WARNING: initial guess yields non-finite likelihood, initializing with prior" << std::endl;
      return StartingValue();
   }

   double yvar          = init(0)*init(0);
   double measerr_scale = init(1);
   double mu            = init(2);

   arma::cx_vec alpha_roots = ARRoots(init);
   arma::vec ma_coefs = ExtractMA(init);
   double sigsqr = yvar / Variance(alpha_roots, ma_coefs, 1.0);

   pKFilter_->SetSigsqr(sigsqr);
   pKFilter_->SetOmega(ExtractAR(init));
   pKFilter_->SetMA(ma_coefs);
   arma::vec proposed_yerr = sqrt(measerr_scale) * yerr_;
   pKFilter_->SetTimeSeriesErr(proposed_yerr);
   arma::vec ycent = y_ - mu;
   pKFilter_->SetTimeSeries(ycent);
   pKFilter_->Filter();

   return init;
}

// get initial guess for the moving average polynomial coefficients, parameterized by kappa
double ZCARMA::StartingKappa() {
    double kappa_normed = RandGen.uniform();
    return kappa_normed;
}

// extract the moving average coefficients from the parameter vector
arma::vec ZCARMA::ExtractMA(arma::vec theta)
{
    double kappa_normed = inv_logit(theta(3 + p_));
    double kappa = (kappa_high_ - kappa_low_) * kappa_normed + kappa_low_;
    // Set the moving average terms
    arma::vec ma_coefs(p_);
	ma_coefs(0) = 1.0;
	for (int i=1; i<p_; i++) {
		ma_coefs(i) = boost::math::binomial_coefficient<double>(p_-1, i) / pow(kappa,i);
	}
    return ma_coefs;
}

/*********************************************************************
                                FUNCTIONS
 ********************************************************************/

double logit(double x) { return log(x / (1.0 - x)); }
double inv_logit(double x) { return exp(x) / (1.0 + exp(x)); }

// Check that all of the roots are unique to within a specified fractional
// tolerance.
bool unique_roots(arma::cx_vec roots, double tolerance)
{
    // Initialize the smallest fractional difference
    double min_frac_diff = 100.0 * tolerance;

    int p = roots.n_elem;

    for (int i=0; i<(p-1); i++) {
        for (int j=i+1; j<p; j++) {
            // Calculate fractional difference between roots(i) and roots(j)
            double frac_diff = std::abs( (roots(i) - roots(j)) /
                                        (roots(i) + roots(j)) );
            if (frac_diff < min_frac_diff) {
                // Found new minimum fractional difference, record it
                min_frac_diff = frac_diff;
            }
        }
    }

    // Test if the roots unique to within the specified tolerance
    bool unique = (min_frac_diff > tolerance);

    return unique;
}

// Return the coefficients of a polynomial given its roots. The polynomial
// is assumed to be of the form:
//
//      p(x) = x^n + c_1 * x^{n-1} + ... + c_{n-1} * x + c_n
//
// where {c_i ; i=1,...,n} are the coefficients. Note that this function
// returns a (n+1)-element column vector, where c_0 = 1.0.

arma::vec polycoefs(arma::cx_vec roots)
{
    arma::cx_vec coefs(roots.n_elem+1);
    coefs.zeros(); // Initialize all values to zero

    coefs(0) = 1.0; // Coefficient for highest order term is set to one

    for (int i=0; i<roots.n_elem; i++) {
        // Calculate the coefficients using a recursion formula
        coefs(arma::span(1,i+1)) = coefs(arma::span(1,i+1)) - roots(i) * coefs(arma::span(0,i));
    }

    // The coefficients must be real, so only return the real part
    return arma::real(coefs);
}


#ifndef ODESOLVERIMEX_HPP_INCLUDED
#define ODESOLVERIMEX_HPP_INCLUDED

#include <petscts.h>
#include <string>
#include <cmath>
#include <vector>
#include <map>
#include <algorithm>
#include <assert.h>
#include <boost/circular_buffer.hpp>
#include "integratorContextImex.hpp"
#include "genFuncs.hpp"

using namespace std;

/*
 * Provides a set of algorithms to solve a system of ODEs of
 * the form y' = f(t,y) using IMEX time stepping,
 * where y is represented as an array of one or more Vecs
 * (PETSc data type).
 *
 * Containers for integration:
 *   var          map<string,Vec> of explicitly integrated variables
 *   varIm    map<string,Vec> of implicitly integrated variables
 *
 * SOLVER TYPE        ALGORITHM
 *  RK32_WBE        explicit part Runge-Kutta (2,3), implicit controlled by user
 *  RK43_WBE        explicit part Runge-Kutta (3,4), implicit controlled by user
 *
 *
 * At minimum, the user must specify:
 *     QUANTITY               FUNCTION
 *  max number of steps      constructor
 *  solver type              constructor
 *  initial conditions       setInitialConds     Note: this array will be modified during integration
 *  initial step size        constructor
 *  step size alg            constructor (this is only used by the adaptive time-stepping algorithm)
 *  f(t,y)                   object passed to integrate must have member function d_dt(PetscScalar, PetscScalar*,PetscScalar*)
 *  final time               constructor and setTimeRange
 *  timeMonitor              object passed to integrate must have member function timeMonitor
 *
 *
 * Optional fields that can also be specified:
 *     QUANTITY               FUNCTION
 *  tolerance                setTolerance
 *  maximum step size        setStepSize
 *  minimum step size        setTimeStepBounds
 *  initial step size        setTimeStepBounds
 *
 * Once the odeSolver context is set, call integrate() to perform
 * the integration.
 *
 * y(t=final time) is stored in the initial conditions array.  Summary output
 * information is provided by viewSolver.  Users can obtain information at
 * each time step within a user-defined monitor function.
 *
 */

class OdeSolverImex
{
public:

  PetscReal               _initT,_finalT,_currT,_deltaT;
  PetscReal          _newDeltaT; // stores future deltaT for access by outside classes, primarily for checkpointing
  PetscInt                _maxNumSteps,_stepCount;
  map<string,Vec>         _varEx,_dvar; // explicit integration variable and rate
  map<string,Vec>         _varIm; // implicit integration variable, once per time step
  vector<string>          _errInds; // which inds of _var to use for error control
  vector<double>          _scale; // scale factor for entries in _errInds
  double                  _runTime;
  string                  _controlType;
  string                  _normType;

  PetscReal   _minDeltaT,_maxDeltaT;
  PetscReal   _totTol; // total tolerance, might be atol, or rtol, or a combination of both
  PetscInt    _numRejectedSteps,_numMinSteps,_numMaxSteps;

  // for PID error control
  boost::circular_buffer<double> _errA;

  // constructor and destructor
  OdeSolverImex(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  virtual ~OdeSolverImex() {};

  // member functions
  PetscErrorCode setInitialStepCount(const PetscReal stepCount);
  PetscErrorCode setToleranceType(const string normType); // type of norm used for error control

  // virtual member functions are declared in base class and redefined in derived class
  virtual PetscErrorCode setTimeRange(const PetscReal initT,const PetscReal finalT) = 0;
  virtual PetscErrorCode setStepSize(const PetscReal deltaT) = 0;
  virtual PetscErrorCode setTolerance(const PetscReal tol) = 0;
  virtual PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT) = 0;
  virtual PetscErrorCode setInitialConds(map<string,Vec>& varEx, map<string,Vec>& varIm) = 0;
  virtual PetscErrorCode setErrInds(vector<string>& errInds) = 0;
  virtual PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale) = 0;

  virtual PetscErrorCode view() = 0;
  virtual PetscErrorCode integrate(IntegratorContextImex *obj) = 0;
  virtual PetscReal computeStepSize(const PetscReal totErr) = 0;
  virtual PetscReal computeError() = 0;
};

// Explicit RK32 scheme from Hairer et al., with added Backward Euler implicit scheme once per time step
// derived class from OdeSolverImex
class RK32_WBE : public OdeSolverImex
{
public:

  // for P or PID error control
  PetscReal   _kappa,_ord; // safety factor in step size determinance, order of accuracy of method
  PetscReal   _totErr; // error between 3rd order solution and embedded 2nd order solution

  // intermediate values for time stepping for the explicit variable
  map<string,Vec> _k1,_f1,_k2,_f2,_y2,_y3;
  map<string,Vec> _vardTIm;

  // constructor and destructor
  RK32_WBE(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~RK32_WBE();

  // member functions
  PetscErrorCode setTimeRange(const PetscReal initT,const PetscReal finalT);
  PetscErrorCode setStepSize(const PetscReal deltaT);
  PetscErrorCode setTolerance(const PetscReal tol);
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT);
  PetscErrorCode setInitialConds(map<string,Vec>& varEx, map<string,Vec>& varIm);
  PetscErrorCode setErrInds(vector<string>& errInds);
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale);
  PetscErrorCode view();
  PetscErrorCode integrate(IntegratorContextImex *obj);
  PetscReal computeStepSize(const PetscReal totErr);
  PetscReal computeError();
};

// Runge-Kutta 4(3) scheme for explicit time integration, with added
// Backward Euler implicit scheme once per time step.
// Based on "ARK4(3)6L[2]SA-ERK" algorithm from Kennedy and Carpenter (2003):
// "Additive Runge-Kutta schemes for convection-diffusion-reaction equations"
// Note: Has matching IMEX equivalent
// derived class from OdeSolverImex
class RK43_WBE : public OdeSolverImex
{
public:

  // for P or PID error control
  // safety factor in step size determinance, order of accuracy of method
  PetscReal   _kappa,_ord;
  // error between 3rd order solution and embedded 2nd order solution
  PetscReal   _totErr;

  // intermediate values for time stepping for the explicit variable
  map<string,Vec> _k1,_k2,_k3,_k4,_k5,_k6,_y4,_y3;
  map<string,Vec> _f1,_f2,_f3,_f4,_f5,_f6;

  // intermediate value for implict variable
  map<string,Vec> _vardTIm;

  // constructor and destructor
  RK43_WBE(PetscInt maxNumSteps,PetscReal finalT,PetscReal deltaT,string controlType);
  ~RK43_WBE();

  // member functions
  PetscErrorCode setTimeRange(const PetscReal initT,const PetscReal finalT);
  PetscErrorCode setStepSize(const PetscReal deltaT);
  PetscErrorCode setTolerance(const PetscReal tol);
  PetscErrorCode setTimeStepBounds(const PetscReal minDeltaT, const PetscReal maxDeltaT);
  PetscErrorCode setInitialConds(map<string,Vec>& varEx, map<string,Vec>& varIm);
  PetscErrorCode setErrInds(vector<string>& errInds);
  PetscErrorCode setErrInds(vector<string>& errInds, vector<double> scale);
  PetscErrorCode view();
  PetscErrorCode integrate(IntegratorContextImex *obj);
  PetscReal computeStepSize(const PetscReal totErr);
  PetscReal computeError();
};

#endif


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/random.hpp>
#include <boost/range.hpp>
#include <boost/iostreams/concepts.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/iostreams/device/back_inserter.hpp>

namespace io = boost::iostreams;
using namespace io;
//using namespace boost;

///////////////////////////////////////

template<typename T = io::source>
class basic_rand_source: public T
{
private:
    std::streamsize  count;
public:
    typedef T super_type;
    typedef typename super_type::char_type char_type;
public:
    basic_rand_source(int c):
        count(c){   }

    std::streamsize read(char_type* s, std::streamsize n)
    {
        using namespace std;

        auto read_count = (min)(n, count);
        if (!read_count)
        {
            return EOF;
        }

        for (streamsize i = 0;i < read_count; ++i)
        {
            *s++ = rand()();
        }

        count -= read_count;
        return read_count;
    }
private:
    typedef boost::variate_generator<boost::rand48, boost::uniform_smallint<>> rand_t;

    static rand_t& rand()
    {
        static rand_t r(boost::rand48(time(0)), boost::uniform_smallint<>('0', 'z'));

        return r;
    }
};

///////////////////////////////////////

void case1()
{
    //typedef std::basic_string<unsigned char> block;

    typedef basic_rand_source<> rand_source;

    string out;
    io::copy(rand_source (20),
        io::back_inserter(out));
    assert(out.size() == 20);

    cout << out << endl;
}

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

void case2()
{
    typedef std::basic_string<unsigned char> block;
    typedef basic_rand_source<io::device<input, unsigned char>> rand_source;

    block out;
    io::copy(rand_source (10),
        io::back_inserter(out));
    assert(out.size() == 10);

    boost::copy(out, ostream_iterator<unsigned char>(cout, " "));
    cout << endl;
}

///////////////////////////////////////

class cout_sink: public io::sink
{
public:
    streamsize write(const char_type* s, streamsize n)
    {
        cout << string(s,n) << endl;
        return n;
    }
};

void case3()
{
    string str("abcd");
    io::copy(
        boost::make_iterator_range(str),
        cout_sink());                             //直接向接收设备输出

}

///////////////////////////////////////

int main()
{
    std::cout << "hello iostreams rand_source" << std::endl;

    case1();
    case2();
    case3();
}


/**
 * aditional functions to plot a graph
 *
 * by R. Falque
 * 20/08/2019
 **/

#ifndef PLOT_GRAPH_HPP
#define PLOT_GRAPH_HPP

#include <string>
#include <Eigen/Core>

#include "graphOptions.hpp"
#include "graph.hpp"
#include "polyscopeWrapper.hpp"

namespace visualization {

    inline void add_graph(libgraphcpp::Graph graph, std::string graph_name) {
        Eigen::MatrixXd nodes = graph.get_nodes();
        Eigen::MatrixXi edges = graph.get_edges();

        visualization::init();
        visualization::add_graph(nodes, edges, graph_name);
    };

    inline void add_graph(libgraphcpp::Graph graph) {
        visualization::add_graph(graph, "graph");
    };

    inline void plot(libgraphcpp::Graph graph, std::string graph_name) {
        visualization::add_graph(graph, graph_name);
        visualization::show();
        visualization::clear_structures();
    };

    inline void plot(libgraphcpp::Graph graph) {
        visualization::plot(graph, "graph");
    };

    inline void return_colors_highlight(std::vector<int> element_to_highlight, int size_array, Eigen::MatrixXd & colors) {
        colors = Eigen::MatrixXd::Constant(size_array,3,0.1);
        for (int element: element_to_highlight) {
            colors(element, 0) = 0.1;
            colors(element, 1) = 0.1;
            colors(element, 2) = 1.0;
        }
    };

    inline void plot_connectivity(libgraphcpp::Graph graph) {

        graph.connectivity_tests();
        Eigen::MatrixXd empty; 			// used to pass empty content
        Eigen::MatrixXd highlights;

        visualization::init();
        visualization::clear_structures();
        visualization::add_graph(graph, "graph_connectivity");

        // add color for the one cut vertices
        std::vector<int> one_cut_vertices;
        graph.is_biconnected(one_cut_vertices);

        return_colors_highlight(one_cut_vertices, graph.num_nodes(), highlights);
        visualization::add_color_to_graph(highlights, empty, "graph_connectivity", "one_cut_vertices");

        // add color for the two cut vertices
        std::vector< std::pair<int, int> > two_cut_vertices;
        graph.is_triconnected(two_cut_vertices);
        std::vector <int> temp;
        for (std::pair<int, int> p: two_cut_vertices) {
            temp.push_back(p.first);
            temp.push_back(p.second);
        }

        return_colors_highlight(temp, graph.num_nodes(), highlights);
        visualization::add_color_to_graph(highlights, empty, "graph_connectivity", "two_cut_vertices");

        // add color for the bridges
        std::vector<int> bridges;
        graph.has_bridges(bridges);

        return_colors_highlight(bridges, graph.num_edges(), highlights);
        visualization::add_color_to_graph(empty, highlights, "graph_connectivity", "bridges");

        visualization::show();
        visualization::clear_structures();
    };

    inline void plot_and_highlight(libgraphcpp::Graph graph, std::vector<int> node_list, std::vector<int> edge_list)
    {
        visualization::init();
        visualization::clear_structures();
        visualization::add_graph(graph, "graph");

        Eigen::MatrixXd nodes_colors, edges_colors;

        return_colors_highlight(node_list, graph.num_nodes(), nodes_colors);
        return_colors_highlight(edge_list, graph.num_edges(), edges_colors);

        visualization::add_color_to_graph(nodes_colors, edges_colors, "graph", "highlight");
        visualization::show();

    };

}

#endif


// Copyright (C) 2008-2018 Lorenzo Caminiti
// Distributed under the Boost Software License, Version 1.0 (see accompanying
// file LICENSE_1_0.txt or a copy at http://www.boost.org/LICENSE_1_0.txt).
// See: http://www.boost.org/doc/libs/release/libs/contract/doc/html/index.html

//[n1962_equal
#include <boost/contract.hpp>
#include <cassert>

// Forward declaration because == and != contracts use one another's function.
template<typename T>
bool operator==(T const& left, T const& right);

template<typename T>
bool operator!=(T const& left, T const& right) {
    bool result;
    boost::contract::check c = boost::contract::function()
        .postcondition([&] {
            BOOST_CONTRACT_ASSERT(result == !(left == right));
        })
    ;

    return result = (left.value != right.value);
}

template<typename T>
bool operator==(T const& left, T const& right) {
    bool result;
    boost::contract::check c = boost::contract::function()
        .postcondition([&] {
            BOOST_CONTRACT_ASSERT(result == !(left != right));
        })
    ;

    return result = (left.value == right.value);
}

struct number { int value; };

int main() {
    number n;
    n.value = 123;

    assert((n == n) == true);   // Explicitly call operator==.
    assert((n != n) == false);  // Explicitly call operator!=.

    return 0;
}
//]


#pragma once

#include "Concepts.hpp"

#include <boost/hana.hpp>

#include <span>
#include <utility>

namespace codys {

namespace detail {

template <TypeIndexedList SystemType, TypeIndexedList ControlsType>
struct StateSpaceSystemIndex {
    constexpr static std::size_t stateSize = decltype(boost::hana::size(
        std::declval<typename SystemType::UnderlyingType>()))::value;

    using UnderlyingType = decltype(boost::hana::concat(
        std::declval<typename SystemType::UnderlyingType>(),
        std::declval<typename ControlsType::UnderlyingType>()));

    template <class WantedStateType> requires SystemStateFor<WantedStateType, SystemType>
    static constexpr std::size_t idx_of() {
        return SystemType::template idx_of<WantedStateType>();
    }

    template <class WantedControlType> requires SystemStateFor<WantedControlType, ControlsType>
    static constexpr std::size_t idx_of() {
        return ControlsType::template idx_of<WantedControlType>() + stateSize;
    }
};

}// namespace detail

template <TypeIndexedList SystemType, TypeIndexedList ControlsType, DerivativeSystemOf<SystemType> StateSpaceType>
struct StateSpaceSystem {
    constexpr static std::size_t stateSize = decltype(boost::hana::size(
        std::declval<typename SystemType::UnderlyingType>()))::value;
    constexpr static std::size_t derivativesSize = decltype(boost::hana::size(
        std::declval<StateSpaceType>().make_dot()))::value;
    constexpr static auto stateIndices = std::make_index_sequence<stateSize>{};

    constexpr static std::size_t controlSize = decltype(boost::hana::size(
        std::declval<typename ControlsType::UnderlyingType>()))::value;

    constexpr static void evaluate(
        std::span<const double, stateSize + controlSize> statesIn,
        std::span<double, stateSize> derivativesOut) {
        constexpr auto dots = StateSpaceType::make_dot();

        boost::hana::for_each(
            stateIndices, [statesIn, derivativesOut, dots](auto idx) {
                constexpr auto outIdx = SystemType::template idx_of<typename std::remove_cvref_t<decltype(boost::hana::at(dots, idx))>::Operand>();
                derivativesOut[outIdx] =
                    boost::hana::at(dots, idx).template evaluate<detail::StateSpaceSystemIndex<SystemType, ControlsType>>(
                        statesIn);
            });
    }
};

} // namespace codys


/*
 * File:   cache.hpp
 * Copyright (C) 2014  K M Masum Habib <masum.habib@gmail.com>
 *
 * Created on January 28, 2014, 2:50 PM
 */

#ifndef CACHE_HPP
#define	CACHE_HPP

#include <armadillo>
#include <stdexcept>
#include "maths/constants.h"
#include "utils/myenums.hpp"

namespace quest{ namespace cache{
using std::out_of_range;
using maths::armadillo::Mat;
using maths::armadillo::field;
using maths::armadillo::dcmplx;

/*
 * A cache of elements stored in a vector.
 */
template <class T>
class Cache{
public:
    Cache(int begin, int end, bool cacheEnabled = true){
        mCacheEnabled = cacheEnabled;
        // Forward moving or backward moving
        if (begin <= end){
            mBegin = begin;
            mEnd = end;

        }else{
            mBegin = end;
            mEnd = begin;
        }

        // Initally we do cache is empty
        mIt = mBegin - 1;

        // Size of the cache
        mLength = mEnd - mBegin + 1;

        if (cacheEnabled == true){
            mM.set_size(mLength);
        }else{
            mM.set_size(1);
        }
    };

    // resets the cache.
    virtual void reset(){
        // reset iterator
        mIt = mBegin - 1;
        mM.reset();
        // reset members.
        if (mCacheEnabled == true){
            mM.set_size(mLength);
        }else{
            mM.set_size(1);
        }
    }

    // () operator is the read only access.
    virtual const T& operator()(int it){
        return getAt(it);
    };

    // convert easy index to array index (starting from 0)
    int toArrayIndx(int it){
        return it - mBegin;
    };

    void enableCache(bool enable = false){
        if (mCacheEnabled != enable){
            mCacheEnabled = enable;
            reset();
        }
    }

    int begin(){ return mBegin; };
    int end(){ return mEnd; };
    int length(){ return mLength; };
    int current(){ return mIt; };

protected:
    T& getAt(int it){
        // within the range
        if (it <= mEnd && it >= mBegin){
            if (mCacheEnabled == true){
                int ii = toArrayIndx(it);
                return mM(ii);
            }else{
                return mM(0);
            }
        }else{
            // out of range, return empty object
            throw out_of_range("In Cache::getAt(i), i out of range.");
        }
    };

public:
protected:
    field<T>    mM;
    int         mIt;
    int         mBegin;
    int         mEnd;
    int         mLength;

    bool      mCacheEnabled;
private:
    Cache();
};

/*
 * Specialization of cache for armadillo matrix
 */
template<typename T>
class MatCache:public Cache<Mat<T> > {
public:
    MatCache( int begin, int end, bool cacheEnabled = true):
        Cache<Mat<T> >(begin, end, cacheEnabled){
    };

protected:
    bool isStored(int it){
        bool result;
        // within the range
        if (it <= this->mEnd && it >= this->mBegin){
            if (this->mCacheEnabled == true){
                int ii = this->toArrayIndx(it);
                if (this->mM(ii).empty()){
                    result = false;
                }else{
                    result = true;
                }
            }else{
                if (it == this->mIt){
                    result = true;
                }else{
                    result = false;
                }
            }
        }else{
            result = false;
        }

        return result;
    };

private:
    MatCache();
};

typedef MatCache<dcmplx> CxMatCache;

}}

#endif	/* CACHE_HPP */


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  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_ARCH_COMMON_SCALAR_FUNCTION_IDIVFIX_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IDIVFIX_HPP_INCLUDED

#include <boost/simd/function/fix.hpp>
#include <boost/simd/function/ifix.hpp>
#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::ifix_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( bd::functor<bs::tag::ifix_> const&
                                    ,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return div(fix, a0, a1);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename A0)
                          , bd::cpu_
                          , bs::tag::ifix_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )

  {
    BOOST_FORCEINLINE bd::as_integer_t<A0> operator() ( bd::functor<bs::tag::ifix_> const&
                                    ,  A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return saturated_(toint)(a0/a1);
    }
  };
} } }

#ifdef BOOST_MSVC
  #pragma warning(pop)
#endif

#endif


/* ================================================================
 *
 * PyCA Project
 *
 * Copyright (c) J. Samuel Preston, Linh K. Ha, Sarang C. Joshi. All
 * rights reserved.  See Copyright.txt or for details.
 *
 * This software is distributed WITHOUT ANY WARRANTY; without even the
 * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  See the above copyright notice for more information.
 *
 * ================================================================ */

#include <GGaussianFilter.h>
#include <boost_mem.h>
#include <GGaussUtils.h>
#include <CudaUtils.h>
#include "SeparableFilter.h"

namespace PyCA {

GGaussianFilter::GGaussianFilter(){
}

void GGaussianFilter::update(){

    GaussianFilterBase<exec_mode>::update();

    mdKx = CreateSharedArray<MEM_DEVICE, float>(mKLength.x);
    mdKy = CreateSharedArray<MEM_DEVICE, float>(mKLength.y);
    mdKz = CreateSharedArray<MEM_DEVICE, float>(mKLength.z);

    mdSx = CreateSharedArray<MEM_DEVICE, float>(mKRadius.x + 1);
    mdSy = CreateSharedArray<MEM_DEVICE, float>(mKRadius.y + 1);
    mdSz = CreateSharedArray<MEM_DEVICE, float>(mKRadius.z + 1);

    GGaussUtils::setConvolutionKernelX(&mKx[0], mKLength.x);
    GGaussUtils::setConvolutionKernelY(&mKy[0], mKLength.y);
    GGaussUtils::setConvolutionKernelZ(&mKz[0], mKLength.z);

    GGaussUtils::setSupplementKernelX(&mSx[0], mKRadius.x + 1);
    GGaussUtils::setSupplementKernelY(&mSy[0], mKRadius.y + 1);
    GGaussUtils::setSupplementKernelZ(&mSz[0], mKRadius.z + 1);

    cpyArrayH2D(mdKx.get(), &mKx[0], mKLength.x);
    cpyArrayH2D(mdKy.get(), &mKy[0], mKLength.y);
    cpyArrayH2D(mdKz.get(), &mKz[0], mKLength.z);

    cpyArrayH2D(mdSy.get(), &mSy[0], mKRadius.y + 1);
    cpyArrayH2D(mdSx.get(), &mSx[0], mKRadius.x + 1);
    cpyArrayH2D(mdSz.get(), &mSz[0], mKRadius.z + 1);

    CudaUtils::CheckCUDAError(__FILE__, __LINE__);
}

void GGaussianFilter::
filter(float *a_o, const float* a_i, float* a_t, StreamT stream){
    GGaussUtils::ConvolutionX3D(a_o,a_i,mKRadius.x,mSize.x,mSize.y,mSize.z, stream);
    GGaussUtils::ConvolutionY3D(a_t,a_o,mKRadius.y,mSize.x,mSize.y,mSize.z, stream);
    GGaussUtils::ConvolutionZ3D(a_o,a_t,mKRadius.z,mSize.x,mSize.y,mSize.z, stream);

    CudaUtils::CheckCUDAError(__FILE__, __LINE__);
}

void GGaussianFilter::
filter2(float *a_o, const float* a_i, float* a_t, StreamT stream){
    SeparableFilter<GGaussianFilter, EXEC_GPU>(*this, a_o, a_i, a_t, mSize, stream);
    CudaUtils::CheckCUDAError(__FILE__, __LINE__);
}

void GGaussianFilter::
convolutionSingleAxis(float* a_o, const float *a_i,
                      size_t sizeX, size_t sizeY, size_t sizeZ,
                      int axis, StreamT stream)
{
   PRECONDITION(sizeX * sizeY * sizeZ == (size_t)mSize.prod(),
		"Incompatible dimensions");

    if (axis == 0) {
        GGaussUtils::ConvolutionX3D(a_o,a_i,
					mdKx.get(),mdSx.get(),mKRadius.x,
					sizeX, sizeY, sizeZ, stream);
    }
    else if (axis == 1) {
        GGaussUtils::ConvolutionX3D(a_o,a_i,
					mdKy.get(),mdSy.get(),mKRadius.y,
					sizeX, sizeY, sizeZ, stream);
    }else if (axis == 2) {
        GGaussUtils::ConvolutionX3D(a_o,a_i,
					mdKz.get(),mdSz.get(),mKRadius.z,
					sizeX, sizeY, sizeZ, stream);
    }

    CudaUtils::CheckCUDAError(__FILE__, __LINE__);
}

} // end namespace PyCA


#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
#include "GSLInterface.h"
#include <gsl/gsl_poly.h>
#include "utils.h"
#include "coordsys.h"
#include "coordtransforms.h"
#include "potential.h"
#include "orbit.h"
#include "aa.h"
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
using namespace boost::python;

namespace {
	boost::python::numeric::array Stack_Triax_Forces(boost::python::numeric::array f){
		StackelTriaxial T(3.61/500.,-30.,-20.);
		VecDoub x(3,0); for(int i=0;i<3;i++)x[i]=extract<double>(f[i]);
		VecDoub v = T.Forces(x);
    	return boost::python::numeric::array(make_tuple(v[0],v[1],v[2]));
	}
	double Stack_Triax_H(boost::python::numeric::array f){
		StackelTriaxial T(3.61/500.,-30.,-20.);
		VecDoub x(6,0);for(int i=0;i<6;i++)x[i]=extract<double>(f[i]);
		return T.H(x);
	}
	boost::python::numeric::array Stack_Triax_Actions(boost::python::numeric::array f){
		StackelTriaxial T(3.61/500.,-30.,-20.);
		VecDoub x(6,0);for(int i=0;i<6;i++)x[i]=extract<double>(f[i]);
		Actions_TriaxialStackel AA(&T);
		VecDoub v = AA.actions(x);
    	return boost::python::numeric::array(make_tuple(v[0],v[1],v[2]));
	}

	boost::python::numeric::array Stack_Triax_Freqs(boost::python::numeric::array f){
		StackelTriaxial T(3.61/500.,-30.,-20.);
		VecDoub x(6,0);for(int i=0;i<6;i++)x[i]=extract<double>(f[i]);
		Actions_TriaxialStackel AA(&T);
		VecDoub v = AA.actions(x,1);
    	return boost::python::numeric::array(make_tuple(v[3],v[4],v[5]));
	}
}

BOOST_PYTHON_MODULE(triax_py){
	boost::python::numeric::array::set_module_and_type("numpy", "ndarray");
    def("Stack_Triax_Forces", Stack_Triax_Forces);
    def("Stack_Triax_H", Stack_Triax_H);
    def("Stack_Triax_Actions", Stack_Triax_Actions);
    def("Stack_Triax_Freqs", Stack_Triax_Freqs);
}


#include "sigen/builder/builder.h"
#include "sigen/common/disjoint_set.h"
#include <algorithm>
#include <boost/foreach.hpp>
#include <cassert>
#include <cmath>
#include <iostream>
#include <map>
#include <queue>
#include <set>
#include <stack>
#include <utility>
#include <vector>
namespace sigen {
void Builder::ConnectNeighbors() {
  std::multimap<IPoint, int> coord_to_index;
  for (int i = 0; i < (int)data_.size(); ++i) {
    BOOST_FOREACH (const IPoint &p, data_[i]->points_) {
      coord_to_index.insert(std::make_pair(p, i));
    }
  }
  for (int i = 0; i < (int)data_.size(); ++i) {
    BOOST_FOREACH (const IPoint &p, data_[i]->points_) {
      for (int dx = -1; dx <= 1; ++dx) {
        for (int dy = -1; dy <= 1; ++dy) {
          for (int dz = -1; dz <= 1; ++dz) {
            const IPoint q(p.x_ + dx, p.y_ + dy, p.z_ + dz);
            typedef std::multimap<IPoint, int>::iterator iter_type;
            std::pair<iter_type, iter_type> range = coord_to_index.equal_range(q);
            for (iter_type it = range.first; it != range.second; ++it) {
              if (it->second != i && data_[i]->HasConnection(data_[it->second]) == false) {
                data_[i]->AddConnection(data_[it->second]);
              }
            }
          }
        }
      }
    }
  }
}

void Builder::CutLoops() {
  assert(is_radius_computed_);
  // use kruskal like algorithm
  // see https://en.wikipedia.org/wiki/Kruskal%27s_algorithm
  DisjointSet<Cluster *> U;
  typedef std::pair<double, std::pair<Cluster *, Cluster *> > item_type;
  std::vector<item_type> E;
  BOOST_FOREACH (ClusterPtr cls, data_) {
    U.Add(cls.get());
    BOOST_FOREACH (Cluster *adj, cls->adjacent_) {
      double strength = (cls->radius_ + adj->radius_) / 2.0;
      Cluster *a = cls.get(), *b = adj;
      if (a < b) {
        E.push_back(std::make_pair(strength, std::make_pair(cls.get(), adj)));
      }
    }
  }
  U.SetUp();
  std::sort(E.begin(), E.end());
  std::reverse(E.begin(), E.end());
  BOOST_FOREACH (const item_type &it, E) {
    Cluster *a = it.second.first;
    Cluster *b = it.second.second;
    if (U.IsSame(a, b)) {
      a->RemoveConnection(b);
      b->RemoveConnection(a);
    } else {
      U.Merge(a, b);
    }
  }
}

static NeuronNode *findEdgeNode(NeuronNode *node) {
  std::queue<NeuronNode *> que;
  NeuronNode *last = node;
  for (int i = 0; i < 2; ++i) {
    std::set<NeuronNode *> used;
    que.push(last);
    used.insert(last);
    while (!que.empty()) {
      NeuronNode *cur = que.front();
      que.pop();
      last = cur;
      BOOST_FOREACH (NeuronNode *next, cur->adjacent_) {
        if (used.count(next)) {
          continue;
        }
        que.push(next);
        used.insert(next);
      }
    }
  }
  return last;
}

void Builder::ComputeGravityPoints() {
  BOOST_FOREACH (ClusterPtr cls, data_) {
    cls->UpdateGravityPoint();
  }
}

void Builder::ComputeRadius() {
  BOOST_FOREACH (ClusterPtr cls, data_) {
    cls->UpdateRadius(scale_xy_, scale_z_);
  }
  is_radius_computed_ = true;
}

std::vector<NeuronNodePtr> Builder::ConvertToNeuronNodes() {
  std::vector<NeuronNodePtr> neuron_nodes;
  BOOST_FOREACH (ClusterPtr p, data_) {
    NeuronNodePtr n = boost::make_shared<NeuronNode>();
    n->setCoord(p->gx_ * scale_xy_,
                p->gy_ * scale_xy_,
                p->gz_ * scale_z_);
    n->radius_ = p->radius_;
    neuron_nodes.push_back(n);
  }
  std::map<Cluster *, int> ptr2index;
  for (int i = 0; i < (int)data_.size(); ++i) {
    ptr2index[data_[i].get()] = i;
  }
  for (int i = 0; i < (int)data_.size(); ++i) {
    BOOST_FOREACH (Cluster *p, data_[i]->adjacent_) {
      int j = ptr2index[p];
      if (i < j) {
        neuron_nodes[i]->AddConnection(neuron_nodes[j]);
        neuron_nodes[j]->AddConnection(neuron_nodes[i]);
      }
    }
  }
  return neuron_nodes;
}

std::vector<Neuron> Builder::ConvertToNeuron() {
  std::vector<NeuronNodePtr> neuron_nodes = ConvertToNeuronNodes();
  // split into some neurons
  std::set<NeuronNode *> used;
  std::vector<Neuron> neurons;
  std::map<NeuronNode *, NeuronNodePtr> ptr2smartptr;

  BOOST_FOREACH (NeuronNodePtr p, neuron_nodes) {
    ptr2smartptr[p.get()] = p;
  }

  BOOST_FOREACH (NeuronNodePtr node, neuron_nodes) {
    if (used.count(node.get())) {
      continue;
    }
    // FIXME
    neurons.push_back(Neuron());
    Neuron &n = neurons.back();
    n.set_root(findEdgeNode(node.get()));
    n.AddNode(node);
    std::stack<NeuronNode *> stk;
    stk.push(node.get());
    used.insert(node.get());
    while (!stk.empty()) {
      NeuronNode *cur = stk.top();
      stk.pop();
      BOOST_FOREACH (NeuronNode *next, cur->adjacent_) {
        // If next != parent
        if (!used.count(next)) {
          n.AddNode(ptr2smartptr[next]);
          stk.push(next);
          used.insert(next);
        }
      }
    }
  }
  return neurons;
}

static void computeIdInner(NeuronNode *cur, NeuronNode *prev, int &id) {
  cur->id_ = id++;
  BOOST_FOREACH (NeuronNode *next, cur->adjacent_) {
    if (next != prev) {
      computeIdInner(next, cur, id);
    }
  }
}

void Builder::ComputeIds(std::vector<Neuron> &neurons) {
  int id = 1;
  for (int i = 0; i < (int)neurons.size(); ++i) {
    computeIdInner(neurons[i].get_root(), NULL, id);
  }
}

static void computeNodeTypesInner(NeuronNode *cur, NeuronNode *prev) {
  cur->UpdateNodeType();
  BOOST_FOREACH (NeuronNode *next, cur->adjacent_) {
    if (next != prev) {
      computeNodeTypesInner(next, cur);
    }
  }
}

void Builder::ComputeNodeTypes(std::vector<Neuron> &neurons) {
  for (int i = 0; i < (int)neurons.size(); ++i) {
    computeNodeTypesInner(neurons[i].get_root(), NULL);
  }
}

std::vector<Neuron> Builder::Build() {
  bool print_progress = true;
  ComputeGravityPoints();
  if (print_progress)
    std::cerr << "compute_gravity_point" << std::endl;
  ComputeRadius();
  if (print_progress)
    std::cerr << "compute_radius" << std::endl;
  ConnectNeighbors();
  if (print_progress)
    std::cerr << "connect_neighbor" << std::endl;
  CutLoops();
  if (print_progress)
    std::cerr << "cut_loops" << std::endl;
  std::vector<Neuron> neurons = ConvertToNeuron();
  if (print_progress)
    std::cerr << "convert_to_neuron" << std::endl;
  ComputeIds(neurons);
  if (print_progress)
    std::cerr << "compute_id" << std::endl;
  ComputeNodeTypes(neurons);
  if (print_progress)
    std::cerr << "compute_node_type" << std::endl;
  return neurons;
}
} // namespace sigen


// Copyright Ryan Schmidt 2011.
// Distributed under the Boost Software License, Version 1.0.
// (See copy at http://www.boost.org/LICENSE_1_0.txt)

#include "opengl.h"
#include "RotInvCoordDeformer.h"
#include "MeshUtils.h"
#include <Wm4LinearSystem.h>
#include <SparseLinearSystem.h>
#include <Solver_TAUCS.h>

#include <Eigen/Core>
#include <Eigen/Sparse>
typedef Eigen::SparseMatrix<double,Eigen::RowMajor> SparseMatrixType;

#include <rmsdebug.h>
#include <rmsprofile.h>

using namespace rms;

RotInvCoordDeformer::RotInvCoordDeformer()
{
	m_pMesh = NULL;
	m_pSolverPos = NULL;
	m_pSystemPos = NULL;
	m_pSolverRot = NULL;
	m_pSystemRot = NULL;
	m_pLs = new gsi::SparseMatrix();
	m_pM = new gsi::SparseMatrix();
	m_pRHSPos = new gsi::Vector[3];
}

RotInvCoordDeformer::~RotInvCoordDeformer()
{
	if ( m_pSolverPos )
		delete m_pSolverPos;
	if ( m_pSystemPos )
		delete m_pSystemPos;
	if ( m_pSolverRot )
		delete m_pSolverRot;
	if ( m_pSystemRot )
		delete m_pSystemRot;
	if ( m_pLs )
		delete m_pLs;
	if ( m_pM )
		delete m_pM;
	if ( m_pRHSPos )
		delete [] m_pRHSPos;
}

gsi::Solver_TAUCS * RotInvCoordDeformer::GetSolverPos()
{
	if ( m_pSolverPos == NULL )
		m_pSolverPos = new gsi::Solver_TAUCS(GetSystemPos());
	return m_pSolverPos;
}
gsi::SparseLinearSystem * RotInvCoordDeformer::GetSystemPos()
{
	if ( m_pSystemPos == NULL )
		m_pSystemPos = new gsi::SparseLinearSystem();
	return m_pSystemPos;
}

gsi::Solver_TAUCS * RotInvCoordDeformer::GetSolverRot()
{
	if ( m_pSolverRot == NULL )
		m_pSolverRot = new gsi::Solver_TAUCS(GetSystemRot());
	return m_pSolverRot;
}
gsi::SparseLinearSystem * RotInvCoordDeformer::GetSystemRot()
{
	if ( m_pSystemRot == NULL )
		m_pSystemRot = new gsi::SparseLinearSystem();
	return m_pSystemRot;
}

void RotInvCoordDeformer::SetMesh(rms::VFTriangleMesh * pMesh)
{
	m_pMesh = pMesh;
	m_vPosConstraints.resize(0);
	m_vRotConstraints.resize(0);
	ComputeWeights();
	m_bMatricesValid = false;
}

void RotInvCoordDeformer::AddBoundaryConstraints(float fWeight)
{
	VFTriangleMesh::vertex_iterator curv(m_pMesh->BeginVertices()), endv(m_pMesh->EndVertices());
	while ( curv != endv ) {
		IMesh::VertexID vID = *curv++;
		if ( m_pMesh->IsBoundaryVertex(vID) ) {
			Wml::Vector3f v;
			m_pMesh->GetVertex(vID, v);
			UpdatePositionConstraint(vID, v, fWeight);
			UpdateOrientationConstraint(vID, m_vVertices[vID].vFrame, fWeight);
		}
	}
}

void RotInvCoordDeformer::UpdatePositionConstraint( IMesh::VertexID vID, const Wml::Vector3f & vPosition, float fWeight )
{
	bool bFound = false;
	size_t nCount = m_vPosConstraints.size();
	for ( unsigned int k = 0; !bFound && k < nCount; ++k ) {
		if ( m_vPosConstraints[k].vID == vID ) {
			m_vPosConstraints[k].vPosition = vPosition;
			if ( m_vPosConstraints[k].fWeight != fWeight )
				m_bMatricesValid = false;
			m_vPosConstraints[k].fWeight = fWeight;
			bFound = true;
		}
	}
	if ( ! bFound ) {
		PosConstraint c;
		c.vID = vID;
		c.vPosition = vPosition;
		c.fWeight = fWeight;
		m_vPosConstraints.push_back(c);
		m_bMatricesValid = false;
	}
}

void RotInvCoordDeformer::UpdateOrientationConstraint( IMesh::VertexID vID, const rms::Frame3f & vFrame, float fWeight )
{
	bool bFound = false;
	size_t nCount = m_vRotConstraints.size();
	for ( unsigned int k = 0; !bFound && k < nCount; ++k ) {
		if ( m_vRotConstraints[k].vID == vID ) {
			m_vRotConstraints[k].vFrame = vFrame;
			if ( m_vRotConstraints[k].fWeight != fWeight )
				m_bMatricesValid = false;
			m_vRotConstraints[k].fWeight = fWeight;
			bFound = true;
		}
	}
	if ( ! bFound ) {
		RotConstraint c;
		c.vID = vID;
		c.vFrame = vFrame;
		c.fWeight = fWeight;
		m_vRotConstraints.push_back(c);
		m_bMatricesValid = false;
	}
}

rms::Frame3f RotInvCoordDeformer::GetCurrentFrame( IMesh::VertexID vID )
{
	Wml::Vector3f vNormal(MeshUtils::EstimateNormal(*m_pMesh, vID));

	// compute frame
	const VtxInfo & vi = m_vVertices[vID];
	Wml::Vector3f vVtx, vNbr;
	m_pMesh->GetVertex(vID, vVtx);
	m_pMesh->GetVertex(vi.nTangentNbr, vNbr);
	Wml::Vector3f xik = vNbr - vVtx;
	Wml::Vector3f xikbar = vNormal.Cross( xik.Cross(vNormal) );
	xikbar.Normalize();
	xik = vNormal.Cross( xikbar );

	rms::Frame3f vFrame(vVtx);
	vFrame.SetFrame( xikbar, xik, vNormal );
	return vFrame;
}
rms::Frame3f RotInvCoordDeformer::GetSolvedFrame( IMesh::VertexID vID )
{
	return m_vVertices[vID].vTransFrame;
}

void RotInvCoordDeformer::SetVertexWeight( IMesh::VertexID vID, float fWeight )
{
	VtxInfo & vi = m_vVertices[vID];
	vi.fVertexWeight = fWeight;
	m_bMatricesValid = false;
}
void RotInvCoordDeformer::SetVertexScale( IMesh::VertexID vID, float fScale )
{
	VtxInfo & vi = m_vVertices[vID];
	vi.fVertexScale = fScale;
}

void RotInvCoordDeformer::ComputeWeights()
{
	unsigned int nVerts = m_pMesh->GetMaxVertexID();
	if ( nVerts != m_pMesh->GetVertexCount() )
		lgBreakToDebugger();		// can happen...

	m_vVertices.resize(0);
	m_vVertices.resize( nVerts );
	m_nEdges = 0;

	m_fAvgVtxArea = 0.0f;
	for ( unsigned int i = 0; i < nVerts; ++i ) {
		VtxInfo & vi = m_vVertices[i];
		vi.vID = i;

		MeshUtils::VertexOneRing(*m_pMesh, vi.vID, vi.vNbrs, false);
		size_t nNbrs = vi.vNbrs.size();
		m_nEdges += (unsigned int)nNbrs;

		MeshUtils::CotangentWeights(*m_pMesh, vi.vID, vi.vNbrs, vi.vNbrWeights);
//		MeshUtils::UniformWeights(*m_pMesh, vi.vID, vi.vNbrs, vi.vNbrWeights);

		Wml::Vector3f vVtx, vNormal, vNbr;
		m_pMesh->GetVertex(vi.vID, vVtx);

		vNormal = MeshUtils::EstimateNormal(*m_pMesh, vi.vID);

		// find most-orthogonal outgoing edge (for tangent-frame calculation)
		float fMinNbrDot = 1.0f;   unsigned int nBestNbr = -1;
		for ( unsigned int k = 0; k < nNbrs; ++k ) {
			m_pMesh->GetVertex( vi.vNbrs[k], vNbr );
			vNbr -= vVtx;   vNbr.Normalize();
			float fDot = (float)fabs(vNbr.Dot(vNormal));
			if ( fDot < fMinNbrDot ) {
				fMinNbrDot = fDot;
				nBestNbr = k;
			}
		}
		if ( nBestNbr == -1 )
			nBestNbr = 0;
		vi.nTangentNbr = vi.vNbrs[nBestNbr];

		// compute frame
		m_pMesh->GetVertex(vi.nTangentNbr, vNbr);
		Wml::Vector3f xik = vNbr - vVtx;
		Wml::Vector3f xikbar = vNormal.Cross( xik.Cross(vNormal) );
		xikbar.Normalize();
		xik = vNormal.Cross( xikbar );
		vi.vFrame.SetFrame( xikbar, xik, vNormal);

		// compute laplacian vector
		vi.vLaplacian = MeshUtils::MeshLaplacian(*m_pMesh, vi.vID, vi.vNbrs, vi.vNbrWeights);
		vi.vFrameLaplacian = vi.vLaplacian;
		vi.vFrame.ToFrameLocal( vi.vFrameLaplacian );

		// compute vertex area
		vi.fVertexArea = 1.0f;
		//vi.fVertexArea = MeshUtils::VertexArea_Mixed(*m_pMesh, vi.vID);
		m_fAvgVtxArea += (vi.fVertexArea / (float)nVerts);

		// additional weighting term
		vi.fVertexWeight = 1.0f;

		// per-vertex scaling factor
		vi.fVertexScale = 1.0f;
	}

	// rescale vertex areas, so that constraint weight scales are not affected
	for ( unsigned int i = 0; i < nVerts; ++i ) {
		VtxInfo & vi = m_vVertices[i];
		vi.fVertexArea /= m_fAvgVtxArea;
	}
}

void RotInvCoordDeformer::UpdateMatrices()
{
	if ( m_bMatricesValid )
		return;

	unsigned int nVerts = (unsigned int)m_vVertices.size();

	/*
	 * build system matrix for orientations
	 */
#if 0
	gsi::SparseMatrix MRot;
	MRot.Resize(3*m_nEdges, 3*nVerts);
	gsi::SparseMatrix RotMInv;
	RotMInv.Resize( 3*m_nEdges, 3*m_nEdges );

	// [TODO] this loop uses vertex ID indexing...need to rewrite w/ vertex<->row map
	unsigned int rij = 0;
	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		IMesh::VertexID i = vi.vID;
		unsigned int ci = 3*i;
		float fVtxWeight = 1.0f / ( vi.fVertexArea ) / vi.fVertexWeight;

		size_t nNbrs = vi.vNbrs.size();
		for ( unsigned int ni = 0; ni < nNbrs; ++ni ) {
			IMesh::VertexID j = vi.vNbrs[ni];
			unsigned int cj = 3*j;
			float fWeight = vi.vNbrWeights[ni];

			Wml::Matrix3f Fi = m_vVertices[i].vFrame.FrameMatrix();
			Wml::Matrix3f Fj = m_vVertices[j].vFrame.FrameMatrix();
			Wml::Matrix3f Rij = Fj.TransposeTimes(Fi);

			for ( int k = 0; k < 3; ++k ) {
				// Mrot(rij+k,ci:ci+2) = Rij(k,:);
				// Mrot(rij+k,cj+k) = -1;
				MRot.Set( rij+k, ci+0, Rij(k, 0) * fWeight );
				MRot.Set( rij+k, ci+1, Rij(k, 1) * fWeight );
				MRot.Set( rij+k, ci+2, Rij(k, 2) * fWeight );
				MRot.Set( rij+k, cj+k, -1 * fWeight);

				RotMInv.Set( rij+k, rij+k, fVtxWeight );
			}
			rij += 3;
		}
	}

	// construct weighted normal equations
	gsi::SparseMatrix MRotTrans;
	MRot.Transpose(MRotTrans);

	//gsi::SparseMatrix MTransTimesMInv;
	//MRotTrans.Multiply(RotMInv, MTransTimesMInv);
	//gsi::SparseMatrix MSysRot( MTransTimesMInv * MRot );
	gsi::SparseMatrix MSysRot;
	MRotTrans.Multiply( MRot, MSysRot );
#endif

#if 1
	SparseMatrixType Rs(3*m_nEdges, 3*nVerts);
	Rs.reserve( (3*m_nEdges)*4 );

	unsigned int rij = 0;
	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		IMesh::VertexID i = vi.vID;
		unsigned int ci = 3*i;
		float fVtxWeight = 1.0f / ( vi.fVertexArea ) / vi.fVertexWeight;

		size_t nNbrs = vi.vNbrs.size();
		for ( unsigned int ni = 0; ni < nNbrs; ++ni ) {
			IMesh::VertexID j = vi.vNbrs[ni];
			unsigned int cj = 3*j;
			float fWeight = vi.vNbrWeights[ni];

			Wml::Matrix3f Fi = m_vVertices[i].vFrame.FrameMatrix();
			Wml::Matrix3f Fj = m_vVertices[j].vFrame.FrameMatrix();
			Wml::Matrix3f Rij = Fj.TransposeTimes(Fi);

			for ( int k = 0; k < 3; ++k ) {
				// Mrot(rij+k,ci:ci+2) = Rij(k,:);
				// Mrot(rij+k,cj+k) = -1;
				Rs.insert( rij+k, ci+0 ) = fWeight * Rij(k, 0);
				Rs.insert( rij+k, ci+1 ) = fWeight * Rij(k, 1);
				Rs.insert( rij+k, ci+2 ) = fWeight * Rij(k, 2);

				Rs.insert( rij+k, cj+k ) = fWeight * -1.0f;
			}
			rij += 3;
		}
	}
	Rs.finalize();
	SparseMatrixType RsT_Rs = SparseMatrixType(Rs.transpose()) * Rs;

	gsi::SparseMatrix MSysRot;
	MSysRot.Resize(3*nVerts,3*nVerts);
	for (unsigned int k=0; k < 3*nVerts; ++k) {
		for ( SparseMatrixType::InnerIterator it(RsT_Rs,k); it; ++it)
			MSysRot.Set( it.row(), it.col(), it.value() );
	}
#endif

	// add soft constraints
	// [TODO] this loop uses vertex ID indexing...need to rewrite w/ vertex<->row map
	size_t nRotCons = m_vRotConstraints.size();
	for ( unsigned int ci = 0; ci < nRotCons; ++ci ) {
		RotConstraint & c = m_vRotConstraints[ci];
		int ri = c.vID * 3;
		for ( int k = 0; k < 3; ++k )
			MSysRot(ri+k,ri+k) = MSysRot(ri+k,ri+k) + c.fWeight*c.fWeight;
	}

	GetSystemRot()->SetMatrix( MSysRot );
	GetSystemRot()->ResizeRHS(3);

	if ( ! GetSystemRot()->Matrix().IsSymmetric() )
		lgBreakToDebugger();

	GetSolverRot()->OnMatrixChanged();
	GetSolverRot()->SetStoreFactorization(true);
	GetSolverRot()->SetSolverMode( gsi::Solver_TAUCS::TAUCS_LLT );
	GetSolverRot()->SetOrderingMode( gsi::Solver_TAUCS::TAUCS_METIS );

	/*
	 * build Laplacian system to find positions
	 */
	GetSystemPos()->Resize(nVerts, nVerts);
	GetSystemPos()->ResizeRHS(3);
	for ( int k = 0; k < 3; ++k )
		m_pRHSPos[k].Resize( nVerts );

#if 0
	_RMSTUNE_start(2);

	gsi::SparseMatrix & Ls = (*m_pLs);
	Ls.Resize(nVerts, nVerts);
	//gsi::SparseMatrix & Minv = (*m_pM);
	//Minv.Resize(nVerts, nVerts);

	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		size_t nNbrs = vi.vNbrs.size();

		double dSum = 0.0f;
		for ( unsigned int k = 0; k < nNbrs; ++k ) {
			Ls(ri, vi.vNbrs[k]) = vi.vNbrWeights[k];
			dSum += vi.vNbrWeights[k];
		}
		Ls(ri, ri) = -dSum;
		//Minv(ri,ri) = ( 1.0f / ( vi.fVertexArea ) ) / vi.fVertexWeight;
	}

	// construct system
	//gsi::SparseMatrix Msys( Ls * Minv * Ls );
	gsi::SparseMatrix Msys( Ls * Ls );

	_RMSTUNE_end(2);
	_RMSInfo("gsi Ls*Ls time was %f\n", _RMSTUNE_time(2));
#endif

#if 1
	_RMSTUNE_start(2);

	SparseMatrixType Ls(nVerts,nVerts);
	Ls.reserve(m_nEdges+nVerts);
	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		size_t nNbrs = vi.vNbrs.size();

		double dSum = 0.0f;
		for ( unsigned int k = 0; k < nNbrs; ++k ) {
			Ls.insert(ri, vi.vNbrs[k]) = vi.vNbrWeights[k];
			dSum += vi.vNbrWeights[k];
		}
		Ls.insert(ri, ri) = -dSum;
		//Minv(ri,ri) = ( 1.0f / ( vi.fVertexArea ) ) / vi.fVertexWeight;
	}
	Ls.finalize();
	SparseMatrixType LsLs = Ls * Ls;

	gsi::SparseMatrix Msys;
	Msys.Resize(nVerts,nVerts);
	for (unsigned int k=0; k < nVerts; ++k) {
		for ( SparseMatrixType::InnerIterator it(LsLs,k); it; ++it)
			Msys.Set( it.row(), it.col(), it.value() );
	}

	_RMSTUNE_end(2);
	_RMSInfo("Eigen Ls*Ls time was %f\n", _RMSTUNE_time(2));
#endif

	// add soft constraints
	unsigned int nCons = (unsigned int)m_vPosConstraints.size();
	for ( unsigned int ci = 0; ci < nCons; ++ci ) {
		PosConstraint & c = m_vPosConstraints[ci];
		int ri = c.vID;
		Msys(ri,ri) = Msys(ri,ri) + c.fWeight*c.fWeight;
	}

	GetSystemPos()->SetMatrix(Msys);

	if ( ! GetSystemPos()->Matrix().IsSymmetric() )
		lgBreakToDebugger();

	GetSolverPos()->OnMatrixChanged();
	GetSolverPos()->SetStoreFactorization(true);
	GetSolverPos()->SetSolverMode( gsi::Solver_TAUCS::TAUCS_LLT );
	GetSolverPos()->SetOrderingMode( gsi::Solver_TAUCS::TAUCS_METIS );

	m_bMatricesValid = true;
}

void RotInvCoordDeformer::UpdateRHSRot()
{
	gsi::SparseLinearSystem * pSystem = GetSystemRot();

	// update soft constraints
	// [TODO] this loop uses vertex ID indexing...need to rewrite w/ vertex<->row map
	size_t nRotCons = m_vRotConstraints.size();
	for ( unsigned int ci = 0; ci < nRotCons; ++ci ) {
		RotConstraint & c = m_vRotConstraints[ci];
		int ri = c.vID * 3;

		Wml::Vector3f vConsValA = c.fWeight*c.fWeight*c.vFrame.X();
		for ( int k = 0; k < 3; ++k )
			pSystem->SetRHS( ri, vConsValA[k], k );

		Wml::Vector3f vConsValB = c.fWeight*c.fWeight*c.vFrame.Y();
		for ( int k = 0; k < 3; ++k )
			pSystem->SetRHS( ri+1, vConsValB[k], k );

		Wml::Vector3f vConsValN = c.fWeight*c.fWeight*c.vFrame.Z();
		for ( int k = 0; k < 3; ++k )
			pSystem->SetRHS( ri+2, vConsValN[k], k );
	}
}

void RotInvCoordDeformer::UpdateRHSPos()
{
	unsigned int nVerts = (unsigned int)m_vVertices.size();

	// transform laplacians to new frames
	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];

		// transform frame-encoded laplacian vector into new 3D frame
		vi.vLaplacian = vi.vFrameLaplacian;
		vi.vTransFrame.ToWorld( vi.vLaplacian );
	}

	float fGlobalScale = GlobalScale();
	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];
		double x[3] = {0,0,0};
		float fWeightSum = 0.0f;
		size_t nNbrs = vi.vNbrs.size();
		for ( unsigned int k = 0; k < nNbrs; ++k ) {
			VtxInfo & ni = m_vVertices[ vi.vNbrs[k] ];
			float fWeight = vi.vNbrWeights[k];
			fWeightSum += fWeight;
			const Wml::Vector3f & vNbrLaplacian = ni.vLaplacian;
			for ( int i = 0; i < 3; ++i ) {
				x[i] += fWeight*vNbrLaplacian[i]*fGlobalScale*ni.fVertexScale;
			}
		}
		const Wml::Vector3f & vLaplacian = vi.vLaplacian;
		for ( int i = 0; i < 3; ++i )
			m_pRHSPos[i][ri] = x[i] - fWeightSum*vLaplacian[i]*fGlobalScale*vi.fVertexScale ;
	}

	// add soft constraints
	unsigned int nCons = (unsigned int)m_vPosConstraints.size();
	for ( unsigned int ci = 0; ci < nCons; ++ci ) {
		PosConstraint & c = m_vPosConstraints[ci];
		int ri = c.vID;
		float fConsWeight = c.fWeight;
		for ( int k = 0; k < 3; ++k )
			m_pRHSPos[k][ri] += c.vPosition[k]*fConsWeight*fConsWeight;
	};

	gsi::SparseLinearSystem * pSystem = GetSystemPos();
	pSystem->SetRHS(0, m_pRHSPos[0]);
	pSystem->SetRHS(1, m_pRHSPos[1]);
	pSystem->SetRHS(2, m_pRHSPos[2]);

#if 0
	gsi::SparseLinearSystem * pSystem = GetSystemPos();

	for ( unsigned int ri = 0; ri < nVerts; ++ri ) {
		VtxInfo & vi = m_vVertices[ri];

		// transform frame-encoded laplacian vector into new 3D frame
		vi.vLaplacian = vi.vFrameLaplacian;
		vi.vTransFrame.ToWorld( vi.vLaplacian );
		Wml::Vector3f vLaplacian = vi.vLaplacian;
		vLaplacian *= GlobalScale() * vi.fVertexScale;
		for ( int k = 0; k < 3; ++k )
			pSystem->SetRHS(ri, vLaplacian[k], k);
	}

	for ( int k = 0; k < 3; ++k ) {
		const gsi::Vector & vRHS = pSystem->GetRHS(k);
		gsi::Vector vLsRHS( (*m_pLs) * vRHS );
		pSystem->SetRHS( k, vLsRHS );
	}

	unsigned int nCons = (unsigned int)m_vPosConstraints.size();
	for ( unsigned int ci = 0; ci < nCons; ++ci ) {
		PosConstraint & c = m_vPosConstraints[ci];
		int ri = c.vID;
		Wml::Vector3f vConsVal = c.fWeight*c.fWeight*c.vPosition;
		for ( int k = 0; k < 3; ++k )
			pSystem->SetRHS( ri, pSystem->GetRHS(ri,k) + vConsVal[k], k );
	};
#endif

}

void RotInvCoordDeformer::Solve()
{
	UpdateMatrices();

	UpdateRHSRot();
	bool bOKRot = GetSolverRot()->Solve();
	if ( ! bOKRot )
		lgBreakToDebugger();

	// extract solved frames
	gsi::SparseLinearSystem * pSystemRot = GetSystemRot();
	int nSize = (int)m_vVertices.size();
	for ( int i = 0; i < nSize; ++i ) {
		VtxInfo & vi = m_vVertices[i];
		int ri = 3*i;
		Wml::Vector3f vFrameV[3];
		for ( int k = 0; k < 3; ++k ) {
			vFrameV[k] = Wml::Vector3f((float)pSystemRot->GetSolution(ri+k,0), (float)pSystemRot->GetSolution(ri+k,1), (float)pSystemRot->GetSolution(ri+k,2) );
			vFrameV[k].Normalize();
		}
		Wml::Vector3f vA = vFrameV[1].Cross(vFrameV[2]);
		vA.Normalize();
		Wml::Vector3f vB = vFrameV[2].Cross( vA );
		vB.Normalize();
		vi.vTransFrame.SetFrame( vA, vB, vFrameV[2] );
	}

	UpdateRHSPos();
	bool bOK = GetSolverPos()->Solve();
	if ( ! bOK )
		lgBreakToDebugger();

	gsi::SparseLinearSystem * pSystem = GetSystemPos();
	int nMatrixCols = (int)m_vVertices.size();
	for ( int i = 0; i < nMatrixCols; ++i ) {
		Wml::Vector3f v((float)pSystem->GetSolution(i,0), (float)pSystem->GetSolution(i,1), (float)pSystem->GetSolution(i,2) );
		m_pMesh->SetVertex(i, v);
	}

}

float RotInvCoordDeformer::GetLaplacianError()
{
	float fErr = 0.0f;
	unsigned int nVerts = m_pMesh->GetMaxVertexID();
	for ( unsigned int i = 0; i < nVerts; ++i ) {
		VtxInfo & vi = m_vVertices[i];
		Wml::Vector3f vCurLaplacian = MeshUtils::MeshLaplacian(*m_pMesh, vi.vID, vi.vNbrs, vi.vNbrWeights);
		Wml::Vector3f vWantLaplacian = vi.vLaplacian * GlobalScale() * vi.fVertexScale;
		//fErr += (vCurLaplacian-vWantLaplacian).SquaredLength();
		vCurLaplacian.Normalize();
		vWantLaplacian.Normalize();
		float fDot = vCurLaplacian.Dot(vWantLaplacian);
		fErr += (1.0f - fDot);
	}
	return fErr;
}

void RotInvCoordDeformer::DebugRender()
{
	glPushAttrib(GL_ENABLE_BIT | GL_POINT_BIT | GL_LINE_BIT);
	glDisable(GL_LIGHTING);

//	glDepthFunc(GL_LEQUAL);

	Wml::Vector3f v;

	// render constraint points
	glPointSize(5.0f);
	glBegin(GL_POINTS);
	for ( unsigned int i = 0; i < m_vPosConstraints.size(); ++i ) {
		glColor3f(0.0f, 1.0f, 0.0f);
		glVertex3fv( m_pMesh->GetVertex(m_vPosConstraints[i].vID) );
		glColor3f(1.0f, 0.0f, 0.0f);
		glVertex3fv(m_vPosConstraints[i].vPosition);
	}
	glEnd();
	glBegin(GL_LINES);
	glColor3f(0.0f, 0.0f, 0.0f);
	for ( unsigned int i = 0; i < m_vPosConstraints.size(); ++i ) {
		glVertex3fv( m_pMesh->GetVertex(m_vPosConstraints[i].vID) );
		glVertex3fv(m_vPosConstraints[i].vPosition);
	}
	glEnd();

	// draw current laplacian vectors
	//glLineWidth(2.0f);
	//glBegin(GL_LINES);
	//glColor3f(1.0f, 0.0f, 0.0f);
	//for ( unsigned int i = 0; i < m_vVertices.size(); ++i ) {
	//	m_pMesh->GetVertex( i, v );
	//	glVertex3fv( v );
	//	glVertex3fv( v + m_vVertices[i].vLaplacian );
	//}
	//glEnd();

	float fLengthScale = 0.005f;
	float fLengthXY = 3 * fLengthScale;
	float fLengthZ = 5 * fLengthScale;

	//// draw initial frames
	//glBegin(GL_LINES);
	//for ( unsigned int i = 0; i < m_vVertices.size(); i += 5 ) {
	//	m_pMesh->GetVertex( i, v );
	//	glColor3f(1,0,0);
	//	glVertex3fv( v );
	//	glVertex3fv( v + fLengthXY*m_vVertices[i].vFrame.X() );
	//	glColor3f(0,1,0);
	//	glVertex3fv( v );
	//	glVertex3fv( v + fLengthXY*m_vVertices[i].vFrame.Y() );
	//	glColor3f(0,0,1);
	//	glVertex3fv( v );
	//	glVertex3fv( v + fLengthZ*m_vVertices[i].vFrame.Z() );
	//}
	//glEnd();

	// draw solved frames
	glLineWidth(4.0f);
	glBegin(GL_LINES);
	for ( unsigned int i = 0; i < m_vVertices.size(); i += 1 ) {
		m_pMesh->GetVertex( i, v );
		glColor3f(1,0,0);
		glVertex3fv( v );
		glVertex3fv( v + fLengthXY*m_vVertices[i].vTransFrame.X() );
		glColor3f(0,1,0);
		glVertex3fv( v );
		glVertex3fv( v + fLengthXY*m_vVertices[i].vTransFrame.Y() );
		glColor3f(0,0,1);
		glVertex3fv( v );
		glVertex3fv( v + fLengthZ*m_vVertices[i].vTransFrame.Z() );
	}
	glEnd();

	glPopAttrib();

}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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 Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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.
 *
*/

#pragma once

#include <Eigen/Core>

#include "kindr/math/LinearAlgebra.hpp"
#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationDiffBase.hpp"
#include "kindr/rotations/Rotation.hpp"
#include "kindr/quaternions/Quaternion.hpp"
#include "kindr/phys_quant/PhysicalQuantities.hpp"

namespace kindr {

/*! \class LocalAngularVelocity
 * \brief Angular velocity in 3D-space expressed in local coordinates (frame fixed to the body).
 *
 * This class implements an angular velocity of a rigid body in 3D-space expressed in body fixed (local) frame.
 *
 * The angular velocity should represent the absolute rotational velocity of a rigid body with respect to an inertial (global) frame I
 * and its coordinates are expressed in the body fixed (local) frame (\f$\B_Omega = B_\omega_{IB}$\f)
 *
 * \tparam PrimType_  Primitive type of the coordinates.
 * \ingroup rotations
 */
template<typename PrimType_>
class LocalAngularVelocity : public RotationDiffBase<LocalAngularVelocity<PrimType_>>, public AngularVelocity<PrimType_, 3> {
 private:
  /*! \brief The base type.
   */
  typedef AngularVelocity<PrimType_, 3> Base;
 public:
  /*! \brief The implementation type.
   *
   *  The implementation type is always an Eigen object.
   */
  typedef typename Base::Implementation Implementation;

  /*! \brief The primitive type of the velocities.
   */
  typedef PrimType_ Scalar;

  /*! \brief Default constructor initializes all velocities with zero.
   */
  LocalAngularVelocity()
    : Base(Base::Zero()) {
  }

  /*! Constructor with three components (x,y,z)
   * \param x   x-coordinate expressed in body fixed (local) frame
   * \param y   y-coordinate expressed in body fixed (local) frame
   * \param z   z-coordinate expressed in body fixed (local) frame
   */
  LocalAngularVelocity(Scalar x, Scalar y, Scalar z)
    : Base(x, y, z) {
  }

  /*! \brief Constructor using vector class.
   *  \param other   AngularVelocity<PrimType_, 3>
   */
  explicit LocalAngularVelocity(const Base& other)
    : Base(other) {
   }

  /*! Constructor with three components (x,y,z)
   * \param vector  Eigen::Matrix<Scalar,3,1>
   */
  LocalAngularVelocity(const Implementation & vector)
    : Base(vector) {
  }

  /*! Constructor with a time derivative of a rotation with a different parameterization
   *
   * \param rotation    rotation the time derivative is taken at
   * \param other       other time derivative
   */
  template<typename RotationDerived_, typename OtherDerived_>
  inline explicit LocalAngularVelocity(const RotationBase<RotationDerived_>& rotation, const RotationDiffBase<OtherDerived_>& other)
    : Base(internal::RotationDiffConversionTraits<LocalAngularVelocity, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived())){
  }

  inline Implementation vector() const {
    return Base::toImplementation();
  }

  inline Implementation& vector() {
    return Base::toImplementation();
  }

  inline Implementation& toImplementation() {
    return Base::toImplementation();
  }

  inline const Implementation& toImplementation() const {
    return Base::toImplementation();
  }

  /*! \brief Cast to the base type.
   *  \returns the base (recommended only for advanced users)
   */
  inline Base& toBase() {
    return static_cast<Base&>(*this);
  }

  /*! \brief Cast to the base type.
   *  \returns the base (recommended only for advanced users)
   */
  inline const Base& toBase() const {
    return static_cast<const Base&>(*this);
  }

  /*! \brief Addition of two angular velocities.
   */
  template<typename Other_>
  LocalAngularVelocity operator +(const Other_& other) const {
    return LocalAngularVelocity(this->toBase() + other.toBase());
  }

  /*! \brief Subtraction of two angular velocities.
   */
  template<typename Other_>
  LocalAngularVelocity operator -(const Other_& other) const {
    return LocalAngularVelocity(this->toBase() - other.toBase());
  }

  /*! \brief Multiplication of an angular velocity with a scalar.
   */
  template<typename PrimTypeFactor_>
  LocalAngularVelocity operator *(const PrimTypeFactor_& factor) const {
    return LocalAngularVelocity(this->toBase()*factor);
  }

  /*! \brief Addition of two angular velocities.
   * \param other   other angular velocity
   */
  template<typename Other_>
  LocalAngularVelocity& operator +=(const Other_& other) {
    this->toBase() += other.toBase();
    return *this;
  }

  /*! \brief Subtraction of two angular velocities.
   * \param other   other angular velocity
   */
  template<typename Other_>
  LocalAngularVelocity& operator -=(const Other_& other) {
    this->toBase() -= other.toBase();
    return *this;
  }

  /*! \brief Sets all components of the angular velocity to zero.
   * \returns reference
   */
  LocalAngularVelocity& setZero() {
    Base::setZero();
    return *this;
  }

  /*! \brief Get zero element.
   * \returns zero element
   */
  static LocalAngularVelocity Zero() {
    return LocalAngularVelocity(Implementation::Zero());
  }
};

/*! \brief Multiplication of an angular velocity with a scalar.
 */
template<typename PrimType_, typename PrimTypeFactor_>
LocalAngularVelocity<PrimType_> operator *(PrimTypeFactor_ factor, const LocalAngularVelocity<PrimType_>& localAngularVelocity) {
  return LocalAngularVelocity<PrimType_>(localAngularVelocity.toBase()*factor);
}

//! \brief 3D angular velocity with primitive type double
typedef LocalAngularVelocity<double>  LocalAngularVelocityPD;
//! \brief 3D angular velocity with primitive type float
typedef LocalAngularVelocity<float>  LocalAngularVelocityPF;
//! \brief 3D angular velocity with primitive type double
typedef LocalAngularVelocity<double>  LocalAngularVelocityD;
//! \brief 3D angular velocity with primitive type float
typedef LocalAngularVelocity<float>  LocalAngularVelocityF;

namespace internal {

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */
template<typename PrimType_, typename Rotation_>
class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, GlobalAngularVelocity<PrimType_>, Rotation_> {
 public:
  inline static LocalAngularVelocity<PrimType_> convert(const Rotation_& rotationLocalToGlobal, const GlobalAngularVelocity<PrimType_>& globalAngularVelocity) {
    return LocalAngularVelocity<PrimType_>(rotationLocalToGlobal.inverseRotate(globalAngularVelocity.toImplementation()));
  }
};

template<typename PrimType_>
class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, RotationQuaternionDiff<PrimType_>, RotationQuaternion<PrimType_>> {
 public:
  inline static LocalAngularVelocity<PrimType_> convert(const RotationQuaternion<PrimType_>& rquat, const RotationQuaternionDiff<PrimType_>& rquatdiff) {
    return LocalAngularVelocity<PrimType_>(2.0*rquat.getLocalQuaternionDiffMatrix()*rquatdiff.toQuaternion().vector());
  }
};

//! \f$B_\hat{w}_IB = (R_IB*dR_IB')$\f
template<typename PrimType_>
class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, RotationMatrixDiff<PrimType_>, RotationMatrix<PrimType_>> {
 public:
  inline static LocalAngularVelocity<PrimType_> convert(const RotationMatrix<PrimType_>& rotationMatrix, const RotationMatrixDiff<PrimType_>& rotationMatrixDiff) {
    return LocalAngularVelocity<PrimType_>(getVectorFromSkewMatrix<PrimType_>(rotationMatrix.inverted().matrix()*rotationMatrixDiff.matrix()));
  }
};

template<typename PrimType_>
class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, EulerAnglesZyxDiff<PrimType_>, EulerAnglesZyx<PrimType_>> {
 public:
  inline static LocalAngularVelocity<PrimType_> convert(const EulerAnglesZyx<PrimType_>& eulerAngles, const EulerAnglesZyxDiff<PrimType_>& eulerAnglesDiff) {
    return LocalAngularVelocity<PrimType_>(eulerAngles.getMappingFromDiffToLocalAngularVelocity()*eulerAnglesDiff.toImplementation());

  }
};

template<typename PrimType_>
class RotationDiffConversionTraits<LocalAngularVelocity<PrimType_>, EulerAnglesXyzDiff<PrimType_>, EulerAnglesXyz<PrimType_>> {
 public:
  inline static LocalAngularVelocity<PrimType_> convert(const EulerAnglesXyz<PrimType_>& eulerAngles, const EulerAnglesXyzDiff<PrimType_>& eulerAnglesDiff) {
    return LocalAngularVelocity<PrimType_>(eulerAngles.getMappingFromDiffToLocalAngularVelocity()*eulerAnglesDiff.toImplementation());

  }
};

} // namespace internal
} // namespace kindr


/* boost random/triangle_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Copyright Steven Watanabe 2011
 * Distributed under 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP
#define BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <iosfwd>
#include <ios>
#include <istream>
#include <boost/assert.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_01.hpp>

namespace lslboost {
namespace random {

/**
 * Instantiations of @c triangle_distribution model a \random_distribution.
 * A @c triangle_distribution has three parameters, @c a, @c b, and @c c,
 * which are the smallest, the most probable and the largest values of
 * the distribution respectively.
 */
template<class RealType = double>
class triangle_distribution
{
public:
    typedef RealType input_type;
    typedef RealType result_type;

    class param_type
    {
    public:

        typedef triangle_distribution distribution_type;

        /** Constructs the parameters of a @c triangle_distribution. */
        explicit param_type(RealType a_arg = RealType(0.0),
                            RealType b_arg = RealType(0.5),
                            RealType c_arg = RealType(1.0))
          : _a(a_arg), _b(b_arg), _c(c_arg)
        {
            BOOST_ASSERT(_a <= _b && _b <= _c);
        }

        /** Returns the minimum value of the distribution. */
        RealType a() const { return _a; }
        /** Returns the mode of the distribution. */
        RealType b() const { return _b; }
        /** Returns the maximum value of the distribution. */
        RealType c() const { return _c; }

        /** Writes the parameters to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._a << " " << parm._b << " " << parm._c;
            return os;
        }

        /** Reads the parameters from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            double a_in, b_in, c_in;
            if(is >> a_in >> std::ws >> b_in >> std::ws >> c_in) {
                if(a_in <= b_in && b_in <= c_in) {
                    parm._a = a_in;
                    parm._b = b_in;
                    parm._c = c_in;
                } else {
                    is.setstate(std::ios_base::failbit);
                }
            }
            return is;
        }

        /** Returns true if the two sets of parameters are equal. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._a == rhs._a && lhs._b == rhs._b && lhs._c == rhs._c; }

        /** Returns true if the two sets of parameters are different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _a;
        RealType _b;
        RealType _c;
    };

    /**
     * Constructs a @c triangle_distribution with the parameters
     * @c a, @c b, and @c c.
     *
     * Preconditions: a <= b <= c.
     */
    explicit triangle_distribution(RealType a_arg = RealType(0.0),
                                   RealType b_arg = RealType(0.5),
                                   RealType c_arg = RealType(1.0))
      : _a(a_arg), _b(b_arg), _c(c_arg)
    {
        BOOST_ASSERT(_a <= _b && _b <= _c);
        init();
    }

    /** Constructs a @c triangle_distribution from its parameters. */
    explicit triangle_distribution(const param_type& parm)
      : _a(parm.a()), _b(parm.b()), _c(parm.c())
    {
        init();
    }

    // compiler-generated copy ctor and assignment operator are fine

    /** Returns the @c a parameter of the distribution */
    result_type a() const { return _a; }
    /** Returns the @c b parameter of the distribution */
    result_type b() const { return _b; }
    /** Returns the @c c parameter of the distribution */
    result_type c() const { return _c; }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _a; }
    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _c; }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_a, _b, _c); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _a = parm.a();
        _b = parm.b();
        _c = parm.c();
        init();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /**
     * Returns a random variate distributed according to the
     * triangle distribution.
     */
    template<class Engine>
    result_type operator()(Engine& eng)
    {
        using std::sqrt;
        result_type u = uniform_01<result_type>()(eng);
        if( u <= q1 )
            return _a + p1*sqrt(u);
        else
        return _c - d3*sqrt(d2*u-d1);
    }

    /**
     * Returns a random variate distributed according to the
     * triangle distribution with parameters specified by param.
     */
    template<class Engine>
    result_type operator()(Engine& eng, const param_type& parm)
    { return triangle_distribution(parm)(eng); }

    /** Writes the distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, triangle_distribution, td)
    {
        os << td.param();
        return os;
    }

    /** Reads the distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, triangle_distribution, td)
    {
        param_type parm;
        if(is >> parm) {
            td.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two distributions will produce identical
     * sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(triangle_distribution, lhs, rhs)
    { return lhs._a == rhs._a && lhs._b == rhs._b && lhs._c == rhs._c; }

    /**
     * Returns true if the two distributions may produce different
     * sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(triangle_distribution)

private:
    /// \cond show_private
    void init()
    {
        using std::sqrt;
        d1 = _b - _a;
        d2 = _c - _a;
        d3 = sqrt(_c - _b);
        q1 = d1 / d2;
        p1 = sqrt(d1 * d2);
    }
    /// \endcond

    RealType _a, _b, _c;
    RealType d1, d2, d3, q1, p1;
};

} // namespace random

using random::triangle_distribution;

} // namespace lslboost

#endif // BOOST_RANDOM_TRIANGLE_DISTRIBUTION_HPP