#ifndef ALM_KATYUSHA_H
#define ALM_KATYUSHA_H

#include "Primal_Dual_LOOPLESS_Katyusha0.h"

#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <sstream>
//#include "cmd_line.h"

//This class implements the method IPALM_Katyusha

/*
The optimization problem to solve is:

min \sum_i f^i(A_ix)+ \sum_i h^i(M_ix)+ P(x)
Assumption 1: For each i, f_i is 1-smooth, P(x) is seperable
// Each subproblem solves problem of the form \sum_i f^i(A_ix)+ \sum_i h^i_{\beta_s}(M_ix;\lambda_s^i) +P(x)+ \beta_s/2\|x-x_s\|^2 by L_Katyuhsa
*/

template<typename L, typename D>
class ALM_Katyusha: public Primal_Dual_LOOPLESS_Katyusha0<L, D>
{
private:

  std::vector<D> Ax_s;
  std::vector<D> old_Ax_s;

  std::vector<D> Mx_s;
  std::vector<D> old_Mx_s;

protected:
  Matrix<L, D>	data_A;

  Matrix<L, D> data_M;

  std::vector<D> x_s;

  std::vector<D> old_x_s;

  std::vector<D> lambda_s;

  std::vector<D> old_lambda_s;

  D beta_s;

  D epsilon_s;

  D m_s;

  D m_0;

  L m_1;

  L m_2;

  L m_3;

  D eta;

  D rho;

  D tau_s;

  std::vector<D> L_phi;

  D function_value;

  L print_every_N_ALM;

  D running_time_ALM;

  L nb_outer_iters;

  ofstream samp_ALM;

  ofstream samp_x_ALM;

public:

  D mu_g;

  D lambda1;

  D lambda2;

  D L_h;

  virtual inline D value_of_f_j(D, L){return D(NULL);}
  virtual inline D value_of_h_j(D, L){return D(NULL);}
  virtual inline D gradient_of_f_j(D, L){return D(NULL);}
  virtual inline D value_of_f_star_j(D, L){return D(NULL);}
  virtual inline D value_of_h_star_j(D, L){return D(NULL);}
  virtual inline D prox_of_h_star_j(D,D, L){return D(NULL);}
  virtual inline D value_of_P_j(D, L){return D(NULL);}
  virtual inline D prox_of_P_j(D, D, L){return D(NULL);}
  virtual inline D feasible_dual(vector<D> &){return D(NULL);}
  virtual inline void compute_just_in_time_prox_grad_without_x_s(D, D, D &, L,L, D, D &, L){}
  virtual inline void rescale(){}
  virtual inline void set_matrix_M(){}
  virtual inline void set_matrix_A(){}

  ALM_Katyusha()
  : Primal_Dual_LOOPLESS_Katyusha0<L,D>(),data_A(),data_M()
  {
  	this->gamma= 1;
  }

  ALM_Katyusha(const char* matrix_file, const char* vector_file)
  : Primal_Dual_LOOPLESS_Katyusha0<L,D>(matrix_file, vector_file),data_A(), data_M()
  {
    this->gamma=1;
  }

  ALM_Katyusha(const char* matrix_file)
  : Primal_Dual_LOOPLESS_Katyusha0<L,D>(matrix_file),data_A(), data_M()
  {
    this->gamma=1;
  }
/*
  ALM_Katyusha(const char* matrix_file, const char* matrix_file2)
  : Primal_Dual_LOOPLESS_Katyusha0<L,D>(),data_A(matrix_file), data_M(matrix_file2)
  {
  	this->matrix_merge(data_A,data_M);
    this->gamma=1;
  }
 */
  inline void set_L_phi(){
  	for(int i=m_3; i<m_2+ m_3; i++){
  		L_phi[i]= this->nsamples;
	  }
  	for(int i= 0; i<m_3; i++){
    	L_phi[i]= this->nsamples/beta_s;
	}
  }

  L get_nb_features(){
    return data_A.get_d();
  }

  inline D value_of_phi_i(D x, L i) {
    if ( i>= data_M.nsamples){
      return value_of_f_j(x, i- data_M.nsamples);
    }
    else{
      D tmp= prox_of_h_star_j(1.0/beta_s*x+ lambda_s[i],beta_s,i);
      return beta_s*(x*tmp- value_of_h_star_j(tmp,i)- beta_s/2*(tmp- lambda_s[i])*(tmp- lambda_s[i]));
    }
  }

  inline D gradient_of_phi_i(D x, L i){
    if (i>= data_M.nsamples){
      return gradient_of_f_j(x,i- data_M.nsamples);
    }
    else{
      D tmp= prox_of_h_star_j(1.0/beta_s*x+ lambda_s[i],beta_s,i);
      return beta_s*tmp;
    }

  }

  inline D value_of_phistar_i(D x, L i) {
    if ( i>= data_M.nsamples){
      return value_of_f_star_j(x,i- data_M.nsamples);
    }
    else{
      return beta_s*value_of_h_star_j(x/beta_s,i)+ 0.5*(x- beta_s*lambda_s[i])*(x- beta_s*lambda_s[i]);
    }
  }

  inline D value_of_g_j(D x, L j){
  	return value_of_P_j(x,j)+ beta_s/2*(x- x_s[j])*(x- x_s[j]);
  }

  inline D value_of_gstar(vector<D> &x){
  	D res= 0;
  	L l= x.size();
  	D tmp= 0;
  	for (L i= 0; i< l; i++){
  		tmp= prox_of_P_j(1.0/beta_s*x[i]+ x_s[i],beta_s,i);
        res+= x[i]*tmp- value_of_P_j(tmp,i)- beta_s/2*(tmp- x_s[i])*(tmp- x_s[i]);
	  }
  	return res;
  }

  inline D gradient_of_gstar_j(D x, L j){
  	return prox_of_P_j(1.0/beta_s*x+ x_s[j],beta_s,j);
  }

  inline D value_of_gstar_minus(vector<D> &x, D scal){
  	L l= x.size();
  	vector<D> tmp(l,0);
  	for (L j= 0;j< l; j++){
  		tmp[j]= -scal*x[j];
	  }
  	return value_of_gstar(tmp);
  }
 /* inline D get_lambda1(){return lambda1;}
  inline D get_lambda2(){return lambda2;}
*/

  void compute_x(){
    for (L i=0; i< this->nfeatures; i++){
      x_s[i]= this->primal_x[i];
    }
    for (L j=m_3; j< m_2+ m_3; j++){
      Ax_s[j- m_3]= compute_AiTx_s(j);
    }
    for (L j=0; j< m_3; j++){
      Mx_s[j]= compute_AiTx_s(j);
	}
  }

  D compute_AiTx_s(L i){
    D res=0;
    for (L k = this->ptr[i]; k < this->ptr[i + 1];k++)
    {
      L j=this->row_idx[k];
      res+= this->A[k]*x_s[j];
    }
    return res;
  }

  inline void compute_just_in_time_prox_grad(D tau, D u, D &x, L t0,L t1, D w, D &y, L j){
    D u_s=u-tau_s*beta_s*x_s[j];
    compute_just_in_time_prox_grad_without_x_s(tau, u_s,  x,  t0,  t1,  w, y,j);
  }

  void compute_function_value(){
    D res= 0;
    for (L i= 0; i< this->nfeatures; i++){
      res+= value_of_P_j(x_s[i],i);
    }
    for (L i= 0; i< data_M.nsamples; i++){
      res+= value_of_h_j(Mx_s[i],i);
    }
    for (L i= 0; i<data_A.nsamples; i++){
      res+= value_of_f_j(Ax_s[i],i);
    }
    function_value= res;
  }

