/*
 * resampling.c
 *
 *  Created on: 5.5.2017
 *      Author: heine
 */
#define _XOPEN_SOURCE 600

#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <mpi.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
//#include "ziggurat.h"

const gsl_rng_type *T; // Generator type
const gsl_rng *rgen;   // Generator

void set_resamplingseed( int id ) {

  struct timespec spec;

  clock_gettime( CLOCK_REALTIME, &spec );

  gsl_rng_env_setup();
  T = gsl_rng_default;
  rgen = gsl_rng_alloc( T );

  gsl_rng_set( rgen, ( unsigned long int ) ( round( spec.tv_nsec / 1.0e6 ) + 223 * id ) );

}

double depth_first_sum(double *x, long len) {

  long n = len;
  double *x_tmp, out;
  x_tmp = malloc(sizeof(double) * len);
  for (long i = 0; i < len; i++)
    x_tmp[i] = x[i];

  while (n > 1) {
    for (long i = 0; i < n / 2; i++) {
      x_tmp[i] = x_tmp[2 * i] + x_tmp[2 * i + 1];
    }
    if (n % 2 != 0) {
      x_tmp[n / 2] = x_tmp[n - 1];
    }
    n = n / 2 + n % 2;
  }
  out = x_tmp[0];
  free(x_tmp);
  return out;
}

double *depth_first_cumsum(double *x, long len) {

  double *W;
  W = malloc(sizeof(double) * len);

  for (long i = 0; i < len; i++) {
    W[i] = depth_first_sum(x, i);
  }
  return W;
}

double *exclusive_prefix_sum(double *w, long len) {
  double *W;
  W = malloc(sizeof(double) * len);

  W[0] = 0;
  for (long i = 1; i < len; i++) {
    W[i] = W[i - 1] + w[i - 1];
  }

  return W;
}

double *combining_exclusive_prefix_sum(double *w, double *w_recv, long n) {

  double *W;
  W = malloc(sizeof(double) * 2 * n);

  W[0] = 0;

  for (long i = 1; i <= n; i++) {
    W[i] = W[i - 1] + w[i - 1];
  }

  for (long i = 0; i < n-1; i++) {
    W[n + i + 1] = W[n + i] + w_recv[i];
  }

  return W;
}

double serial_multinomial_resample(long n, double *w, double *w_recv, long *inds) {

  double *W;
  double total_weight, lnmax = 0, u;
  long j = 2 * n - 1;

  W = combining_exclusive_prefix_sum(w, w_recv, n);

  total_weight = log(W[2 * n - 1] + w_recv[n - 1]);

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    inds[i - 1] = j;
  }

  free(W);

  return exp(total_weight);
}

double serial_multinomial_resample_sng(long n, double *w, long *inds) {

  double *W;
  double total_weight, lnmax = 0, u;
  long j = n - 1;

  W = exclusive_prefix_sum(w, n);

  total_weight = log(W[n - 1] + w[n - 1]);

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    inds[i - 1] = j;
  }

  free(W);

  return exp(total_weight);
}

double serial_multinomial_resample_sng_noutk(long n, double *w, long *inds, long k) {

  double *W;
  double total_weight, lnmax = 0, u;
  long j = k - 1;

  W = exclusive_prefix_sum(w, k);

  total_weight = log( W[ k - 1 ] + w[ k - 1 ] );

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    inds[i - 1] = j;
  }

  free(W);

  return exp(total_weight);
}

double serial_multinomial_resample_noutk(long n, double *w, double* W, long *inds, long k) {

  double total_weight, lnmax = 0, u;
  long j = k - 1;

  total_weight = log(W[k - 1] + w[k - 1]);

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    inds[i - 1] = j;
  }

  return -1;
}

void binary_resampler(long n, double w, long *counts)  {

  counts[0] = 0;
  counts[1] = 0;

  for ( long i = 0; i < n; i++ )
    counts[  gsl_rng_uniform_pos( rgen ) < w ? 0 : 1 ]++;

}

void
simplified_resample(
		    long n,
		    long *inds,
		    long k)
{

  for ( long i = 0; i < n; i++ )
    inds[ i ] = ( long ) ( gsl_rng_uniform_int( rgen, ( unsigned long int ) k ) );
}

void serial_multinomial_resample_counts(long n, long k, double *w, long *cnts) {

  double *W;
  double total_weight, lnmax = 0, u;
  long j = k - 1;

  for( long i = 0; i < k ; i++ )
    cnts[ i ] = 0;

  W = exclusive_prefix_sum(w, k);

  total_weight = log(W[k - 1] + w[k - 1]);

  for (long i = n; i >= 1; i--) {

    u = gsl_rng_uniform_pos( rgen );

    lnmax = lnmax + log(u) / (double) i;

    u = exp(total_weight + lnmax);

    while (u < W[j])
      j--;

    cnts[j]++;
  }

  free(W);

}


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


// 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);
}


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


#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


/*
 *
 * ML fitting of dots using a 2D Gaussian model provided by gaussianInt2.
 * I.e., only x and y are fitted.
 *
 * Compilation:
 * For debugging:
 *  gcc  3.c -Wall -std=c99 -I/usr/local/include -L/usr/local/lib -lgsl -lgslcblas -lm -g -o 3
 *  valgrind ./3
 * For speed:
 *  gcc  -Wall -std=c99 3.c -I/usr/local/include -L/usr/local/lib -lgsl -lgslcblas -lm -O3 -o 3
 *  ./3
 * Memory accesses can also be checked with Valgrind or the gcc -fmudflap memory protection option.
 *
 * MATLAB interface in df_mlfit1.c
 * 2017.04.12. Valgrind ok.
 *
 * TODO:
 *  - What are good step sizes?
 *  - Set the initial positions with higher than integer precision if possible.
 *
 */

/*
   From the GSL documentation:

   If necessary you can turn off range checking completely without
   modifying any source files by recompiling your program with the
   preprocessor definition GSL_RANGE_CHECK_OFF. Provided your compiler
   supports inline functions the effect of turning off range checking is
   to replace calls to gsl_vector_get(v,i) by v->data[i*v->stride] and
   calls to gsl_vector_set(v,i,x) by v->data[i*v->stride]=x. Thus there
   should be no performance penalty for using the range checking
   functions when range checking is turned off.

*/

#include <assert.h>
#include <inttypes.h>
#include <stdint.h>
#include <stdio.h>

#include <gsl/gsl_multimin.h>
#include <gsl/gsl_statistics.h>

#include "gaussianInt2.h"
#include "mlfit.h"
#include "blit3.h"
#include "mlfit1.h"

// use the compile flag -D verbose=0 etc
// 0 = no information
// 1 = per dot information
// 2 = per iteration information
#ifndef verbose
#define verbose 1
#endif

// Globals
const size_t maxIterations = 10000; // for z-fitting
const double convCriteria = 1e-7; // for z-fitting

// Possibly, add the window size, denoted Ws in most places

// Headers
double lxy (const gsl_vector *, void *);
double lz (const gsl_vector *, void *);
int localizeDotXY(double *, size_t, double *,  double *,
    double); // sigma
int localizeDotZ(double *, size_t, double *,  double *,
    double); // sigma
int localize(double *, size_t, size_t, size_t, double *, size_t, double *,
    double, double); // sigma_xy, sigma_z -- size of dot
int unit_tests(void);

void lxy_df (const gsl_vector *v, void *params, gsl_vector * df);
void lxy_fdf (const gsl_vector *v, void *params, double * f, gsl_vector *df);

// Optimization constants
typedef struct {
  double * R; // local region
  double * G; // A already allocated temporary space for the Gaussian kernel
  size_t Rw; // R and G has size Rw*Rw
  double sigma; // For constant sigma fitting
  double bg;
} optParams;

double lz (const gsl_vector *v, void *params)
{
  double x;

  optParams *p = (optParams *) params;

  size_t Rw = p->Rw;
  double * R = p->R;
  double * GI = p->G;
  double sigma = p->sigma;
  double bg = p->bg;

#if verbose > 2
  printf("Rw: %lu\n", Rw);
  printf("Nphot: %lu\n", Nphot);
  printf("sigma: %f\n", sigma);
  printf("bg   : %f\n", bg);
#endif

  // Get the other parameters ...
  x = gsl_vector_get(v, 0);
  double Nphot = gsl_vector_get(v,1);

  //  printf("bg: %f, Nphot: %f, sigma: %f z: %f\n", bg, Nphot, sigma, x);

  // Create Gaussian ...
  double mu[] = {x};
  gaussianInt1(GI, mu, &sigma, Rw);
  for(size_t kk = 0; kk<Rw; kk++)
    GI[kk] = bg+Nphot*GI[kk];
#if verbose>2
  printf("GI:\n");
  showRegion(GI, Rw);
#endif

  /*
   * from LL2PG.m
   *   model = x(3)+x(4)*gaussianInt2([x(1), x(2)], x(5), (size(patch, 1)-1)/2);
   *   mask = disk2d((size(patch,1)-1)/2);
   *   %L = -sum(sum(-(patch-model).^2./model - .5*log(model)));
   *   L = -sum(sum(mask.*(-(patch-model).^2./model - .5*log(model))));
   */

  double E = 0;
  for (size_t kk=0; kk<Rw; kk++)
  {
    E+= (GI[kk]-R[kk])*(GI[kk]-R[kk])/GI[kk] - .5*log(GI[kk]);
  }
  //  E = -E;

  if(0){
    printf("R=[");
    for (size_t kk=0; kk<Rw; kk++)
    {
      printf("%f ", R[kk]);
    }
    printf("]\n");
    printf("G = [");
    for (size_t kk=0; kk<Rw; kk++)
    {
      printf("%f ", GI[kk]);
    }
    printf("]\n");
    printf("E: %f, mu: %f\n", E, mu[0]);
  }

  /* Quadratic
   * for (size_t kk=0; kk<Rw*Rw; kk++)
   * E+= (GI[kk]-R[kk])*(GI[kk]-R[kk]);
   */

  if(!isfinite(E))
    E = 10e99;

  return E;
}