  /*void compute_infeasibility(){
  D tmp1= 0;
  D tmp2= 0;
  for (L j=0; j< this->nsamples; j++){
  if (1- Ax_s[j]> tmp1){
  tmp1= 1- Ax_s[j];
}
tmp2+= max(0.0,1- Ax_s[j]);
}
residual1= tmp1;
residual2= tmp2/this->nsamples;
}*/

inline void compute_m0(D beta0, D val_eta, D val_rho, L val_tau){
  cout<<"beta0="<<beta0<<"val_eta="<<val_eta<<"; val_rho="<<val_rho<<"; val_tau="<<val_tau<<endl;
  m_s= this->nsamples/val_tau*1e+6;

  //D tmp7=1-val_tau/this->n*sqrt(beta0/(max_Lf_s+max_M_s/beta0+beta0));
  //cout<<"tmp7: "<<tmp7<<"; "<<val_tau/(this->n+0.)*sqrt(beta0/(max_Lf_s+max_M_s/beta0+beta0))<<endl;
  //m_0=(2*log(val_rho)+log(val_eta)+log(2))/log(tmp7);

  cout<<" m_s="<<m_s<<endl;
}

void update_y(){
  for(L j=0;j<m_3;j++)
  {
    D aiTx= Mx_s[j];
    lambda_s[j]= prox_of_h_star_j(1.0/beta_s*aiTx+ lambda_s[j],beta_s,j);
  }
}

void Initialize(D beta_0, D epsilon_0,  D val_eta, D val_rho,L val_tau, vector<D> & x0,vector<D> & y0){
  cout<<"start initializing"<<endl;
  set_matrix_M();
  set_matrix_A();

  this->tau=val_tau;
  m_1=data_A.get_d();
  m_2=data_A.get_n();
  m_3=data_M.get_n();
  cout<<"m_1="<<m_1<<endl;
  cout<<"m_2="<<m_2<<endl;
  cout<<"m_3="<<m_3<<endl;

  beta_s=beta_0;
  //tau_s= this->nsamples;
  tau_s= 1;

  epsilon_s=epsilon_0;
  compute_m0(beta_s,val_eta,val_rho,val_tau);

  eta=val_eta;
  rho=val_rho;

  x_s.resize(m_1,0);
  old_x_s.resize(m_1,0);
  lambda_s.resize(m_3,0);
  old_lambda_s.resize(m_3,0);
  for(L i=0;i<m_1;i++){
    x_s[i]=x0[i];
    old_x_s[i]= x0[i];
  }
  for(L j=0;j<m_3;j++){
    lambda_s[j]=y0[j];
    old_lambda_s[j]= y0[j];
  }

  Ax_s.clear();
  Ax_s.resize(m_2,0);
  old_Ax_s.clear();
  old_Ax_s.resize(m_2,0);
  Mx_s.clear();
  Mx_s.resize(m_3,0);
  old_Mx_s.clear();
  old_Mx_s.resize(m_3,0);
  L_phi.clear();
  L_phi.resize(m_2+ m_3,0);
  for(int i=0; i< m_3; i++){
  	L_phi[i]= this->nsamples/beta_0;
  }
  for(int i=m_3; i< m_2+ m_3; i++){
  	L_phi[i]= this->nsamples;
  }
}

void reset_everything(){
  epsilon_s*=rho;
  beta_s*=eta;

}

void update_m_s(L val_tau){
  D tmp= 0;
  D tmpx= 0;
  D tmpy= 0;
  D tmpy2= 0;
  for(L j=0;j<m_3;j++)
  {
    D tmp_p= prox_of_h_star_j(Mx_s[j]/beta_s/eta+ lambda_s[j], beta_s/eta, j);
    tmpy+=(tmp_p- lambda_s[j])*(tmp_p- lambda_s[j]);
    tmp+= (lambda_s[j]- old_lambda_s[j])*(lambda_s[j]- old_lambda_s[j]);
    tmpy2+= (beta_s*eta*lambda_s[j]- beta_s*old_lambda_s[j])*(beta_s*eta*lambda_s[j]- beta_s*old_lambda_s[j]);
  }
  for (L i= 0; i< m_1;i++){
    tmpx= (x_s[i]- old_x_s[i])*(x_s[i]- old_x_s[i]);
  }
  D tmp4= 2*epsilon_s+ beta_s*tmp;
  D tmp5= (1- eta)*beta_s/2*tmpy;
  D tmp6= sqrt(tmp)*beta_s*(1+ eta)*L_h;
  D tmp7= sqrt(tmp)*sqrt(tmpy2);
  D tmp1= log(tmp4+ tmp5+ tmp6+ tmp7+ 2/(2*eta- 1)*tau_s*beta_s/2*tmpx);
  D tmp2= log(epsilon_s)+ log(rho)- log(2);
  D tmp3= sqrt(lambda2/this->sumLi);
  m_s= ceil((tmp1- tmp2)/log(2)*4*max(1.0*this->nsamples,1/tmp3)/val_tau);
}

inline void compute_and_record_res(){
  if(nb_outer_iters%print_every_N_ALM==0){
    compute_function_value();
    cout<<setprecision(9)<<"Iteration: "<<nb_outer_iters<<"; time="<<running_time_ALM<< "; function value="<<function_value<<endl;
    samp_ALM<<setprecision(9)<<nb_outer_iters<<" "<<running_time_ALM<<" "<< function_value<<" "<<endl;
  }
}

void ALM_solve_with_L_Katyusha(D beta_0, D epsilon_0,  D eta, D rho,vector<D> & x0,vector<D> & y0, L val_tau, L max_nb_outer, L p_N_1, L p_N_2, string filename1, string filename2, D time){
  Initialize(beta_0, epsilon_0, eta, rho,val_tau, x0, y0);
  nb_outer_iters=0;
  //string sampname2= ALGparam.data_dir +"/results/L_Katyusha_"+filename2;
  string sampname2= "results/L_Katyusha_"+filename2;
  this->samp.open(sampname2.c_str());
  string sampname_x="results/L_Katyusha_x_"+filename1;
  //filename1= ALGparam.data_dir +"/results/ALM_"+filename1;
  filename1= "results/ALM_"+filename1;
  samp_x_ALM.open(sampname_x.c_str());
  samp_ALM.open(filename1.c_str());
  running_time_ALM=0;
  print_every_N_ALM=p_N_1;
  this->set_print_every_N(p_N_2);
  compute_and_record_res();
  D start;
  start = std::clock();
  //cout<<"m0="<<ceil(m_s/this->n*val_tau)<<endl;
  cout<<"m_s= "<< ceil(m_s/this->nsamples*val_tau)<<"; beta_s="<<beta_s<<"; epsilon_s="<<epsilon_s<<endl;
  //this->L_Katyusga_MU(x_s, val_tau, val_mu_f, val_mu_g, 3, p_N_2, ceil(m_s/this->n*val_tau), epsilon_s, filename2,1);
  rescale();
  set_L_phi();
  this->loopless_Katyusha(y0, x_s, filename2, L_phi, mu_g+ tau_s*beta_s, epsilon_s, ceil(m_s/this->nsamples*val_tau), val_tau, 1, 1, 0, 1);
  for(L i=0;i<m_1;i++){
    old_x_s[i]=x_s[i];
  }
  for(L i=0;i<m_2;i++){
    old_Ax_s[i]=Ax_s[i];
  }
  compute_x();
  for(L i=0;i<m_3;i++){
    old_lambda_s[i]=lambda_s[i];
  }
  update_y();
  update_m_s(val_tau);
  nb_outer_iters++;
  running_time_ALM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  compute_and_record_res();
  start = std::clock();
  reset_everything();
  running_time_ALM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  while(nb_outer_iters<max_nb_outer){
    start = std::clock();
    cout<<"ms="<<ceil(m_s/this->nsamples*val_tau)<<"; beta_s="<<beta_s<<"; epsilon_s"<<epsilon_s<<endl;
    rescale();
    set_L_phi();
    this->loopless_Katyusha(y0, x_s, filename2, L_phi, mu_g+ tau_s*beta_s, epsilon_s, ceil(m_s/this->nsamples*val_tau), val_tau, 1, 1, 0, 1);
    for(L i=0;i<m_1;i++){
      old_x_s[i]=x_s[i];
    }
    for(L i=0;i<m_2;i++){
      old_Ax_s[i]=Ax_s[i];
    }
    for(L i=0;i<m_3;i++){
      old_lambda_s[i]=lambda_s[i];
    }
    compute_x();
    update_y();
    update_m_s(val_tau);
    nb_outer_iters++;
    running_time_ALM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    compute_and_record_res();
    start = std::clock();
    reset_everything();
    running_time_ALM+=( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    if (running_time_ALM> time){
      break;
    }
  }
  for (L i= 0; i< m_1; i++){
      	samp_x_ALM<< x_s[i]<< endl;
  }
}
};

#endif /* MIN_SMOOTH_CONVEX_H */


#ifndef _PROBABILITY_H_
#define _PROBABILITY_H_

#include <gsl/gsl_rng.h>
// Non-uniform distributions:
//There are a HUGE variety of distributions to choose from, which are
//  documented at the gsl website.
#include <gsl/gsl_randist.h>
// Seeding:
#include <sys/time.h>

//When using ANY gsl code, must add -lgsl -lgslcblas [-lm ???] to compile lines
//USE_LINK_FLAG -lgsl -lgslcblas

//RandGen is a virtual class.  Use it in function definitions,
//  but when DECLARING a random number generator, use one of its derived
//  classes, such as gsl_RandGen.
#define UNUSED_ARGUMENT(x) (void)x

class RandGen
{
 public:
  //Create a random number generator with Seed generated by system clock:
  RandGen() {}
  //Create a random number generator, and specify seed
  RandGen(const long & seed) {UNUSED_ARGUMENT(seed);}
  virtual ~RandGen() {}

  //uniform double on [0, 1):
  virtual double uniform(void) = 0;
  //uniform double on (0, 1):
  virtual double openUniform(void) = 0;
  //unsigned long int on [0, 1, 2, ..., n - 1]:
  virtual unsigned long unsignedInt(const unsigned long & n) = 0;
  // bool, true with probability p, false with probability 1-p:
  virtual bool bernoulli(const double & p) = 0;
  //exponentially distributed, p(x) ~ exp(-x/mu)
  virtual double exponential(const double & mu) = 0;
  //gaussian double with zero mean and standard deviation sigma:
  virtual double gaussian(const double & sigma) = 0;
  //poisson unsigned int with mean Mu:
  virtual unsigned int poisson(const double & mu) = 0;
  //binomial unsigned int for n trials with probability P of success
  virtual unsigned int binomial(const double & p, unsigned intnN) = 0;
};

class GslRandGen : public RandGen
{
 public:
  GslRandGen();
  GslRandGen(const long & seed);
  virtual ~GslRandGen();

  //uniform double on [0, 1):
  virtual double uniform(void);
  //uniform double on (0, 1):
  virtual double openUniform(void);
  //unsigned long int on [0, 1, 2, ..., n - 1]:
  virtual unsigned long unsignedInt(const unsigned long & n);

  // bool, true with probability p, false with probability 1-p:
  bool bernoulli(const double & p);

  //exponentially distributed, p(x) ~ exp(-x/mu)
  double exponential(const double & mu);
  //gaussian double with zero mean and standard deviation sigma:
  double gaussian(const double & sigma);
  //poisson unsigned int with mean Mu:
  unsigned int poisson(const double & mu);
  //binomial unsigned int for n trials with probability P of success
  unsigned int binomial(const double & p, unsigned intnN);

 private:
  gsl_rng* r;
  static bool firstInit;
};

#endif


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#pragma once

#include "Data/Data.h"
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>

#include <assert.h>
#include <array>
#include <gsl/gsl>

#include "eigen.h"

namespace mage
{
    using Quaternion = Eigen::Quaternionf;

    inline Quaternion ToQuat(const cv::Matx33f& rotation)
    {
        Eigen::Matrix3f eigenMat;
        cv::cv2eigen(rotation, eigenMat);

        Quaternion eigenQuat(eigenMat);
        // TODO validate if this normalize is even necessary, eigen might already be doing this
        eigenQuat.normalize();

        return eigenQuat;
    }

    template<int N>
    inline Eigen::Map<Eigen::Matrix<float, N, 1>> ToMap(cv::Vec<float, N>& vec)
    {
        return Eigen::Map<Eigen::Vector3f>{ vec.val };
    }

    template<int N>
    inline Eigen::Map<const Eigen::Matrix<float, N, 1>> ToCMap(const cv::Vec<float, N>& vec)
    {
        return Eigen::Map<const Eigen::Matrix<float, N, 1>>{ vec.val };
    }

    inline Eigen::Map<Eigen::Vector3f> ToMap(cv::Matx31f& vec)
    {
        return Eigen::Map<Eigen::Vector3f>{ vec.val };
    }

    inline Eigen::Map<const Eigen::Vector3f> ToCMap(const cv::Matx31f& vec)
    {
        return Eigen::Map<const Eigen::Vector3f>{ vec.val };
    }

    inline Eigen::Map<Eigen::Vector3f> ToMap(cv::Point3f& vec)
    {
        return Eigen::Map<Eigen::Vector3f>{ &vec.x };
    }

    inline Eigen::Map<const Eigen::Vector3f> ToCMap(const cv::Point3f& vec)
    {
        return Eigen::Map<const Eigen::Vector3f>{ &vec.x };
    }

    inline Eigen::Map<Eigen::Vector2f> ToMap(cv::Point2f& vec)
    {
        return Eigen::Map<Eigen::Vector2f>{ &vec.x };
    }

    inline Eigen::Map<const Eigen::Vector2f> ToCMap(const cv::Point2f& vec)
    {
        return Eigen::Map<const Eigen::Vector2f>{ &vec.x };
    }

    template<size_t N>
    inline cv::Vec<float, N> ToVec(gsl::span<const float, N> values)
    {
        return cv::Vec<float, N>(values.data());
    }

    inline Quaternion QuatFromTwoVectors(const cv::Vec3f& from, const cv::Vec3f& to)
    {
        Quaternion quat;
        quat.setFromTwoVectors(Eigen::Vector3f{ from(0), from(1), from(2) }, Eigen::Vector3f{ to(0), to(1), to(2) });
        return quat;
    }

    inline cv::Matx33f ToMat(const Quaternion& quat)
    {
        cv::Matx33f mat;
        cv::eigen2cv(quat.toRotationMatrix(), mat);
        return mat;
    }