double lxy (const gsl_vector *v, void *params)
  // likelihood in xy, function to optimize
{

  double x, y;

  optParams *p = (optParams *) params;

  size_t Rw = p->Rw;
  double * R = p->R;
  double * GI = p->G;
  double sigma = p->sigma;
  double bg = p->bg;

#if verbose > 2
  printf("Rw: %lu\n", Rw);
  printf("Nphot: %lu\n", Nphot);
  printf("sigma: %f\n", sigma);
  printf("bg   : %f\n", bg);
#endif

  // Get the other parameters ...
  x = gsl_vector_get(v, 0);
  y = gsl_vector_get(v, 1);
  double Nphot = gsl_vector_get(v,2);

  // Create Gaussian ...
  double mu[] = {x,y};
  gaussianInt2(GI, mu, &sigma, Rw);
  for(size_t kk = 0; kk<Rw*Rw; kk++)
    GI[kk] = bg+Nphot*GI[kk];
#if verbose>2
  printf("GI:\n");
  showRegion(GI, Rw);
#endif

  /*
   * from LL2PG.m
   *   model = x(3)+x(4)*gaussianInt2([x(1), x(2)], x(5), (size(patch, 1)-1)/2);
   *   mask = disk2d((size(patch,1)-1)/2);
   *   %L = -sum(sum(-(patch-model).^2./model - .5*log(model)));
   *   L = -sum(sum(mask.*(-(patch-model).^2./model - .5*log(model))));
   */

  double E = 0;
  for (size_t kk=0; kk<Rw*Rw; kk++)
    E+= (GI[kk]-R[kk])*(GI[kk]-R[kk])/GI[kk] - .5*log(GI[kk]);
  //  E = -E;

  /* Quadratic
   * for (size_t kk=0; kk<Rw*Rw; kk++)
   * E+= (GI[kk]-R[kk])*(GI[kk]-R[kk]);
   */
//  printf("E = %f\n", E);
  return E;
}

void lxy_df (const gsl_vector *v, void *params, gsl_vector * df)
{
  double h = 1.49e-8; // step size
  gsl_vector * dv = gsl_vector_alloc(3);
  gsl_vector_memcpy(dv, v);

  for(int kk = 0; kk<3; kk++)
  {
    gsl_vector_set(dv, kk, gsl_vector_get(v, kk) + h); // Set
    gsl_vector_set(df, kk, (lxy(dv, params)-lxy(v, params))/h);
   // printf("%f ", gsl_vector_get(df, kk));
    gsl_vector_set(dv, kk, gsl_vector_get(v, kk)); // Reset
  }
//  printf("\n");

  gsl_vector_free(dv);

}

void lxy_fdf (const gsl_vector *v, void *params, double * f, gsl_vector *df)
{
  *f = lxy(v, params);
  lxy_df(v, params, df);
}

int localizeDotXY(double * V, size_t Vm,
    double * D,  double * F, double sigma)
  // Localization for a dot roughly centered in V of size Vm x Vm
  // D[0], D[1], D[3] are the global coordinates of the dot
  // F are the fitted coordinates
{

  /* Parameters that will not be optimized */
  optParams par;
  par.R = V;
  par.Rw = Vm;
  par.sigma = sigma;
  par.bg = estimateBG(V, Vm);
  par.G = (double *) malloc(Vm*Vm*sizeof(double));

  const gsl_multimin_fdfminimizer_type *T =
    gsl_multimin_fdfminimizer_vector_bfgs;
  //const gsl_multimin_fminimizer_type *T =
    //  gsl_multimin_fminimizer_nmsimplex2;

  gsl_multimin_fdfminimizer *s = NULL;
  gsl_vector *ss, *x;
  gsl_multimin_function_fdf minex_func;

  size_t iter = 0;
  int status;
  double size;

  /* Starting point for parameters to be optimized */
  x = gsl_vector_alloc (3);
  gsl_vector_set(x, 0, 0); // x position
  gsl_vector_set(x, 1, 0); // y position
  gsl_vector_set(x, 2, estimateNphot(V, Vm)); // Nphot

  /* Grid search to find a reasonable starting point
   * for (x,y) to avoid local minima */

  if(1) {
  double xmin = 0;
  double ymin = 0;
  double emin = 10e99;
  int ng = 5; // ng*ng grid points
  for(double dx = -.4; dx<.4; dx=dx+0.8/ng)
    for(double dy = -.4; dy<.4; dy=dy+0.8/ng)
    {
      gsl_vector_set(x, 0, dx); // x position
      gsl_vector_set(x, 1, dy); // y position

      double err = lxy(x, (void *) &par);
//    printf("%f %f :  %f\n", dx, dy, err);
      if( err < emin)
      {
        emin = err;
        xmin = dx;
        ymin = dy;
      }
    }
   gsl_vector_set(x, 0, xmin); // x position
   gsl_vector_set(x, 1, ymin); // y position
 //  printf("Initial position: %f %f\n", xmin, ymin);
  }

  /* Set initial step sizes */
  ss = gsl_vector_alloc(3);
  gsl_vector_set_all(ss, 0.1);

  /* Initialize method and iterate */
  minex_func.n = 3;
  minex_func.f = lxy; // Error
  minex_func.df = lxy_df; // Derivative
  minex_func.fdf = lxy_fdf; // Error and Derivative
  minex_func.params = ∥

  s = gsl_multimin_fdfminimizer_alloc (T, 3);
  gsl_multimin_fdfminimizer_set (s, // Optimizer
      &minex_func, // Function to optimize
      x, // Start point
      0.1, // Default step size of the line search
      0.1); // tol. 0 = exact line search 0.1 = recommended from GSL manual

  /* tol=0.1 was about twice as fast as tol=0 for this problem with no noticeable
   * regression in accuracy */

size_t maxIterationsQN = 500; // abort if more iterations

  do
  {
    iter++;
    status = gsl_multimin_fdfminimizer_iterate(s);

    if (status)
      break;

    size = 0; //gsl_multimin_fdfminimizer_size(s);
    status = gsl_multimin_test_gradient (s->gradient, 1e-4);

    if (status == GSL_SUCCESS)
    {
#if verbose > 0
      printf ("converged to minimum at\n");
      printf ("%5lu x:%10.3e y:%10.3e NP:%6.1f f() = %7.3f size = %10.3e\n",
          iter,
          gsl_vector_get (s->x, 0),
          gsl_vector_get (s->x, 1),
          gsl_vector_get (s->x, 2),
          0.0, size);
#endif
    }
  }
  while (status == GSL_CONTINUE && iter < maxIterationsQN);

  // F is always written even if the convergence failed
  F[0] = gsl_vector_get (s->x, 0)+nearbyint(D[0]);
  F[1] = gsl_vector_get (s->x, 1)+nearbyint(D[1]);
  F[2] = D[2]; // Not optimized

  gsl_vector_free(x);
  gsl_vector_free(ss);
  gsl_multimin_fdfminimizer_free (s);

  free(par.G);

  return status;
}

int localizeDotZ(double * V, size_t Vm,
    double * D,  double * F, double sigma)
  // Localization for a dot roughly centered in V of size Vm x Vm
  // D[0], D[1], D[3] are the global coordinates of the dot
  // F are the fitted coordinates
{

  // Non-optimized parameters
  optParams par;
  par.R = V;
  par.Rw = Vm;
  par.sigma = sigma;
  par.bg = estimateBG(V, Vm);
  par.G = (double *) malloc(Vm*sizeof(double));

  const gsl_multimin_fminimizer_type *T =
    gsl_multimin_fminimizer_nmsimplex2;
  gsl_multimin_fminimizer *s = NULL;
  gsl_vector *ss, *x;
  gsl_multimin_function minex_func;

  size_t iter = 0;
  int status;
  double size;

  /* Starting point */
  x = gsl_vector_alloc (2);
  gsl_vector_set(x, 0, 0); // z position
  gsl_vector_set(x, 1, estimateNphot(V, Vm)); // Nphot

  /* Set initial step sizes */
  ss = gsl_vector_alloc(2);
  gsl_vector_set_all(ss, 0.1);
  gsl_vector_set(ss, 1, 10); // Number of photons

  /* Initialize method and iterate */
  minex_func.n = 2;
  minex_func.f = lz;
  minex_func.params = ∥

  //printf(".\n"); fflush(stdout);

  s = gsl_multimin_fminimizer_alloc (T, 2);

  //  printf("..\n"); fflush(stdout);
  int error = gsl_multimin_fminimizer_set(s,
      &minex_func,
      x, // starting point
      ss); // Step sizes
  if(error>0)
    printf("error: %d\n", error);
  // printf("...\n"); fflush(stdout);

  do
  {
    iter++;
    status = gsl_multimin_fminimizer_iterate(s);
    //  printf("....\n"); fflush(stdout);

    if (status)
      break;

    size = gsl_multimin_fminimizer_size(s);
    status = gsl_multimin_test_size(size, convCriteria);
    //  printf(".....\n"); fflush(stdout);

    if (status == GSL_SUCCESS)
    {
#if verbose > 0
      printf ("converged to minimum at\n");
      printf ("%5lu z:%10.3e NP:%6.1f f() = %7.3f size = %10.3e\n",
          iter,
          gsl_vector_get (s->x, 0),
          gsl_vector_get (s->x, 1),
          s->fval, size);
#endif
    }
  }
  while (status == GSL_CONTINUE && iter < maxIterations);

  // Always Update Z position -- even if convergence failed
  F[2] = gsl_vector_get (s->x, 0)+nearbyint(D[2]);

  gsl_vector_free(x);
  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free (s);

  free(par.G);

  return status;
}
int localize(double * V, size_t Vm, size_t Vn, size_t Vp,
    double * D, size_t Dm, double * F, double sigma_xy, double sigma_z)
  // run the localization routine for a list of dots
{

  /* The window size is a very important parameter. Small windows can not capture the full signals and that results in
   * bad fittings.
   * Ws = 7 is good until sigma = 1.1
   * Ws = 9 is good at least until sigma = 1.6
   */
  int Ws = 9; // Window size
  double * W = malloc(Ws*Ws*sizeof(double));

  for(size_t kk =0; kk<Dm; kk++)
  {
    // Copy the neighbourhood around each D into W
    if(getRegion(W, Ws,
          V, Vm, Vn, Vp, D+kk*3) == 0)
    {
#if verbose > 1
      printf("W:\n");
      showRegion(W, Ws);
#endif
      // Local fitting in W
#if verbose > 0
      int status = localizeDotXY(W,  Ws, D+kk*3, F+kk*3, sigma_xy);
      printf("Status: %d\n", status);
#else
      localizeDotXY(W,  Ws, D+kk*3, F+kk*3, sigma_xy);
#endif

      if(getZLine(W,Ws,
            V, Vm, Vn, Vp, D+kk*3) == 0)
      {
        localizeDotZ(W, Ws, D+kk*3, F+kk*3, sigma_z);
      }

    }
    else
    {
      // TODO: add also Nphot and status to the output
      F[kk*3] = -1;
      F[kk*3+1] = -1;
      F[kk*3+2] = -1;
    }
  }
  free(W);
  return 0;
}