    inline cv::Matx33f ToMat(float pitchRads, float rollRads, float yawRads)
    {
        // Formula for (Pitch * Roll * Yaw) from http://www.songho.ca/opengl/gl_anglestoaxes.html
        float a = pitchRads;
        float b = yawRads;
        float c = rollRads;

        float sa = sinf(a);
        float sb = sinf(b);
        float sc = sinf(c);

        float ca = cosf(a);
        float cb = cosf(b);
        float cc = cosf(c);

        return cv::Matx33f{
            cc * cb,                  -sc,         cc * sb,
            ca * sc * cb + sa * sb,   ca * cc,     ca * sc * sb - sa * cb,
            sa * sc * cb - ca * sb,   sa * cc,     sa * sc * sb + ca * cb
        };
    }

    inline cv::Matx44f To4x4(const cv::Matx34f& mat)
    {
        return{
            mat(0, 0), mat(0, 1), mat(0, 2), mat(0, 3),
            mat(1, 0), mat(1, 1), mat(1, 2), mat(1, 3),
            mat(2, 0), mat(2, 1), mat(2, 2), mat(2, 3),
            0,         0,         0,         1
        };
    }

    inline cv::Matx34f To3x4(const cv::Matx44f& mat)
    {
        return{
            mat(0, 0), mat(0, 1), mat(0, 2), mat(0, 3),
            mat(1, 0), mat(1, 1), mat(1, 2), mat(1, 3),
            mat(2, 0), mat(2, 1), mat(2, 2), mat(2, 3)
        };
    }

    inline cv::Matx44f To4x4(const cv::Matx33f& rot, const cv::Vec3f& trans)
    {
        return{
            rot(0, 0), rot(0, 1), rot(0, 2), trans(0),
            rot(1, 0), rot(1, 1), rot(1, 2), trans(1),
            rot(2, 0), rot(2, 1), rot(2, 2), trans(2),
            0,         0,         0,         1
        };
    }

    inline cv::Matx44f FromQuatAndTrans(const Quaternion& quat, const cv::Vec3f& trans)
    {
        return To4x4(ToMat(quat), trans);
    }

    inline cv::Matx33f Rotation(const cv::Matx44f& mat)
    {
        return{
            mat(0, 0), mat(0, 1), mat(0, 2),
            mat(1, 0), mat(1, 1), mat(1, 2),
            mat(2, 0), mat(2, 1), mat(2, 2)
        };
    }

    inline cv::Matx33f Rotation(const cv::Matx34f& mat)
    {
        return{
            mat(0, 0), mat(0, 1), mat(0, 2),
            mat(1, 0), mat(1, 1), mat(1, 2),
            mat(2, 0), mat(2, 1), mat(2, 2)
        };
    }

    inline cv::Vec3f Translation(const cv::Matx44f& mat)
    {
        return{
            mat(0, 3),
            mat(1, 3),
            mat(2, 3)
        };
    }

    inline cv::Vec3f Translation(const cv::Matx34f& mat)
    {
        return{
            mat(0, 3),
            mat(1, 3),
            mat(2, 3)
        };
    }
	//Swap the returned parameters to suppress x64 compile warning c4324
	//c:\program files(x86)\microsoft visual studio\2017\enterprise\vc\tools\msvc\14.11.25503\include\utility(271) : warning C4324 : 'std::pair<mage::Quaternion,cv::Vec3f>' : structure was padded due to alignment specifier
    inline std::pair<cv::Vec3f, Quaternion> Decompose(const cv::Matx44f& matrix)
    {
        return std::make_pair(Translation(matrix), ToQuat(matrix.get_minor<3, 3>(0, 0)));
    }

    template<int ROWS, int COLS>
    inline bool MatxEqual(const cv::Matx<float, ROWS, COLS>& mat0, const cv::Matx<float, ROWS, COLS>& mat1)
    {
        for (int i = 0; i < ROWS; i++)
        {
            for (int j = 0; j < COLS; j++)
            {
                if (mat0(i,j) != mat1(i,j))
                    return false;
            }
        }

        return true;
    }

    inline bool MatEqual(const cv::Mat& mat0, const cv::Mat& mat1)
    {
        if (mat0.size != mat1.size)
            return false;

        for (int i = 0; i < mat0.rows; i++)
        {
            for (int j = 0; j < mat0.cols; j++)
            {
                if (mat0.at<float>(i, j) != mat1.at<float>(i, j))
                    return false;
            }
        }

        return true;
    }

    template<int ROWS, int COLS>
    inline cv::Matx44f Invert(const cv::Matx<float, ROWS, COLS>& transform)
    {
        static_assert((ROWS == 3 || ROWS == 4) && COLS == 4, "Invert only supports 3x4 or 4x4 matrices");

        //invert rotation by transpose on copy of upper 3x3 for inverse rotation
        cv::Matx44f invRotation{
            transform(0, 0), transform(1, 0), transform(2, 0), 0,
            transform(0, 1), transform(1, 1), transform(2, 1), 0,
            transform(0, 2), transform(1, 2), transform(2, 2), 0,
                          0,               0,               0, 1
        };

#ifndef NDEBUG
        // this approach would only work if the upper 3x3 was a pure rotation matrix (no scale)
        // that would mean all rows and columns were unit length and orthogonal. this tests for the former.
        for(int idx=0; idx < 3; idx++)
        {
            assert(abs(1.0 - cv::norm(invRotation.col(idx))) <= 0.001 && "Expecting unit length (no scale) for rotation matrix");
            assert(abs(1.0 - cv::norm(invRotation.row(idx))) <= 0.001 && "Expecting unit length (no scale) for rotation matrix");
        }

        //a valid rotation matrix has determinant 1 (a mirror would be -1)
        assert(abs(1.0 - cv::determinant(invRotation)) < 0.001  && "Expecting determinant of 1 for rotation matrix");
#endif

        //invert translation by negation on copy
        cv::Matx44f invTranslation{
            1, 0, 0, -transform(0, 3),
            0, 1, 0, -transform(1, 3),
            0, 0, 1, -transform(2, 3),
            0, 0, 0,               1
        };

        //inverse matrix
        return invRotation * invTranslation;
    }

    inline cv::Matx44f ComputeFrameTransform(const cv::Matx44f& from, const cv::Matx44f& to)
    {
        return to * Invert(from);
    }

    inline cv::Point3f UnProject(const cv::Matx33f& invCameraMatrix, const cv::Matx44f& viewMatrix, const cv::Point2i& point, float depth)
    {
        cv::Matx31f pixelSpace{ static_cast<float>(point.x), static_cast<float>(point.y), 1.f };
        cv::Matx31f cameraSpace = invCameraMatrix * pixelSpace;
        cameraSpace *= depth;

        cv::Matx41f world = Invert(viewMatrix) * cv::Matx41f{ cameraSpace(0), cameraSpace(1), cameraSpace(2), 1 };
        return{ world(0), world(1), world(2) };
    }

    const float NRM_EPSILON = 0.00001f;

    inline float Length(const cv::Vec3f& vec)
    {
        return std::sqrtf(vec.dot(vec));
    }

    inline cv::Vec3f Normalize(const cv::Vec3f& vec)
    {
        float distance = Length(vec);
        if (distance == 0)
            return vec;

        cv::Vec3f normVec = (vec / distance);
        assert(abs(sqrt(normVec.dot(normVec)) - 1.0f) < NRM_EPSILON);
        return normVec;
    }

    //rightHanded, column vec convention, intended for rotation of vectors (active)
    //https://en.wikipedia.org/wiki/Rotation_matrix
    //vs http://mathworld.wolfram.com/RotationMatrix.html intended for rotation of coordinate frames
    inline cv::Matx44f RotationXForVectors(float radians)
    {
        float cosR = std::cosf(radians);
        float sinR = std::sinf(radians);

        return{ 1.0f, 0.0f, 0.0f, 0.0f,
                 0.0f, cosR, -sinR, 0.0f,
                 0.0f, sinR, cosR, 0.0f,
                 0.0f, 0.0f, 0.0f, 1.0f };
    }

    //rightHanded, column vec convention, intended for rotation of vectors (active)
    //https://en.wikipedia.org/wiki/Rotation_matrix
    //vs http://mathworld.wolfram.com/RotationMatrix.html intended for rotation of coordinate frames
    inline cv::Matx44f RotationYForVectors(float radians)
    {
        float cosR = std::cosf(radians);
        float sinR = std::sinf(radians);

        return{ cosR, 0.0f, sinR, 0.0f,
                0.0f, 1.0f, 0.0f, 0.0f,
                -sinR, 0.0f, cosR, 0.0f,
                0.0f, 0.0f, 0.0f, 1.0f };
    }

    //rightHanded, column vec convention, intended for rotation of vectors (active)
    //https://en.wikipedia.org/wiki/Rotation_matrix
    //vs http://mathworld.wolfram.com/RotationMatrix.html intended for rotation of coordinate frames
    inline cv::Matx44f RotationZForVectors(float radians)
    {
        float cosR = std::cosf(radians);
        float sinR = std::sinf(radians);

        return{ cosR, -sinR, 0.0f, 0.0f,
                sinR, cosR, 0.0f, 0.0f,
                0.0f, 0.0f, 1.0f, 0.0f,
                0.0f, 0.0f, 0.0f, 1.0f };

    }

    //rightHanded, column vec convention, intended for rotation of coordinate frames (passive)
    //https://en.wikipedia.org/wiki/Rotation_matrix
    //ala http://mathworld.wolfram.com/RotationMatrix.html
    inline cv::Matx44f RotationXForCoordinateFrames(float radians)
    {
        float cosR = std::cosf(radians);
        float sinR = std::sinf(radians);

        return{ 1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, cosR, sinR, 0.0f,
            0.0f, -sinR, cosR, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f };
    }

    //rightHanded, column vec convention,intended for rotation of coordinate frames (passive)
    //https://en.wikipedia.org/wiki/Rotation_matrix
    //ala http://mathworld.wolfram.com/RotationMatrix.html
    inline cv::Matx44f RotationYForCoordinateFrames(float radians)
    {
        float cosR = std::cosf(radians);
        float sinR = std::sinf(radians);

        return{ cosR, 0.0f, -sinR, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            sinR, 0.0f, cosR, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f };
    }

    //rightHanded, column vec convention, intended for rotation of coordinate frames (passive)
    //https://en.wikipedia.org/wiki/Rotation_matrix
    //ala http://mathworld.wolfram.com/RotationMatrix.html
    inline cv::Matx44f RotationZForCoordinateFrames(float radians)
    {
        float cosR = std::cosf(radians);
        float sinR = std::sinf(radians);

        return{ cosR, sinR, 0.0f, 0.0f,
            -sinR, cosR, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f };
    }

    // encoding of axis/angle in a vec3f
    // http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#rodrigues
    inline cv::Matx33f RotationFromRodrigues(const cv::Matx31f& rotation)
    {
        cv::Matx33f rotationMat;
        cv::Rodrigues(rotation, rotationMat);
        return rotationMat;
    }

    template<typename T, size_t M, size_t N>
    inline std::array<T, M*N> ArrayFromMat(const cv::Matx<T, M, N>& mat)
    {
        std::array<T, M * N> out;
        std::copy(std::begin(mat.val), std::end(mat.val), out.begin());
        return out;
    }