int unit_tests(){
  double * V; // image
  int Vm = 1024; int Vn = 1024; int Vp = 60;
  double * D; // list of dots
  size_t Dm = 5; // number of dots
  double * F; // fitted dots

  printf("Image size: %dx%dx%d\n", Vm, Vn, Vp);
  printf("Localizing %lu dots\n", Dm);

  V = (double *) malloc(Vm*Vn*Vp*sizeof(double));
  D = (double *) malloc(3*Dm*sizeof(double));
  F = (double *) malloc(3*Dm*sizeof(double));

  // Initialize the data
  for(int kk=0; kk<Vm*Vn*Vp; kk++)
    V[kk] = 1000+rand_range(0,0);

  for(uint32_t kk=0; kk<Dm; kk++)
  {
    size_t pos = kk*3;
    D[pos] =   rand_range(6, Vm-7);
    D[pos+1] = rand_range(6, Vn-7);
    D[pos+2] = rand_range(6, Vp-7);
    if(D[pos] < 6)
      D[pos] = 6;
    if(D[pos+1] < 6)
      D[pos+1] = 6;
    if(D[pos+2] < 6)
      D[pos+2] = 6;
#if verbose > 0
    printf("D %03d %f %f %f\n", kk, D[pos], D[pos+1], D[pos+2]);
#endif
  }

  for(uint32_t kk=0; kk<Dm; kk++)
  {
    size_t pos = D[kk*3] + D[kk*3+1]*Vm + D[kk*3+2]*Vm*Vn;
    V[pos] = V[pos]+5;
  }

  D[0] = 100; D[1] = 100; D[2] = 30;
  V[(int) D[0]+(int) D[1]*Vm+(int) D[2]*Vm*Vn] = 7;
  V[(int) D[0]+1+(int) D[1]*Vm+(int) D[2]*Vm*Vn] = 6;

  // Run the optimization
  localize(V, Vm, Vn, Vp, D, Dm, F, 1.1, 1.1);

  if(1)
  {
    for(double sigma = 0.1; sigma<2; sigma=sigma+0.1)
      localize(V, Vm, Vn, Vp, D, Dm, F, sigma, sigma);
  }

  // In next version, also supply clustering information

#if verbose >0
  for(size_t kk = 0; kk<Dm; kk++)
  {
    size_t pos = kk*3;
    printf("%6lu (%f, %f, %f) -> (%f, %f, %f)\n", kk,
        D[pos], D[pos+1], D[pos+2],
        F[pos], F[pos+1], F[pos+2]);
  }
#endif
  free(F);
  free(D);
  free(V);
  return 0;
}

int main(int argc, char ** argv)
  // For testing, not used any more, see the MATLAB interface in
  // df_mlfit.c
{

  printf("%s\n", argv[0]);

  if(argc == 1)
    return unit_tests();

  return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_blas.h>

#include <omp.h>

#include "util.h"

int main(int argc, char *argv[]) {
    char* input_fileName1 = argv[1];
    char* input_fileName2 = argv[2];
    int N_doc_bg = atoi(argv[3]);
    int N_kw = atoi(argv[4]);
    int N_obs = atoi(argv[5]);
    int N_iter = atoi(argv[6]);
    double sec_budget = atoi(argv[7]) / 100.0;
    char* output_fileName = argv[8];
    int N_doc = 480000 / 2;
    int fixed_padding = 2 * log(2) * (64 + log(N_doc) / log(2)) / sec_budget;
    printf("Padding: %d\n", fixed_padding);

    int* matrix = (int*) malloc(sizeof(int) * N_obs * N_obs);
    int* matrix_bg = (int*) malloc(sizeof(int) * N_kw * N_kw);
    int* matrix_padded = (int*) malloc(sizeof(int) * N_obs * N_obs);
    int* true_index = (int*) malloc(sizeof(int) * N_kw);
    int* permutation = (int*) malloc(sizeof(int) * N_obs);
    gsl_matrix* matrix_obs;

    for (int round = 0; round < 10; round++)
    {
        char input_fileName1_extend[40];
        char input_fileName2_extend[40];
        sprintf(input_fileName1_extend, "%s%d", input_fileName1, round);
        sprintf(input_fileName2_extend, "%s%d", input_fileName2, round);

        // Setup
        struct timeval tv1,tv2;
        gettimeofday(&tv1, NULL);
        read_matrix(&true_index, &matrix_bg, 1.0*N_doc/N_doc_bg, N_kw, input_fileName2_extend);
        read_matrix(&true_index, &matrix, 1.0, N_obs, input_fileName1_extend);
        gettimeofday(&tv2, NULL);
        printf("Reading done: %f.\n", (double) (tv2.tv_usec - tv1.tv_usec) / 1000000 + (double) (tv2.tv_sec - tv1.tv_sec));
        fflush(stdout);

        for (int iter = 0; iter < 10; iter++)
        {
            printf("Run %d\n", iter);
            matrix_obs = gsl_matrix_alloc(N_obs, N_obs);

            gettimeofday(&tv1, NULL);
            pad_matrix(&matrix_padded, &matrix, N_obs, N_doc, sec_budget, fixed_padding);
            gettimeofday(&tv2, NULL);
            printf("Padding done: %f.\n", (double) (tv2.tv_usec - tv1.tv_usec) / 1000000 + (double) (tv2.tv_sec - tv1.tv_sec));
            fflush(stdout);

            gettimeofday(&tv1, NULL);
            observe_matrix(matrix_obs, &matrix_padded, N_obs);
            gettimeofday(&tv2, NULL);
            printf("Observed matrix generated: %f.\n", (double) (tv2.tv_usec - tv1.tv_usec) / 1000000 + (double) (tv2.tv_sec - tv1.tv_sec));
            fflush(stdout);

            // Permute observed matrix randomly and attack
            gettimeofday(&tv1, NULL);
            attack(matrix_obs, &matrix_bg, &permutation, N_kw, N_obs, N_doc, sec_budget, fixed_padding, N_iter);
            gettimeofday(&tv2, NULL);
            printf("Main attack done: %f.\n",  (tv2.tv_usec - tv1.tv_usec) / 1000000.0 + (tv2.tv_sec - tv1.tv_sec));
            fflush(stdout);

            char output_fileName_full[40];
            sprintf(output_fileName_full, "%s%d-%d", output_fileName, round, iter);
            print_result(output_fileName_full, &permutation, &true_index, N_obs);
            //sprintf(output_fileName_full, "%s%d-%d-full", output_fileName, round, iter);
            //print_full_result(output_fileName_full, &permutation, &true_index, N_obs);
        }
    }

    free(matrix);
    free(matrix_padded);
    gsl_matrix_free(matrix_obs);
    return(0);
}

double log_score(int idx1, int idx2, gsl_matrix* matrix_obs, int** matrix, int** permutation, int N_kw, int N_doc, double sec_budget, int fixed_padding)
{
    int idx1_m = (*permutation)[idx1];
    int idx2_m = (*permutation)[idx2];

    if (idx1 == idx2)
    {
        int N_upper = 3 * sqrt((*matrix)[idx1_m*N_kw + idx2_m] * (N_doc - (*matrix)[idx1_m*N_kw + idx2_m]) / (double) N_doc);
        int N_lower = (*matrix)[idx1_m*N_kw + idx2_m] - N_upper;
        N_upper = (*matrix)[idx1_m*N_kw + idx2_m] + N_upper;
        int count_total = (int) gsl_matrix_get(matrix_obs, idx1, idx2);

        double score = 0;
        double prob = (*matrix)[idx1_m*N_kw + idx2_m] / (double) N_doc;
        for (int kk = N_lower; kk < N_upper+1; kk+=20)
            score += gsl_ran_binomial_pdf(kk, prob, N_doc) * gsl_ran_laplace_pdf(count_total - fixed_padding - 2*kk, 2.0/sec_budget);
        if (score == 0)
            return(-500.0);
    }

    int n1 = (int) gsl_matrix_get(matrix_obs, idx1, idx1) - (*matrix)[idx1_m*N_kw + idx1_m];
    int n2 = (int) gsl_matrix_get(matrix_obs, idx2, idx2) - (*matrix)[idx2_m*N_kw + idx2_m];

    double mean = 0.5 * gsl_matrix_get(matrix_obs, idx2, idx2) / N_doc * n1 + 0.5 * gsl_matrix_get(matrix_obs, idx1, idx1) / N_doc * n2;
    double var = 1.0 * (*matrix)[idx1_m*N_kw + idx2_m] / N_doc * (N_doc - (*matrix)[idx1_m*N_kw + idx2_m]);
    var += 0.25 * n1 / N_doc * gsl_matrix_get(matrix_obs, idx2, idx2) * (2 * N_doc - gsl_matrix_get(matrix_obs, idx2, idx2)) / N_doc;
    var += 0.25 * n2 / N_doc * gsl_matrix_get(matrix_obs, idx1, idx1) * (2 * N_doc - gsl_matrix_get(matrix_obs, idx1, idx1)) / N_doc;
    int count_total = (int) gsl_matrix_get(matrix_obs, idx1, idx2);

    int N_upper = 3 * sqrt((*matrix)[idx1_m*N_kw + idx2_m] * (N_doc - (*matrix)[idx1_m*N_kw + idx2_m]) / (double) N_doc);
    int N_lower = (*matrix)[idx1_m*N_kw + idx2_m] - N_upper;
    N_upper = (*matrix)[idx1_m*N_kw + idx2_m] + N_upper;

    double score = 0;
    double prob = (*matrix)[idx1_m*N_kw + idx2_m] / (double) N_doc;
    for (int kk = N_lower; kk < N_upper+1; kk+=20)
        score +=  gsl_ran_binomial_pdf(kk, prob, N_doc) * gsl_ran_gaussian_pdf(count_total - mean - kk, sqrt(var));

    if (score == 0)
        return(-500.0);
    return(log(20*score));
}

void initial_solution(int** permutation, gsl_matrix* matrix_obs, int** matrix, int fixed_padding, int N_kw, int N_obs)
{
    int* diff = (int*) malloc(sizeof(int) * N_kw);
    int* index = (int*) malloc(sizeof(int) * N_kw);
    int* occupied = (int*) malloc(sizeof(int) * N_kw);

    for (int ii = 0; ii < N_kw; ii++)
        occupied[ii] = -1;

    for (int ii = 0; ii < N_obs; ii++)
    {
        // reset index
        for (int jj = 0; jj < N_kw; jj++)
            index[jj] = jj;

        // compute differences
        for (int jj = 0; jj < N_kw; jj++)
            diff[jj] = abs(gsl_matrix_get(matrix_obs, ii, ii) - fixed_padding - 2*(*matrix)[jj*N_kw+jj]);

        // sorting
        for (int jj = 0; jj < N_kw-1; jj++)
        {
            for (int kk = 0; kk < N_kw-jj-1; kk++)
            {
                if (diff[kk] > diff[kk+1])
                {
                    int temp = diff[kk+1];
                    diff[kk+1] = diff[kk];
                    diff[kk] = temp;

                    temp = index[kk+1];
                    index[kk+1] = index[kk];
                    index[kk] = temp;
                }
            }
        }

        // use the smallest index available
        int idx = 0;
        while (occupied[idx] > 0)
            idx++;

        // update the data structures
        (*permutation)[ii] = index[idx];
        occupied[idx] = 1;
    }

}

void attack(gsl_matrix* matrix_obs, int** matrix, int** permutation, int N_kw, int N_obs, int N_doc, double sec_budget, int fixed_padding, int N_iter)
{
    // Initialise data structures
    double* score_matrix = (double*) malloc(sizeof(double) * N_obs * N_obs);
    double* score_row1 = (double*) malloc(sizeof(double) * N_obs);
    double* score_row2 = (double*) malloc(sizeof(double) * N_obs);

    int* permutation_tmp = (int*) malloc(sizeof(int) * N_obs);
    int* permutation_inv = (int*) malloc(sizeof(int) * N_kw);

    // Initialise permutations
    // Initial solution
    initial_solution(permutation, matrix_obs, matrix, fixed_padding, N_kw, N_obs);
    for (int ii = 0; ii < N_obs; ii++)
        permutation_tmp[ii] = (*permutation)[ii];
    for (int ii = 0; ii < N_kw; ii++)
        permutation_inv[ii] = -1;
    for (int ii = 0; ii < N_obs; ii++)
        permutation_inv[permutation_tmp[ii]] = ii;

    // Initialising RNG
    const gsl_rng_type * T;
    gsl_rng * r;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);

    struct timeval tv1,tv2;
    gettimeofday(&tv1, NULL);

    // Compute initial score
    #pragma omp parallel for shared(score_matrix, matrix_obs, matrix)
    for (int ii = 0; ii < N_obs * N_obs; ii++)
        score_matrix[ii] = log_score((int) (ii / N_obs), ii % N_obs, matrix_obs, matrix, permutation, N_kw, N_doc, sec_budget, fixed_padding);
    gettimeofday(&tv2, NULL);
    printf("Initial score computed: %f.\n", (double) (tv2.tv_usec - tv1.tv_usec) / 1000000 + (double) (tv2.tv_sec - tv1.tv_sec));

    // Iterations of simulated annealing
    double temp = (double) N_kw;
    int N_stuck = 0;
    for (int iter = 0; iter < N_iter; iter++)
    {
        /* Status code */
        if (iter % (N_iter / 10) == 0)
        {
            gettimeofday(&tv1, NULL);
            printf("Iteration: %d, %d, %d.\n", iter, N_stuck, (int) (tv1.tv_sec - tv2.tv_sec));
            fflush(stdout);
            gettimeofday(&tv2, NULL);
        }

	    // used to be 10k
        if (N_stuck >= N_kw * 20)
            iter = N_iter;

        /* Main code */
        int idx1, idx2;
        permutation_generation(&idx1, &idx2, &permutation_tmp, permutation, &permutation_inv, matrix_obs, matrix, fixed_padding, N_kw, N_obs, N_doc, sec_budget);
        if (idx1 == idx2)
        {
            N_stuck += 1;
                continue;
        }

        #pragma omp parallel for shared(score_row1)
        for (int ii = 0; ii < N_obs; ii++)
            score_row1[ii] = log_score(idx1, ii, matrix_obs, matrix, &permutation_tmp, N_kw, N_doc, sec_budget, fixed_padding);

        if (idx2 >= 0)
            #pragma omp parallel for shared(score_row2)
            for (int ii = 0; ii < N_obs; ii++)
                score_row2[ii] = log_score(idx2, ii, matrix_obs, matrix, &permutation_tmp, N_kw, N_doc, sec_budget, fixed_padding);

        double score_diff = 0;
        for (int ii = 0; ii < N_obs; ii++)
            score_diff += score_row1[ii];
        for (int ii = 0; ii < N_obs; ii++)
            score_diff -= score_matrix[idx1*N_obs + ii];
        if (idx2 >= 0)
        {
            for (int ii = 0; ii < N_obs; ii++)
                score_diff += score_row2[ii];
            for (int ii = 0; ii < N_obs; ii++)
                score_diff -= score_matrix[idx2*N_obs + ii];
        }

        // compute difference in score, with exponentiation
        score_diff = score_diff / temp;

        if (score_diff < -40)
            score_diff = 0;
        else if (score_diff > 0)
            score_diff = 1.01;
        else
            score_diff = exp(score_diff);

        if (gsl_ran_flat(r, 0, 1) < score_diff)
        {
            // Update the scores
            for (int ii = 0; ii < N_obs; ii++)
                score_matrix[idx1*N_obs + ii] = score_row1[ii];
            for (int ii = 0; ii < N_obs; ii++)
                score_matrix[ii*N_obs + idx1] = score_row1[ii];
            if (idx2 >= 0)
            {
                for (int ii = 0; ii < N_obs; ii++)
                    score_matrix[idx2*N_obs + ii] = score_row2[ii];
                for (int ii = 0; ii < N_obs; ii++)
                    score_matrix[ii*N_obs + idx2] = score_row2[ii];
            }

            // Update the permutation
            permutation_inv[(*permutation)[idx1]] = -1;
            (*permutation)[idx1] = permutation_tmp[idx1];
            permutation_inv[permutation_tmp[idx1]] = idx1;
            if (idx2 >= 0)
            {
                (*permutation)[idx2] = permutation_tmp[idx2];
                permutation_inv[permutation_tmp[idx2]] = idx2;
            }
            N_stuck = 0;
        }
        else
        {
            // Update the permutation
            permutation_tmp[idx1] = (*permutation)[idx1];
            if (idx2 >= 0)
                permutation_tmp[idx2] = (*permutation)[idx2];
            N_stuck += 1;
        }

        temp *= 0.995;
    }

    free(score_matrix);
    free(score_row1);
    free(score_row2);
    gsl_rng_free(r);
}