    inline cv::Matx44f ToCVMat4x4(const mage::Matrix& matrix)
    {
        return cv::Matx44f(&matrix.M11);
    }

    inline bool IsEntirelyOffscreen(const cv::Rect& rect, const cv::Size& screenResPixels)
    {
        assert(rect.width > 0 && "invalid rect");
        assert(rect.height > 0 && "invalid rect");
        assert(screenResPixels.width > 0 && "invalid screenResPixels");
        assert(screenResPixels.height > 0 && "invalid screenResPixels");

        int maxHorizPixel = rect.width + rect.x - 1;
        int maxVerticalPixel = rect.height + rect.y - 1;

        bool offscreenX = (maxHorizPixel < 0) || (rect.x >(int)(screenResPixels.width - 1));
        bool offscreenY = (maxVerticalPixel < 0) || (rect.y >(int)(screenResPixels.height - 1));

        return (offscreenX || offscreenY);
    }

    inline bool IsWithinArea(const cv::Point2f& pt, const cv::Rect& crop)
    {
        assert(crop.width > 0 && "width must be positive nonzero");
        assert(crop.height > 0 && "height must be positive nonzero");

        return (pt.x >= crop.x && pt.x < (crop.x+crop.width)) && (pt.y >= crop.y && pt.y <= (crop.y + crop.height));
    }

    inline cv::Rect ToCVRect(const mage::Rect& rect)
    {
        return { (int)rect.X, (int)rect.Y, (int)rect.Width, (int)rect.Height };
    }

    cv::Mat CreateGrayCVMat(const cv::Size& resolution, const PixelFormat& format, const gsl::span<const uint8_t> imageBytes);
    cv::Mat CreateBGRCVMat(const cv::Mat& source, const PixelFormat& format);
}


#include <gsl/gsl_rng.h>
#include "gsl-sprng.h"
#include <gsl/gsl_randist.h>
#include <mpi.h>

int main(int argc,char *argv[])
{
  int k,i,iters; double x,can,a,alpha; gsl_rng *r;
  FILE *s; char filename[15];
  MPI_Init(&argc,&argv);
  MPI_Comm_rank(MPI_COMM_WORLD,&k);
  if ((argc != 3)) {
    if (k == 0)
      fprintf(stderr,"Usage: %s <iters> <alpha>\n",argv[0]);
    MPI_Finalize(); return(EXIT_FAILURE);
  }
  iters=atoi(argv[1]); alpha=atof(argv[2]);
  r=gsl_rng_alloc(gsl_rng_sprng20);
  sprintf(filename,"chain-%03d.tab",k);
  s=fopen(filename,"w");
  if (s==NULL) {
    perror("Failed open");
    MPI_Finalize(); return(EXIT_FAILURE);
  }
  x = gsl_ran_flat(r,-20,20);
  fprintf(s,"Iter X\n");
  for (i=0;i<iters;i++) {
    can = x + gsl_ran_flat(r,-alpha,alpha);
    a = gsl_ran_ugaussian_pdf(can) / gsl_ran_ugaussian_pdf(x);
    if (gsl_rng_uniform(r) < a)
      x = can;
    fprintf(s,"%d %f\n",i,x);
  }
  fclose(s);
  MPI_Finalize(); return(EXIT_SUCCESS);
}


#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <virginian.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

int main(int argc, char **argv)
{
	if(argc != 3) {
		fprintf(stderr, "%s <database name> <rows>\n", argv[0]);
		exit(1);
	}

	unlink(argv[1]);
	virginian virg;
	virginian *v = &virg;

	virg_init(v);
	virg_db_create(v, argv[1]);
	virg_table_create(v, "test", VIRG_INT);
	virg_table_addcolumn(v, 0, "uniformi", VIRG_INT);
	virg_table_addcolumn(v, 0, "normali5", VIRG_INT);
	virg_table_addcolumn(v, 0, "normali20", VIRG_INT);
	virg_table_addcolumn(v, 0, "uniformf", VIRG_FLOAT);
	virg_table_addcolumn(v, 0, "normalf5", VIRG_FLOAT);
	virg_table_addcolumn(v, 0, "normalf20", VIRG_FLOAT);

	int i;
	void *buff = malloc(4 * 6);
	int *buff_i = (int*)buff;
	float *buff_f = (float*)buff;

	const gsl_rng_type *type;
	gsl_rng *ran;

	type = gsl_rng_ranlxd2;
	ran = gsl_rng_alloc(type);
	gsl_rng_set(ran, time(0));

	for(i = 0; i < atoi(argv[2]); i++) {
		buff_i[0] = (int)gsl_ran_flat(ran, -100, 100);
		buff_i[1] = (int)gsl_ran_gaussian(ran, 5);
		buff_i[2] = (int)gsl_ran_gaussian(ran, 20);
		buff_f[3] = (float)gsl_ran_flat(ran, -100, 100);
		buff_f[4] = (float)gsl_ran_gaussian(ran, 5);
		buff_f[5] = (float)gsl_ran_gaussian(ran, 20);
		virg_table_insert(v, 0, (char*)&i, buff, NULL);

		if(i % 10000 == 0) {
			printf("%i,", i);
			fflush(stdout);
		}
	}
	printf("\n");

	free(buff);

	virg_db_close(v);
	virg_close(v);
	gsl_rng_free(ran);

	return 0;
}


/* histogram/add.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_histogram.h>

#include "find.c"

int
gsl_histogram_increment (gsl_histogram * h, double x)
{
  int status = gsl_histogram_accumulate (h, x, 1.0);
  return status;
}

int
gsl_histogram_accumulate (gsl_histogram * h, double x, double weight)
{
  const size_t n = h->n;
  size_t index = 0;

  int status = find (h->n, h->range, x, &index);

  if (status)
    {
      return GSL_EDOM;
    }

  if (index >= n)
    {
      GSL_ERROR ("index lies outside valid range of 0 .. n - 1",
                 GSL_ESANITY);
    }

  h->bin[index] += weight;

  return GSL_SUCCESS;
}


/* Overlap-add method
 *
 * Copyright (C) 2017-2018 Jean-Pierre Cimalando.
 */

#pragma once
#include <jsl/dynarray>
#include <gsl/span>

template <class R>
class overlap_add {
public:
    overlap_add() noexcept {}
    overlap_add(size_t step, gsl::span<const R> window);

    // compute one block of overlap-add
    void process(const R *in/*[winsize]*/, R *out/*[step]*/);

    const size_t step() const
        { return step_; }

    const jsl::dynarray<R> &window() const
        { return window_; }

private:
    size_t step_ = 0;
    jsl::dynarray<R> window_;
    jsl::dynarray<R> buffer_;
};

#include "util/dsp/overlap_add.tcc"


#ifndef _B_SYSTEM_H_
#define _B_SYSTEM_H_

#include <iostream>
#include <vector>

using namespace std;

#include "Ode.h"
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

class B_system{
	public:
		B_system(double ini_vx_pass, double ini_vy_pass,
				 double ini_xx_pass, double ini_xy_pass,
				 double t_start_pass, double t_end_pass);
		~B_system();
		void solve();                  //solve the ODE and do the interpolation
		double vx(double t_now);       //return velocity in x direction at t_now
		double vy(double t_now);       //return velocity in y direction at t_now
		double xx(double t_now);       //return location in x direction at t_now
		double xy(double t_now);       //return location in y direction at t_now