void print_result(char* output_fileName, int** permutation, int** true_index, int N_obs)
{
    FILE* fp = fopen(output_fileName, "w");
    int count = 0;
    for (int ii = 0; ii < N_obs; ii++)
    {
        //if (ii < 50)
        //    printf("%d, %d\n", (*permutation)[ii], (*true_index)[ii]);
        if ((*permutation)[ii] == (*true_index)[ii])
            count++;
    }

    fprintf(fp, "%d\n", count);
    fclose(fp);
    printf("Success: %d/%d.\n", count, N_obs);
}

void print_full_result(char* output_fileName, int** permutation, int** true_index, int N_obs)
{
    FILE* fp = fopen(output_fileName, "w");
    int count = 0;
    for (int ii = 0; ii < N_obs; ii++)
        if ((*permutation)[ii] == (*true_index)[ii])
            count++;

    fprintf(fp, "%d\n", count);
    for (int ii = 0; ii < N_obs; ii++)
        fprintf(fp, "%d,%d\n", (*permutation)[ii], (*true_index)[ii]);

    fclose(fp);
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpCosmo_core.h>
#include <gbpCosmo_NFW_etc.h>
#include <gsl/gsl_sf_expint.h>

// From Alam et al '02
double R_half_V_max_NFW(double M_vir, double z, int mode, cosmo_info **cosmo) {
    double c_vir;
    double R_vir;
    double r_o;
    double r_val = 0.;

    set_NFW_params(M_vir, z, mode, cosmo, &c_vir, &R_vir);

    r_o   = R_vir / c_vir;
    r_val = 0.13 * r_o;

    return (r_val);
}