	private:
		double ini_vx;                 //initial velocity in x direction
		double ini_vy;                 //initial velocity in x direction
		double ini_xx;                 //initial location in x direction
		double ini_xy;                 //initial location in x direction
		double t_start;                //the start time
		double t_end;                  //the end time
		int N = 1000;      			   //number of dots used in solving ODE
		ODE ode;                       //the initinization of the ODE class see the B_system constructor
		vector<double> ti_ode;         //this vector stores all
		vector<double> vx_ode;         //this vector stores all velocity in x dirction solved from ODE
		vector<double> vy_ode;         //this vector stores all velocity in y dirction solved from ODE
		gsl_interp_accel *vx_acc;      //workspace for interpolation loop up
		gsl_interp_accel *vy_acc;      //workspace for interpolation loop up
		gsl_spline *vx_spline;         //workspace for interpolation interface
		gsl_spline *vy_spline;         //workspace for interpolation interface
};

#endif


/* -*- mode: C; c-basic-offset: 4 -*- */
/* ex: set shiftwidth=4 tabstop=4 expandtab: */
/*
 * Copyright (c) 2018, Colorado School of Mines
 * All rights reserved.
 *
 * Author(s): Neil T. Dantam <ndantam@miens.edu>
 * Georgia Tech Humanoid Robotics Lab
 * Under Direction of Prof. Mike Stilman <mstilman@cc.gatech.edu>
 *
 *
 * This file is provided under the following "BSD-style" License:
 *
 *
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 *
 */

// uncomment to check that local allocs actually get freed
// #define AA_ALLOC_STACK_MAX 0

#include <stdlib.h>
#include <cblas.h>

#include "amino.h"
#include "amino/mat.h"

#include "amino/test.h"

static inline void
admeq( const char *name, const struct aa_dmat *A, const struct aa_dmat *B, double tol)
{
    aafeq( name, aa_dmat_ssd(A,B), 0, tol );
}

static inline void
adveq( const char *name, const struct aa_dvec *x, const struct aa_dvec *y, double tol)
{
    aafeq( name, aa_dvec_ssd(x,y), 0, tol );
}

static void test_transpose()
{
    double X[3*2 + 1] = { 1,2,3, 4,5,6, 7};

    {
        double Y[3*2] = {0};
        double x0[] = {1,4, 2,5, 3,6};
        struct aa_dmat mX = AA_DMAT_INIT(3,2,X,3);
        struct aa_dmat mY = AA_DMAT_INIT(2,3,Y,2);
        struct aa_dmat mr = AA_DMAT_INIT(2,3,x0,2);

        aa_dmat_trans(&mX,&mY);
        admeq( "dmat_trans-0", &mY, &mr, 1e-6 );
    }

    {
        double Y[3*3] = {0};
        double x1[] = {1,4,0, 2,5,0, 3,6,0};
        struct aa_dmat mX = AA_DMAT_INIT(3,2,X,3);
        struct aa_dmat mY = AA_DMAT_INIT(2,3,Y,3);
        struct aa_dmat mr = AA_DMAT_INIT(2,3,x1,3);
        aa_dmat_trans(&mX,&mY);
        admeq( "dmat_trans-1", &mY, &mr, 1e-6 );
    }

    {
        double Y[2*2] = {0};
        double x2[] = {1,4, 2,5};
        struct aa_dmat mX = AA_DMAT_INIT(2,2,X,3);
        struct aa_dmat mY = AA_DMAT_INIT(2,2,Y,2);
        struct aa_dmat mr = AA_DMAT_INIT(2,2,x2,2);
        aa_dmat_trans(&mX,&mY);
        admeq( "dmat_trans-2", &mY, &mr, 1e-6 );
    }

}

static void test_inv()
{

    double A[] = {1,3,0, 2,4,0 };
    double Ai[] = {-2, 1.5, 1, -.5};
    struct aa_dmat mA = AA_DMAT_INIT(2,2,A,3);
    struct aa_dmat mAi = AA_DMAT_INIT(2,2,Ai,2);
    aa_dmat_inv(&mA);
    admeq( "dmat_inv", &mA, &mAi, 1e-6 );

}

static void test_pinv()
{
    {

        double A[] = {1,2, 3,4 };
        double B[4];
        double Ai[] = {-2,1, 1.5,-.5};
        struct aa_dmat mA = AA_DMAT_INIT(2,2,A,2);
        struct aa_dmat mB = AA_DMAT_INIT(2,2,B,2);
        struct aa_dmat mAi = AA_DMAT_INIT(2,2,Ai,2);
        aa_dmat_pinv(&mA,-1,&mB);
        admeq( "dmat_pinv-0", &mB, &mAi, 1e-6 );
    }
    {
        double A[] = {1,2,3,4,5,6};
        double A_star[6] = {0};
        double R[] = {-1.3333, -.33333, 0.66667, 1.083333, 0.33333, -0.4166667};

        struct aa_dmat mA  = AA_DMAT_INIT(2,3,A,2);
        struct aa_dmat mAs = AA_DMAT_INIT(3,2,A_star,3);

        int r = aa_dmat_pinv( &mA, -1, &mAs );
        aveq( "la_pinv-1", 6, A_star, R, .0001 );
        assert(0 == r );
    }

    {
        double A[] = {1,3,5, 2,4,6 };
        double Ai[6];
        double R[] = {-1.3333,1.08333, -0.333333,0.3333333, 0.6666667, -0.416667};
        struct aa_dmat mA  = AA_DMAT_INIT(3,2,A,3);
        struct aa_dmat mAi = AA_DMAT_INIT(2,3,Ai,2);
        struct aa_dmat mR  = AA_DMAT_INIT(2,3,R,2);
        aa_dmat_pinv(&mA, -1, &mAi);
        admeq( "dmat_pinv-2", &mAi, &mR, 1e-3 );
    }

}

static void test_dpinv()
{
    //dpinv
    {
        double A[] = {1,2,3,4};
        double A_star[4] = {0};
        struct aa_dmat mA  = AA_DMAT_INIT(2,2,A,2);
        struct aa_dmat mAs = AA_DMAT_INIT(2,2,A_star,2);
        double R[] = {-2,1, 1.5,-.5};
        int r = aa_dmat_dpinv( &mA, 1e-4, &mAs );
        //printf("r0: %d\n", r);
        aveq( "la_dpinv-0", 4, A_star, R, 1e-2 );
        assert(0 == r );
    }
    {
        double A[] = {1,2,3,4,5,6};
        double A_star[6] = {0};
        double R[] = {-1.3333, -.33333, 0.66667, 1.083333, 0.33333, -0.4166667};

        struct aa_dmat mA  = AA_DMAT_INIT(2,3,A,2);
        struct aa_dmat mAs = AA_DMAT_INIT(3,2,A_star,3);

        int r = aa_dmat_dpinv( &mA, 1e-4, &mAs );
        //printf("r0: %d\n", r);
        aveq( "la_dpinv-1", 6, A_star, R, .001 );
        assert(0 == r );
    }

    {
        double A[] = {1,3,5, 2,4,6 };
        double A_star[6] = {0};
        double R[] = {-1.3333,1.08333, -0.333333,0.3333333, 0.6666667, -0.416667};
        //double R[6] = {-0.95256, 0.44785, -0.11224, 0.11159, 0.72808, -0.22467};

        struct aa_dmat mA  = AA_DMAT_INIT(3,2,A,3);
        struct aa_dmat mAs = AA_DMAT_INIT(2,3,A_star,2);

        int r = aa_dmat_dpinv( &mA, 1e-3, &mAs );
        //printf("r0: %d\n", r);
        aveq( "la_dpinv-2", 6, A_star, R, 1e-2 );
        assert(0 == r );
    }

    {
        double A[] = {1,2,3,4,5,6};
        double A_star[6] = {0};
        double R[6] = {-0.95256, 0.44785, -0.11224, 0.11159, 0.72808, -0.22467};

        struct aa_dmat mA  = AA_DMAT_INIT(3,2,A,3);
        struct aa_dmat mAs = AA_DMAT_INIT(2,3,A_star,2);

        int r = aa_dmat_dpinv( &mA, 1e-3, &mAs );
        //printf("r0: %d\n", r);
        aveq( "la_dpinv-3", 6, A_star, R, 1e-2 );
        assert(0 == r );
    }

}

static void test_dzdpinv()
{
    //dpinv
    {
        double A[] = {1,2,3,4};
        double A_star[4] = {0};
        struct aa_dmat mA  = AA_DMAT_INIT(2,2,A,2);
        struct aa_dmat mAs = AA_DMAT_INIT(2,2,A_star,2);
        double R[] = {  -2, 1,  1.5,  -0.5 };
        int r = aa_dmat_dzdpinv( &mA, .005, &mAs );
        //printf("r0: %d\n", r);
        aveq( "la_dzdpinv-0", 4, A_star, R, .0001 );
        assert(0 == r );
    }
    {
        double A[] = {1,2,3,4,5,6};
        double A_star[6] = {0};
        double R[] = {-1.3333, -.33333, 0.66667, 1.083333, 0.33333, -0.4166667};

        struct aa_dmat mA  = AA_DMAT_INIT(2,3,A,2);
        struct aa_dmat mAs = AA_DMAT_INIT(3,2,A_star,3);

        int r = aa_dmat_dzdpinv( &mA, .005, &mAs );
        //printf("r0: %d\n", r);
        aveq( "la_dzdpinv-1", 6, A_star, R, .0001 );
        assert(0 == r );
    }

    {
        double A[] = {1,3,5, 2,4,6 };
        double A_star[6] = {0};
        double R[] = {-1.3333,1.08333, -0.333333,0.3333333, 0.6666667, -0.416667};
        //double R[6] = {-0.95256, 0.44785, -0.11224, 0.11159, 0.72808, -0.22467};

        struct aa_dmat mA  = AA_DMAT_INIT(3,2,A,3);
        struct aa_dmat mAs = AA_DMAT_INIT(2,3,A_star,2);

        int r = aa_dmat_dzdpinv( &mA, .001, &mAs );
        //printf("r0: %d\n", r);

        assert(0 == r );
    }

}

static void test_scal()
{
    {
        double d[] = {1,2, 3,4};
        double d2[] = {2,4, 6,8};
        struct aa_dmat A = AA_DMAT_INIT(2,2,d,2);
        struct aa_dmat A2 = AA_DMAT_INIT(2,2,d2,2);
        aa_dmat_scal(&A,2);
        admeq( "dmat_scal-0", &A, &A2, 1e-6 );
    }
    {
        double d[] = {1,2,3,0, 4,5,6,0};
        double d2[] = {2,4,6, 8,10,12 };
        struct aa_dmat A = AA_DMAT_INIT(3,2,d,4);
        struct aa_dmat A2 = AA_DMAT_INIT(3,2,d2,3);
        aa_dmat_scal(&A,2);
        admeq( "dmat_scal-1", &A, &A2, 1e-6 );
    }
    {
        double d[] =  {1,2,0, 3,4,0, 5,6,0};
        double d2[] = {2,4, 6,8, 10,12};
        struct aa_dmat A = AA_DMAT_INIT(2,3,d,3);
        struct aa_dmat A2 = AA_DMAT_INIT(2,3,d2,2);
        aa_dmat_scal(&A,2);
        admeq( "dmat_scal-2", &A, &A2, 1e-6 );
    }
}

static void test_mat_inc()
{
    {
        double d[] = {1,2, 3,4};
        double d2[] = {2,3, 4,5};
        struct aa_dmat A = AA_DMAT_INIT(2,2,d,2);
        struct aa_dmat A2 = AA_DMAT_INIT(2,2,d2,2);
        aa_dmat_inc(&A,1);
        admeq( "dmat_inc-0", &A, &A2, 1e-6 );
    }
    {
        double d[] = {1,2,3,0, 4,5,6,0};
        double d2[] = {2,3,4, 5,6,7 };
        struct aa_dmat A = AA_DMAT_INIT(3,2,d,4);
        struct aa_dmat A2 = AA_DMAT_INIT(3,2,d2,3);
        aa_dmat_inc(&A,1);
        admeq( "dmat_inc-1", &A, &A2, 1e-6 );
    }
}

static void test_mat_axpy()
{
    {
        double d[] = {1,2, 3,4};
        double e[] = {4,5, 6,7};
        double d2[] = {9,12, 15,18};
        struct aa_dmat A = AA_DMAT_INIT(2,2,d,2);
        struct aa_dmat B = AA_DMAT_INIT(2,2,e,2);
        struct aa_dmat A2 = AA_DMAT_INIT(2,2,d2,2);
        aa_dmat_axpy(2,&B,&A);
        admeq( "dmat_axpy-0", &A, &A2, 1e-6 );
    }
    {
        double d[] = {1,2,3,0, 4,5,6,0};
        double e[] = {7,8,9, 10,11,12};
        double d2[] = {15,18,21, 24,27,30 };
        struct aa_dmat A = AA_DMAT_INIT(3,2,d,4);
        struct aa_dmat B = AA_DMAT_INIT(3,2,e,3);
        struct aa_dmat A2 = AA_DMAT_INIT(3,2,d2,3);
        aa_dmat_axpy(2, &B, &A);
        admeq( "dmat_axpy-1", &A, &A2, 1e-6 );
    }
}

static void
test_inc()
{
    double xd[] = {1,2,3};
    double yd[] = {2,3,4};

    struct aa_dvec x = AA_DVEC_INIT(3,xd,1);
    struct aa_dvec y = AA_DVEC_INIT(3,yd,1);
    aa_dvec_inc(1,&x);

    adveq( "dinc", &x,&y,0);

}

static void test_nrm2()
{
    double Ad[] = {1,2,3,1024,   4,5,6,2048 };
    struct aa_dmat A = AA_DMAT_INIT( 3, 2, Ad, 4 );
    aafeq( "mat-nrm2", aa_dmat_nrm2(&A),
           sqrt(1*1 + 2*2 + 3*3 + 4*4 + 5*5 + 6*6),
           1e-6 );

}

int main(void)
{
    {
        double Ad[] = {1,2,3,1024,   4,5,6,2048 };
        double xd[] = {7, 4096, 11, 4096};
        double yd[3];
        double rd[3] = {51, 69, 87};

        struct aa_dmat A;
        struct aa_dvec x,y;

        aa_dmat_view( &A, 3, 2, Ad, 4 );
        aa_dvec_view( &x, 2, xd, 2 );
        aa_dvec_view( &y, 3, yd, 1 );

        aa_dmat_gemv( CblasNoTrans, 1, &A, &x, 0, &y );

        aveq( "dgemv", 3,yd,rd,0);
    }

    test_transpose();
    test_inv();
    test_pinv();
    test_dpinv();
    test_dzdpinv();
    test_scal();

    test_mat_inc();
    test_mat_axpy();

    test_inc();
    test_nrm2();

    printf("mat_test: OK\n");
    return 0;
}


//This does CELL (~soma) stage of minimal GRU (gated recurrent unit) model.
//This requires each neuron to have 2 input time series, X and Xf,
//where X is the usual input and Xf the input for the forget-gate.
//Both X and Xf are the output of a linear IN stage (weights and baises).

//For dim=0: F[:,t] = sig(Xf[:,t] + Uf*Y[:,t-1])
//           H[:,t] = F[:,t].*Y[:,t-1]
//           Y[:,t] = H[:,t] + (1-F[:,t]).*tanh(X[:,t] + U*H[:,t])
//
//For dim=1: F[t,:] = sig(Xf[t,:] + Y[t-1,:]*Uf)
//           H[t,:] = F[t,:].*Y[t-1,:]
//           Y[t,:] = H[t,:] + (1-F[t,:]).*tanh(X[t,:] + H[t,:]*U)
//
//where sig is the logistic (sigmoid) nonlinearity = 1/(1+exp(-x)),
//F is the forget gate signal, H is an intermediate vector,
//U and Uf are NxN update matrices, and Y is the output.

//Note that, the neurons of a layer are independent only if U and Uf are diagonal matrices.
//This is only really a CELL (~soma) stage in that case.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cblas.h>

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int gru_min2_s (float *Y, const float *X, const float *Xf, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int gru_min2_d (double *Y, const double *X, const double *Xf, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int gru_min2_inplace_s (float *X, const float *Xf, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);
int gru_min2_inplace_d (double *X, const double *Xf, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim);

int gru_min2_s (float *Y, const float *X, const float *Xf, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    size_t nT, tN;

    float *F, *H;
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_min2_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_min2_s: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        F[0] = 1.0f / (1.0f+expf(-Xf[0]));
        Y[0] = (1.0f-F[0]) * tanhf(X[0]);
        for (size_t t=1; t<T; ++t)
        {
            F[0] = 1.0f / (1.0f+expf(-Xf[t]-Uf[0]*Y[t-1]));
            H[0] = F[0] * Y[t-1];
            Y[t] = H[0] + (1.0f-F[0])*tanhf(X[t]+U[0]*H[0]);
        }
    }
    else if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-Xf[n]));
                Y[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_scopy((int)N,&Xf[tN],1,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_scopy((int)N,&X[tN],1,&Y[tN],1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0f-F[n])*tanhf(Y[tN+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-Xf[nT]));
                Y[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&Xf[t],(int)T,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_scopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0f-F[n])*tanhf(Y[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-Xf[nT]));
                Y[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&Xf[t],(int)T,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_scopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0f-F[n])*tanhf(Y[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-Xf[n]));
                Y[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_scopy((int)N,&Xf[tN],1,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_scopy((int)N,&X[tN],1,&Y[tN],1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0f-F[n])*tanhf(Y[tN+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_min2_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int gru_min2_d (double *Y, const double *X, const double *Xf, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    size_t nT, tN;

    double *F, *H;
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_min2_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_min2_d: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        F[0] = 1.0 / (1.0+exp(-X[0]));
        Y[0] = (1.0-F[0]) * tanh(X[0]);
        for (size_t t=1; t<T; ++t)
        {
            F[0] = 1.0 / (1.0+exp(-X[t]-Uf[0]*Y[t-1]));
            H[0] = F[0] * Y[t-1];
            Y[t] = H[0] + (1.0-F[0])*tanh(X[t]+U[0]*H[0]);
        }
    }
    else if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-Xf[n]));
                Y[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_dcopy((int)N,&Xf[tN],1,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_dcopy((int)N,&X[tN],1,&Y[tN],1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0-F[n])*tanh(Y[tN+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-Xf[nT]));
                Y[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&Xf[t],(int)T,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_dcopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0-F[n])*tanh(Y[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-Xf[nT]));
                Y[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&Xf[t],(int)T,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[t-1+n*T];
                }
                cblas_dcopy((int)N,&X[t],(int)T,&Y[t],(int)T);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    Y[t+nT] = H[n] + (1.0-F[n])*tanh(Y[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-Xf[n]));
                Y[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_dcopy((int)N,&Xf[tN],1,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&Y[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * Y[tN-N+n];
                }
                cblas_dcopy((int)N,&X[tN],1,&Y[tN],1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&Y[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    Y[tN+n] = H[n] + (1.0-F[n])*tanh(Y[tN+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_min2_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int gru_min2_inplace_s (float *X, const float *Xf, const float *U, const float *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const float o = 1.0f;
    size_t nT, tN;

    float *F, *H;
    if (!(F=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_min2_inplace_s: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(float *)malloc(N*sizeof(float)))) { fprintf(stderr,"error in gru_min2_inplace_s: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        F[0] = 1.0f / (1.0f+expf(-X[0]));
        X[0] = (1.0f-F[0]) * tanhf(X[0]);
        for (size_t t=1; t<T; ++t)
        {
            F[0] = 1.0f / (1.0f+expf(-X[t]-Uf[0]*X[t-1]));
            H[0] = F[0]*X[t-1];
            X[t] = H[0] + (1.0f-F[0])*tanhf(X[t]+U[0]*H[0]);
        }
    }
    else if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-Xf[n]));
                X[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_scopy((int)N,&Xf[tN],1,F,1);
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * X[tN-N+n];
                }
                cblas_sgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN+n] = H[n] + (1.0f-F[n])*tanhf(X[tN+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-Xf[nT]));
                X[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&Xf[t],(int)T,F,1);
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * X[t-1+n*T];
                }
                cblas_sgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = H[n] + (1.0f-F[n])*tanhf(X[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0f / (1.0f+expf(-Xf[nT]));
                X[nT] = (1.0f-F[n]) * tanhf(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_scopy((int)N,&Xf[t],(int)T,F,1);
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * X[t-1+n*T];
                }
                cblas_sgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = H[n] + (1.0f-F[n])*tanhf(X[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0f / (1.0f+expf(-Xf[n]));
                X[n] = (1.0f-F[n]) * tanhf(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_scopy((int)N,&Xf[tN],1,F,1);
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0f / (1.0f+expf(-F[n]));
                    H[n] = F[n] * X[tN-N+n];
                }
                cblas_sgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN+n] = H[n] + (1.0f-F[n])*tanhf(X[tN+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_min2_inplace_s: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

int gru_min2_inplace_d (double *X, const double *Xf, const double *U, const double *Uf, const size_t N, const size_t T, const char iscolmajor, const size_t dim)
{
    const double o = 1.0;
    size_t nT, tN;

    double *F, *H;
    if (!(F=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_min2_inplace_d: problem with malloc. "); perror("malloc"); return 1; }
    if (!(H=(double *)malloc(N*sizeof(double)))) { fprintf(stderr,"error in gru_min2_inplace_d: problem with malloc. "); perror("malloc"); return 1; }

    if (N==1u)
    {
        F[0] = 1.0 / (1.0+exp(-X[0]));
        X[0] = (1.0-F[0]) * tanh(X[0]);
        for (size_t t=1; t<T; ++t)
        {
            F[0] = 1.0 / (1.0+exp(-X[t]-Uf[0]*X[t-1]));
            H[0] = F[0]*X[t-1];
            X[t] = H[0] + (1.0-F[0])*tanh(X[t]+U[0]*H[0]);
        }
    }
    else if (dim==0u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-Xf[n]));
                X[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_dcopy((int)N,&Xf[tN],1,F,1);
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * X[tN-N+n];
                }
                cblas_dgemv(CblasColMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN+n] = H[n] + (1.0-F[n])*tanh(X[tN+n]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-Xf[nT]));
                X[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&Xf[t],(int)T,F,1);
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * X[t-1+n*T];
                }
                cblas_dgemv(CblasRowMajor,CblasNoTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = H[n] + (1.0-F[n])*tanh(X[t+nT]);
                }
            }
        }
    }
    else if (dim==1u)
    {
        if (iscolmajor)
        {
            for (size_t n=0u; n<N; ++n)
            {
                nT = n*T;
                F[n] = 1.0 / (1.0+exp(-Xf[nT]));
                X[nT] = (1.0-F[n]) * tanh(X[nT]);
            }
            for (size_t t=1; t<T; ++t)
            {
                cblas_dcopy((int)N,&Xf[t],(int)T,F,1);
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[t-1],(int)T,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * X[t-1+n*T];
                }
                cblas_dgemv(CblasColMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[t],(int)T);
                for (size_t n=0u; n<N; ++n)
                {
                    nT = n*T;
                    X[t+nT] = H[n] + (1.0-F[n])*tanh(X[t+nT]);
                }
            }
        }
        else
        {
            for (size_t n=0u; n<N; ++n)
            {
                F[n] = 1.0 / (1.0+exp(-Xf[n]));
                X[n] = (1.0-F[n]) * tanh(X[n]);
            }
            for (size_t t=1; t<T; ++t)
            {
                tN = t*N;
                cblas_dcopy((int)N,&Xf[tN],1,F,1);
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,Uf,(int)N,&X[tN-N],1,o,F,1);
                for (size_t n=0u; n<N; ++n)
                {
                    F[n] = 1.0 / (1.0+exp(-F[n]));
                    H[n] = F[n] * X[tN-N+n];
                }
                cblas_dgemv(CblasRowMajor,CblasTrans,(int)N,(int)N,o,U,(int)N,H,1,o,&X[tN],1);
                for (size_t n=0u; n<N; ++n)
                {
                    X[tN+n] = H[n] + (1.0-F[n])*tanh(X[tN+n]);
                }
            }
        }
    }
    else
    {
        fprintf(stderr,"error in gru_min2_inplace_d: dim must be 0 or 1.\n"); return 1;
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


#ifndef __INC_MODELSTRUCT__
#define __INC_MODELSTRUCT__

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

/**
 * @file modelstruct.h
 * \brief defines modelstruct which contains the raw data to be worked on by emulator and estimator
 */
struct optstruct;
//! holds the main data structures for a single model emulator run

/**
 * \struct modelstruct
 * this structure is everything you need to do run the estimation and the
 * emulation
 *
 * it contains:
 * training vector
 * model points
 * theta vector (hyperparameters)
 * the options used to define this model
 * \note: the options used to be kept separately, they are now always contained in the modelstruct
 */
typedef struct modelstruct{
	/**
	 * \brief x_values (nmodel_points * nparams)
	 *
	 * the points at which the model (that which is to be emulated) was evaluated,
	 * each row is a new model point, the columns are the dimensions
	 */
	gsl_matrix* xmodel;
	/**
	 * \brief y_values (nmodel_points)
	 *
	 * the values of the model evaluated at each model point
	 * this is what we aim to interpolate
	 */
	gsl_vector* training_vector;
	/**
	 * \brief hyperparameters (nthetas)
	 *
	 * working and eventually best values of the covariance fns (hyper)parameters which
	 * should force the emulated model to pass through all of the training points
	 */
	gsl_vector* thetas;
	/**
   * the average distances between samples in the design in each dimension
	 * this sets soft minima on the correlation function length
	 */
	gsl_vector* sample_scales;
	/**
	 * the options struct used to init this structure
	 */
	struct optstruct* options;

	/**
	 * fnptrs which define various aspects of the regression etc
	 * should these be in optstruct?
	 */

	/**
	 * setup the regression H vector, depends upon the regression order
	 *
	 * (needed in regression.h)
	 */
	void (*makeHVector)(gsl_vector *h_vector, gsl_vector *x_location, int nparams);

	/**
	 * points to the covariance function,
	 *
	 * (needed in emulator.h)
	 */
	double (*covariance_fn)(gsl_vector*, gsl_vector*, gsl_vector*, int, int);

	/**
	 * compute the gradient matrix for the length setting theta values
	 * dC/dTheta = (C-nugget) * (1/2)*(x_i - x_j)^(alpha) * alpha / (thetaLength)
	 *
	 * this is a fn-ptr which will be set when the cov function is setup
	 * the different target fns are in emulator.c called derivative_l_<covfnname>
	 *
	 * @param covsub the covariance matrix with the nugget term subtracted
	 * @param thetaLength the current value of the length scale we're differentiating wrt
	 * @param index, the direction we're looking in
	 * @return dCdTheta the matrix of the derivative of C wrt index
	 *
	 * (needed in maxmultimin.h)
	 */
	void (*makeGradMatLength)(gsl_matrix *dCdTheta,  gsl_matrix* xmodel,
														double thetaLength, int index, int nmodel_points, int nparams);

} modelstruct;

#include "optstruct.h"

void alloc_modelstruct(modelstruct* the_model, optstruct* options);
void free_modelstruct(modelstruct* the_model);
void fill_modelstruct(modelstruct* the_model, optstruct* options, char** input_data);

void load_modelstruct(FILE* fptr, modelstruct* the_model, optstruct* opts);
void dump_modelstruct(FILE* fptr, modelstruct* the_model, optstruct* opts);

void copy_modelstruct(modelstruct* dst, modelstruct* src);

modelstruct * alloc_modelstruct_2(gsl_matrix* xmodel, gsl_vector* training_vector, int cov_fn_index, int regression_order);
void free_modelstruct_2(modelstruct * model);
void dump_modelstruct_2(FILE *fptr, modelstruct* the_model);
modelstruct* load_modelstruct_2(FILE *fptr);

void set_global_ptrs(modelstruct * model);
gsl_vector * fill_sample_scales_vec(gsl_matrix* xmodel);

#endif


#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

extern void getTimeSeries(int points, double* times, int from, int nAgents, double eBirth, double eDeath, int rng_seed, int* output);


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_linalg.h>

/* number of data points to fit */
#define N        6

/* number of fit coefficients */
#define NCOEFFS  6

/* Spline order */
#define K  3

/* nbreak = ncoeffs + 2 - k */
#define NBREAK   (NCOEFFS + 2 - K)

void gsl_solve( gsl_matrix *A,  const size_t n,
                double xmax, double xmin,
                double *B )
{

  printf("\n *** Solve with GSL *** \n");
  gsl_vector *d; d = gsl_vector_alloc(n);
  gsl_vector *e; e = gsl_vector_alloc(n-1);
  gsl_vector *f; f = gsl_vector_alloc(n-1);
  gsl_vector *b; b = gsl_vector_alloc(n);
  gsl_vector *x; x = gsl_vector_alloc(n);

  for (int i = 0; i < n; i++)
    {
      gsl_vector_set(d, i, gsl_matrix_get(A,i,i));
      if (i>0)
        gsl_vector_set(f, i-1, gsl_matrix_get(A,i,i-1));
      if (i<n-1)
        gsl_vector_set(e, i, gsl_matrix_get(A,i,i+1));
    }

  for (int i=0; i<n; i++)
    gsl_vector_set(b,i,B[i]);

  gsl_linalg_solve_tridiag ( d, e, f, b, x);

  for (int i=0; i<n; i++)
    printf ("x[%d] = %f \n", i, gsl_vector_get(x,i));

  gsl_vector_free(e); e = gsl_vector_alloc(n);
  gsl_vector_free(f); f = gsl_vector_alloc(n);

  double alpha = 0.5;
  double beta  = 0.5;
  printf("\n *** A Matrix *** \n");
  for (int i = 0; i < n; i++)
    { for (int j = 0; j < n; j++)
        {
          printf ("%6.3f", gsl_matrix_get (A, i, j));
        }
      printf("\n");
    }

  for (int i = 0; i < n; i++)
    {
      gsl_vector_set(d, i, gsl_matrix_get(A,i,i));
      if (i>0)
        gsl_vector_set(f, i-1, gsl_matrix_get(A,i,i-1));
      if (i<n-1)
        gsl_vector_set(e, i, gsl_matrix_get(A,i,i+1));
    }

  gsl_vector_set(e,n-1,alpha);
  gsl_vector_set(f,n-1,beta);

  for (int i=0; i<n; i++) gsl_vector_set(b,i,B[i]);

  // A = ( d_0 e_0  0  f_3 )
  //     ( f_0 d_1 e_1  0  )
  //     (  0  f_1 d_2 e_2 )
  //     ( e_3  0  f_2 d_3 )
  for (int i=0; i<n; i++)
    printf ("%d = %f %f %f \n", i, gsl_vector_get(d,i)
                                 , gsl_vector_get(e,i)
                                 , gsl_vector_get(f,i));

  gsl_linalg_solve_cyc_tridiag (d, e, f, b, x);

  printf("\n cyclic \n\n");
  for (int i=0; i<n; i++)
    printf ("x[%d] = %f \n", i, gsl_vector_get(x,i));
}

void tridag(double a[],
            double b[],
            double c[],
            double r[],
            double u[],
            int n)
{
  double bet;
  double *gam;

  gam = (double *) malloc(n*sizeof(double));

  if (b[0] == 0.0) printf("Error 1 in tridag");

  u[0]=r[0]/(bet=b[0]);

  for (int j=1;j<n;j++)
  {
    gam[j]= c[j-1]/bet;
    bet=b[j]-a[j]*gam[j];
    if (bet == 0.0) printf("Error 2 in tridag");
    u[j]=(r[j]-a[j]*u[j-1])/bet;
  }

  for (int j=(n-2);j>=0;j--)
    u[j] -= gam[j+1]*u[j+1];

  free(gam);

}

void cyclic(double a[],
            double b[],
            double c[],
            double alpha,
            double beta,
            double r[],
            double x[], int n)

{
  double fact,gamma;
  double *bb;
  double *u;
  double *z;

  if (n <= 2) printf("n too small in cyclic");
  bb = (double *) malloc(n*sizeof(double));
  u  = (double *) malloc(n*sizeof(double));
  z  = (double *) malloc(n*sizeof(double));

  gamma   = -b[0];
  bb[0]   =  b[0]-gamma;
  bb[n-1] =  b[n-1]-alpha*beta/gamma;

  for (int i=1;i<n-1;i++)
    bb[i]=b[i];

  tridag(a,bb,c,r,x,n);
  u[0]  =gamma;
  u[n-1]=alpha;

  for (int i=1;i<n-1;i++)
    u[i]=0.0;

  tridag(a,bb,c,u,z,n);

  fact=(x[0]+beta*x[n-1]/gamma)/(1.0+z[0]+beta*z[n-1]/gamma);

  for (int i=0;i<n;i++)
    x[i] -= fact*z[i];

  free(z);
  free(u);
  free(bb);
}

void nr_solve( gsl_matrix *A,
               int n,
               double xmax,
               double xmin,
               double *B )
{

  printf("\n *** Solve with Numerical Recipes *** \n");

  double *a; a = (double *) malloc(n*sizeof(double));
  double *b; b = (double *) malloc(n*sizeof(double));
  double *c; c = (double *) malloc(n*sizeof(double));
  double *r; r = (double *) malloc(n*sizeof(double));
  double *u; u = (double *) malloc(n*sizeof(double));

  for (int i = 0; i < n; i++)
    {
      if (i>0) a[i] = gsl_matrix_get (A, i, i-1);
      b[i] = gsl_matrix_get (A, i, i);
      if (i<n-1) c[i] = gsl_matrix_get (A, i, i+1);
      r[i] = B[i];
    }

  for (int i=0; i<n; i++)
    printf ("%d = %f %f %f \n", i, a[i], b[i], c[i]);

  tridag ( a, b, c, r, u, n);
  for (int i=0; i<n; i++)
    printf ("u[%d] = %f \n", i, u[i]);

  double alpha = 0.5;
  double beta  = 0.5;

  cyclic( a, b, c, alpha, beta, r, u, n);

  printf("\n cyclic \n\n");
  for (int i=0; i<n; i++)
    printf ("u[%d] = %f \n", i, u[i]);
}

int
main (void)
{
  const size_t n = N;
  const size_t ncoeffs = NCOEFFS;
  const size_t nbreak = NBREAK;
  size_t i, j;
  gsl_bspline_workspace *bw;
  gsl_vector *B;
  double xmin=0.0, xmax=2.0;
  const size_t k = K;
  gsl_vector *tau;
  gsl_matrix *A;

  double *y;

  double dx = (xmax-xmin)/(n-1);
  double xc = (xmax+xmin)*0.5;
  y = (double *) malloc(ncoeffs*sizeof(double));

  tau = gsl_vector_alloc(n);

  /* allocate a cubic bspline workspace */
  bw = gsl_bspline_alloc(k, nbreak);
  B  = gsl_vector_alloc(ncoeffs);
  A  = gsl_matrix_alloc(n, ncoeffs);

  /* gsl_bspline_knots(breakpts, bw)
  Compute the knots from the given breakpoints:
   knots(1:k) = breakpts(1)
   knots(k+1:k+l-1) = breakpts(i), i = 2 .. l
   knots(n+1:n+k) = breakpts(l + 1) */

  /* use uniform breakpoints on [xmin, xmax] */
  gsl_bspline_knots_uniform(xmin, xmax, bw);
  //knots(1:k) = xmin
  //knots(k+1:k+l-1) = xmin + i*delta, i = 1 .. l - 1
  //knots(n+1:n+k) = xmax

  /* construct the fit matrix X */
  for (i = 0; i < n; ++i)
    {
      double taui = i * dx;
      double ti   = gsl_vector_get(bw->knots, i);

      gsl_vector_set(tau, i, taui);

      /* compute B_j(xi) for all j */
      gsl_bspline_eval(taui, B, bw);

      printf("%f %f\n", taui, ti);

      /* fill in row i of X */
      for (j = 0; j < ncoeffs; ++j)
      {
        double Bj = gsl_vector_get(B, j);
        gsl_matrix_set(A, i, j, Bj);
      }

    }

  for (int i = 0; i < n; i++)
    { for (int j = 0; j < n; j++)
        {
          printf ("%6.3f", gsl_matrix_get (A, i, j));
        }
      printf("\n");
    }

  for (int i=0; i<n;i++)
    y[i] = fabs(i*dx-xc);

  nr_solve( A, ncoeffs, xmax, xmin, y);

  gsl_solve( A, ncoeffs, xmax, xmin, y);

  gsl_bspline_free(bw);
  gsl_vector_free(B);
  gsl_matrix_free(A);
  return 0;

}


#ifndef VECTORS_H
#define VECTORS_H

#include <cmath>
#include <gsl/gsl_blas.h>
#include "types.h"
#include "cuda.h"

__host__ __device__ void angle_ab(const double dir1[3], const double vel[3], double dir2[3]);
__host__ __device__ double doppler(const double dir1[3], const double vel[3]);
__host__ __device__ void scatter_dir(const double dir_in[3], double cos_theta, double dir_out[3]);
__host__ __device__ void get_rand_isotropic_unitvec(double vecout[3]);
__host__ __device__ void move_pkt(PKT *pkt_ptr, double distance, const double time);

__host__ __device__
inline double vec_len(const double x[3])
// return the the magnitude of a vector
{
  #ifdef __CUDA_ARCH__
    return sqrt((x[0] * x[0]) + (x[1] * x[1]) + (x[2] * x[2]));
  #else
    return cblas_dnrm2(3, x, 1);
  #endif
}

__host__ __device__
inline void vec_norm(const double vec_in[3], double vec_out[3])
// Routine for normalizing a vector.
{
  const double magnitude = vec_len(vec_in);

  vec_out[0] = vec_in[0] / magnitude;
  vec_out[1] = vec_in[1] / magnitude;
  vec_out[2] = vec_in[2] / magnitude;

  // vec_copy(vec_out, vec_in);
  // cblas_dscal(3, 1 / magnitude, vec_out, 1)
}

__host__ __device__
inline double dot(const double x[3], const double y[3])
// Routine for taking dot product.
{
  #ifdef __CUDA_ARCH__
    return (x[0] * y[0]) + (x[1] * y[1]) + (x[2] * y[2]);
  #else
    return cblas_ddot(3, x, 1, y, 1);
  #endif
}

__host__ __device__
inline void get_velocity(const double x[3], double y[3], const double t)
// Routine for getting velocity vector of the flow at a position with homologous expansion.
{
  y[0] = x[0] / t;
  y[1] = x[1] / t;
  y[2] = x[2] / t;
}

__host__ __device__
inline void cross_prod(const double vec1[3], const double vec2[3], double vecout[3])
{
  vecout[0] = (vec1[1] * vec2[2]) - (vec2[1] * vec1[2]);
  vecout[1] = (vec1[2] * vec2[0]) - (vec2[2] * vec1[0]);
  vecout[2] = (vec1[0] * vec2[1]) - (vec2[0] * vec1[1]);
}

__host__ __device__
inline void vec_scale(double vec[3], const double scalefactor)
{
  #ifdef __CUDA_ARCH__
    for (int d = 0; d < 3; d++)
    {
      vec[d] *= scalefactor;
    }
  #else
    cblas_dscal(3, scalefactor, vec, 1);
  #endif
}

__host__ __device__
inline void vec_copy(double destination[3], const double source[3])
{
  #ifdef __CUDA_ARCH__
    for (int d = 0; d < 3; d++)
    {
      destination[d] = source[d];
    }
  #else
    cblas_dcopy(3, source, 1, destination, 1);
  #endif
}

__host__ __device__
inline double doppler_packetpos(const PKT *const pkt_ptr)
{
  double vel_vec[3];
  get_velocity(pkt_ptr->pos, vel_vec, pkt_ptr->prop_time); // homologous flow velocity
  const double dopplerfactor = doppler(pkt_ptr->dir, vel_vec);
  return dopplerfactor;
}

#endif //VECTORS_H


//Include guard
#ifndef MISC_H
#define MISC_H

//Forward declared dependencies

//Included dependencies
#include <vector>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_matrix.h>
#include "sequence.h"
#include "diploidSequence.h"
#include "modelMR.hpp"
#include "model.hpp"
#include "simParamPH.h"
#include "anaParam.h"
#include "stdlib.h"
#include "stdio.h"
#include "string.h"

void rescale(std::vector<double> &vec);
void rescaleZeroAccepted(std::vector<double> &vec);
void rescaleMin(std::vector<double> &vec, double min);
void printDoubleVector(std:: vector<double> &vec);
std::string printDoubleVectorToString(std:: vector<double> &vec);
void printIntVector(std:: vector<int> &vec);
std::string printIntVectorToString(std:: vector<int> &vec);
std::vector<int> multinomialSampling(int N, std::vector<double> p, const gsl_rng *r);
int sumOfVector(std::vector<int> intVec);
double sumOfVector(std::vector<double> doubleVec);
std::vector<double> subtractVectorsDouble(std::vector<double> &a, std::vector<double> &b);
std::vector<double> addVectorsDouble(std::vector<double> &a, std::vector<double> &b);
double my_gsl_linalg_LU_det (gsl_matrix * LU, int signum);
double determinant(gsl_matrix *A);
double logMultivariateNormalPDF(std::vector<double> &x, std::vector<double> &mu, gsl_matrix *sigma);
double logMultivariateNormalPDFPrint(std::vector<double> &x, std::vector<double> &mu, gsl_matrix *sigma);
void printMatrix(gsl_matrix *A);
void printMatrixMathematica(gsl_matrix *A);
std::string printMatrixMathematicaToString(gsl_matrix* A);
Sequence randomSequence(int length, gsl_rng *r);
std::vector<Sequence> generateRandomFullHaplotypes(int length, int number, gsl_rng *r);
std::vector<Sequence> generateMajorMinorFullHaplotypes(int length, int number, gsl_rng *r);
bool identicalSeqs(Sequence & a, Sequence & b);
bool identicalIntVectors(std::vector<int> & a, std::vector<int> & b);
bool nonZeroIntVector(std::vector<int> & a);
bool nonZeroDoubleVector(std::vector<double> & a);

void setupRNG(gsl_rng *r, unsigned long int seed);

DiploidSequence generateRandomDiploidSequence(int length, gsl_rng *r);
DiploidSequence generateRandomSemiConstrainedDiploidSequence(Sequence constraint, gsl_rng *r);
DiploidSequence generateRandomSemiConstrainedDiploidSequence(DiploidSequence constraint, gsl_rng *r);
std::vector<Sequence> generateFullHapsFromAlleles(DiploidSequence &fullHapAlleles);

void randomiseSequenceVector(std::vector<Sequence> & s, gsl_rng *r);

std::vector<int> pickRandomInts(gsl_rng *r, int max, int length);

void addRandom(std::vector<double>& oldVec, std::vector<double>& newVec, double delta, gsl_rng *r);
void addRandomMin(std::vector<double>& oldVec, std::vector<double>& newVec, double delta, gsl_rng *r, double min);
void addRandom(std::vector<double>& vec, double delta, gsl_rng *r);
double logMultinomialProb(std::vector<double> &freq, std::vector<int> & obs);
void findLogFactorial(std::vector<double> & fact_store, int N);
double logDirMultProbC(double C, std::vector<int> &obs, std::vector<double> &inf, std::vector<double> &fact_store);
double logDirMultProb(std::vector<double>& alpha, std::vector<int> &x, int& n);
double logBetaBinProb(double alpha, double beta, int x, int n);
double BetaBinCDF(double alpha, double beta, double threshold, int n);

bool fileExists(std::string &fileName);
std::vector<int> DirMultSampling(int N, std::vector<double> & freqs, double C, const gsl_rng *r);

//Mean, no var
double computeLSelection(std::vector<int> &obsB, std::vector<int> &obsA, int Nt, AnaParam* ap, std::vector<double> &qBfhFreqs, std::vector<double> &hapFit, std::vector<std::vector<int> >* contribs, bool print);
double computeLNeutral(std::vector<int>& obsB, std::vector<int>& obsA, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, std::vector<std::vector<int> >* contribs, bool print);

//Mean and var
double computeLSelTVar(std::vector<int>& obsA, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, gsl_matrix* var, std::vector<double>& hapFitT, std::vector<std::vector<int> >* contribs, bool print);
double computeLSelTVarOld(std::vector<int>& obsA, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, gsl_matrix* var, std::vector<double>& hapFitT, std::vector<std::vector<int> >* contribs, bool print);
double computeLSelTSelGVar(std::vector<int>& obsA, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, gsl_matrix* var, std::vector<double>& hapFitT, std::vector<double> &hapFitG, std::vector<std::vector<int> >* contribs, bool print);
double computeLSelTSelGVarOld(std::vector<int>& obsA, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, gsl_matrix* var, std::vector<double>& hapFitT, std::vector<double> &hapFitG, std::vector<std::vector<int> >* contribs, bool print);
double computeLNeutralVar(std::vector<int>& obsA, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, gsl_matrix* var, std::vector<std::vector<int> >* contribs, bool print);
double computeLNeutralVarAdvanced(std::vector<int>& obsA, int& deltaDays, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, gsl_matrix* var, std::vector<std::vector<int> >* contribs, bool print);
double computeLNeutralVarReduced(std::vector<int>& obsA, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, gsl_matrix* var, std::vector<std::vector<int> >* contribs, bool print);
double computeLNeutralSelGVar(std::vector<int>& obsA, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, gsl_matrix* var, std::vector<double>& hapFitG, std::vector<std::vector<int> >* contribs, bool print);
double computeLNeutralSelGVarOld(std::vector<int>& obsA, int Nt, AnaParam* ap, std::vector<double>& qBfhFreqs, gsl_matrix* var, std::vector<double>& hapFitG, std::vector<std::vector<int> >* contribs, bool print);

void constructMatrixM(std::vector<double> x, gsl_matrix *M);
void constructMatrixMoriginal(std::vector<double> x, gsl_matrix *M);

std::vector<double> computeHapFit(std::vector<Sequence> &fullHaps, Sequence &selModel, std::vector<double> &selCoefsNew);
std::vector<double> computeSimulationHapFitT(std::vector<Sequence> &fullHaps, SimParamPH *spPH);
std::vector<double> computeSimulationHapFitG(std::vector<Sequence> &fullHaps, SimParamPH *spPH);

std::vector<DiploidSequence> createOneLocusCollapsed(std::vector<std::vector<DiploidSequence> > &oneLocusModels);

std::vector<Model> generateModelsFromPreviousBest(std::vector<DiploidSequence> &collapsedFullHaps, Model &bestModelBefore);
std::vector<ModelMR> generateModelsFromPreviousBest(std::vector<DiploidSequence> &collapsedFullHaps, ModelMR &bestModelBefore);

int binomialCoeff(int n, int k);
int logBinCoeffStirling(int n, int k);

int getNextLeft(std::vector<int> alreadyPicked, int max);
std::vector<Sequence> findRemainingSeqs(std::vector<Sequence> subset, std::vector<Sequence> set);

DiploidSequence constructDiploidFromFullHap(std::vector<Sequence> &fhs, std::vector<double> &freqs);
DiploidSequence constructDiploidFromFullHap(std::vector<Sequence> &fhs);
bool noEntryTheSameExceptZero(std::vector<double> &v);

std::vector<std::vector<int> > getAllCombs(int N, int k);

std::vector<std::vector<int> > getLinkedPairs(std::vector<Sequence> &haps);

std::vector<std::string> split(const std::string& s, char delim);

//Related to gaining information on memory usage
int parseLine(char* line);
int getValue();

bool stringToBool(std::string& s);
bool compareBySize(const std::vector<int>& a, const std::vector<int>& b);

//gamma and delta functions for compound solutions
double gamma_n(int n, double lambda, int Nt);
double delta_n(int n, double lambda, int Nt);
double qPochhammer(double a, double q, int n);

#endif