#pragma once
#include <boost/operators.hpp>
#include <cmath>
#include <gsl/gsl>
#include <iostream>
#include <nlohmann/json.hpp>
#include <numeric>

namespace Simulator {
template <class T, class Category> class UnitBase : public boost::multiplicative<T, double> {
  public:
    UnitBase() = default;
    explicit constexpr UnitBase(double val) : _val(val) {}
    explicit constexpr UnitBase(long double val) : _val(gsl::narrow_cast<double>(val)) {}
    explicit constexpr UnitBase(int val) : _val(gsl::narrow<double>(val)) {}
    explicit constexpr UnitBase(std::int64_t val) : _val(gsl::narrow<double>(val)) {}

    explicit constexpr operator double() const noexcept { return _val; }
    explicit constexpr operator int() const noexcept { return gsl::narrow_cast<int>(_val); }
    explicit constexpr operator std::int64_t() const noexcept { return gsl::narrow_cast<std::int64_t>(_val); }
    explicit constexpr operator float() const noexcept { return gsl::narrow_cast<float>(_val); }

    constexpr bool operator<(const T &rhs) const noexcept { return _val < rhs._val; }
    constexpr bool operator<=(const T &rhs) const noexcept { return _val <= rhs._val; }
    constexpr bool operator>(const T &rhs) const noexcept { return _val > rhs._val; }
    constexpr bool operator>=(const T &rhs) const noexcept { return _val >= rhs._val; }
    constexpr bool operator==(const T &rhs) const noexcept { return _val == rhs._val; }
    constexpr bool operator!=(const T &rhs) const noexcept { return !(*this == rhs); }

    constexpr T &operator+=(const T &rhs) noexcept {
        _val += rhs._val;
        return static_cast<T &>(*this);
    }

    template <class U> constexpr T operator+(const UnitBase<U, Category> &rhs) const noexcept {
        auto ret = *this;
        return ret += rhs;
    }

    constexpr T &operator-=(const T &rhs) noexcept {
        _val -= rhs._val;
        return static_cast<T &>(*this);
    }

    template <class U> constexpr T operator-(const UnitBase<U, Category> &rhs) const noexcept {
        auto ret = *this;
        return ret -= rhs;
    }

    constexpr T &operator*=(double rhs) noexcept {
        _val *= rhs;
        return static_cast<T &>(*this);
    }

    constexpr T &operator/=(double rhs) noexcept {
        _val /= rhs;
        return static_cast<T &>(*this);
    }

    constexpr T operator-() const noexcept { return T(-_val); }

    constexpr double operator/(const T &rhs) const noexcept { return _val / rhs._val; }

    friend constexpr T abs(const T &x) noexcept { return T(std::abs(x._val)); }
    friend constexpr T round(const T &x) noexcept { return T(std::round(x._val)); }
    friend constexpr T floor(const T &x) noexcept { return T(std::floor(x._val)); }
    friend constexpr T ceil(const T &x) noexcept { return T(std::ceil(x._val)); }
    friend constexpr T fmod(const T &x, const T &y) { return T(std::fmod(x._val, y._val)); }
    friend std::string to_string(const T &x) { return std::to_string(x._val); }

    friend std::ostream &operator<<(std::ostream &os, const T &rhs) { return os << to_string(rhs); }
    friend std::istream &operator>>(std::istream &is, T &rhs) {
        is >> rhs._val;
        return is;
    }
    [[nodiscard]] constexpr double getValue() const noexcept { return _val; }

  protected:
    double _val = 0;
};

// NOLINTNEXTLINE
#define DEFINE_UNIT(name, cat, s)                                                                                \
    class name : public UnitBase<name, cat> {                                                                          \
      public:                                                                                \
        static constexpr name infinity() { return name(std::numeric_limits<double>::infinity()); }                     \
        using category = cat;                                                                                \
        static constexpr double scale = (s);                                                                           \
        using UnitBase<name, cat>::UnitBase;                                                                           \
        constexpr name(const UnitBase<name, cat> &other) : UnitBase<name, cat>(other) {}                               \
        template <class U>                                                                                \
        constexpr name(const UnitBase<U, cat> &other) : UnitBase<name, cat>(other.getValue() * (U::scale / scale)) {   \
            static_assert(U::scale == U::scale && scale == scale);                                                     \
        }                                                                                \
    };                                                                                \
    inline void from_json(const nlohmann::json &j, name &v) { v = name(j.get<double>()); }                             \
    inline void to_json(nlohmann::json &j, const name &v) { j = v.getValue(); }

struct MasteryCategory {};
DEFINE_UNIT(Mastery, MasteryCategory, 1.0);
struct CriticalRatingCategory {};
DEFINE_UNIT(CriticalRating, CriticalRatingCategory, 1.0);
struct AlacrityRatingCategory {};
DEFINE_UNIT(AlacrityRating, AlacrityRatingCategory, 1.0);
struct PowerCategory {};
DEFINE_UNIT(Power, PowerCategory, 1.0);
struct ForceTechDamageCategory {};
DEFINE_UNIT(FTPower, ForceTechDamageCategory, 1.0);
struct AccuracyRatingCategory {};
DEFINE_UNIT(AccuracyRating, AccuracyRatingCategory, 1.0);
struct TimeCategory {};
DEFINE_UNIT(Second, TimeCategory, 1.0);
DEFINE_UNIT(Minute, TimeCategory, 60.0);
constexpr Second _tinyTime{1e-7};
struct HealthCategory {};
DEFINE_UNIT(HealthPoints, HealthCategory, 1.0);
struct ArmorCatergory {};
DEFINE_UNIT(Armor, ArmorCatergory, 1.0);
struct EnergyCategory {};
DEFINE_UNIT(EnergyCost, EnergyCategory, 1.0);
} // namespace Simulator


#include "defines.h"
#include <string.h>
#include "lib.h"
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <sys/types.h>
#include <sys/stat.h>

extern REAL * opa;
extern int NTERMS;

/**
 * @param float * w
 * @param float * sig
 * @param float * spectro
 * @param int nspectro
 * @param float * spectra
 * @param float * d_spectra
 * @param float * beta
 * @param float * alpha
 *
 * */
int covarm(REAL *w,REAL *sig,float *spectro,int nspectro,REAL *spectra,REAL  *d_spectra,REAL *beta,REAL *alpha){

	int j,i,bt_nf,bt_nc,aux_nf,aux_nc;

	REAL AP[NTERMS*NTERMS*NPARMS],BT[NPARMS*NTERMS];

	REAL *BTaux,*APaux;

	//printf("\nVALORES DEL SIGMA SQUARE\n");

	for(j=0;j<NPARMS;j++){
		for(i=0;i<nspectro;i++){
			opa[i]= w[j]*(spectra[i+nspectro*j]-spectro[i+nspectro*j]);
		}

		BTaux=BT+(j*NTERMS);
		APaux=AP+(j*NTERMS*NTERMS);
		multmatrixIDLValueSigma(opa,nspectro,1,d_spectra+j*nspectro*NTERMS,NTERMS,nspectro,BTaux,&bt_nf,&bt_nc,sig+(nspectro*j)); //bt de tam NTERMS x 1
		multmatrix_transpose_sigma(d_spectra+j*nspectro*NTERMS,NTERMS,nspectro,d_spectra+j*nspectro*NTERMS,NTERMS,nspectro,APaux,&aux_nf,&aux_nc,w[j], sig+(nspectro*j));//ap de tam NTERMS x NTERMS

	}

	totalParcialf(BT,NPARMS,NTERMS,beta); //beta de tam 1 x NTERMS
	totalParcialMatrixf(AP,NTERMS,NTERMS,NPARMS,alpha); //alpha de tam NTERMS x NTERMS

	return 1;
}

/**
 * @param REAL * W
 * @param REAL * sig
 * @param float * spectro
 * @param int nspectro
 * @param REAL * spectra
 * @param REAL * d_spectra
 * @param REAL * beta
 * @param REAL * alpha
 *
 * */
int covarm2(REAL *w,REAL *sig,float *spectro,int nspectro,REAL *spectra,REAL  *d_spectra,REAL *beta,REAL *alpha){

	int j,i,h,k,aux_nf,aux_nc;

	REAL AP[NTERMS*NTERMS*NPARMS],BT[NPARMS*NTERMS];

	REAL *BTaux,*APaux;
	REAL sum,sum2;

	for(j=0;j<NPARMS;j++){
		for(i=0;i<nspectro;i++){
			opa[i]= w[j]*(spectra[i+nspectro*j]-spectro[i+nspectro*j]);
		}

		BTaux=BT+(j*NTERMS);
		APaux=AP+(j*NTERMS*NTERMS);

		for ( i = 0; i < NTERMS; i++){
		    for ( h = 0; h < NTERMS; h++){
				sum=0;
				if(i==0){
					sum2=0;
				}
				for ( k = 0;  k < nspectro; k++){
					REAL dAux = (*(d_spectra+j*nspectro*NTERMS+h*nspectro+k));
					sum += (*(d_spectra+j*nspectro*NTERMS+i*nspectro+k) * dAux ) * (w[j]/sig[nspectro*j+k]);

					if(i==0){
						sum2+= ((opa[k] * dAux  ))/sig[nspectro*j+k];
					}
				}

				APaux[NTERMS*i+h] = sum;
				if(i==0){
					BTaux[h] = sum2;
				}
     		}

		}
	}

	totalParcialf(BT,NPARMS,NTERMS,beta); //beta de tam 1 x NTERMS
	totalParcialMatrixf(AP,NTERMS,NTERMS,NPARMS,alpha); //alpha de tam NTERMS x NTERMS

	return 1;
}

/**
 * @param spectra: array with synthetic spectro
 * @param nspectro: size of spectro
 * @param spectro: original spectro
 * @param w: array of weight for I,Q,U,V
 * @param sig: array with sigma for I,Q,U,V
 * @param nfree: (nspectro * NPARMS) - NTERMS, NPARAMs is 4 and NTERMS 11.
 * */
REAL fchisqr(REAL * spectra,int nspectro,float *spectro,REAL *w,REAL *sig,REAL nfree){

	REAL TOT,dif1,dif2,dif3,dif4;
	REAL opa1,opa2,opa3,opa4;
	int i,j;

	TOT=0;
	opa1=0;
	opa2=0;
	opa3=0;
	opa4=0;
	for(i=0;i<nspectro;i++){
		dif1=spectra[i]-spectro[i];
		dif2=spectra[i+nspectro]-spectro[i+nspectro];
		dif3=spectra[i+nspectro*2]-spectro[i+nspectro*2];
		dif4=spectra[i+nspectro*3]-spectro[i+nspectro*3];

		opa1+= (((dif1*dif1)*w[0])/(sig[i]));
		opa2+= (((dif2*dif2)*w[1])/(sig[i+nspectro]));
		opa3+= (((dif3*dif3)*w[2])/(sig[i+nspectro*2]));
		opa4+= (((dif4*dif4)*w[3])/(sig[i+nspectro*3]));
	}
	TOT+= opa1+opa2+opa3+opa4;

	return TOT/nfree;

}

/*

	Multiplica la matriz a (tamaño naf,nac)
	por la matriz b (de tamaño nbf,nbc)
	al estilo IDL, es decir, filas de a por columnas de b,
	el resultado se almacena en resultOut (de tamaño fil,col)

	El tamaño de salida (fil,col) corresponde con (nbf,nac).

	El tamaño de columnas de b, nbc, debe de ser igual al de filas de a, naf.

*/

/**
 * @param REAL * a
 * @param int naf
 * @param int nac
 * @param REAL * b
 * @param int nbf
 * @param int nbc
 * @param REAL * result
 * @param int * fil
 * @param int * col
 * @param REAL value
 * */
int multmatrixIDLValue(REAL *a,int naf,int nac,REAL *b,int nbf,int nbc,REAL *result,int *fil,int *col,REAL value){

   int i,j,k;
   REAL sum;

	if(naf==nbc){
		(*fil)=nbf;
		(*col)=nac;

		for ( i = 0; i < nbf; i++){
		    for ( j = 0; j < nac; j++){
				sum=0;
				for ( k = 0;  k < naf; k++){
					sum += a[k*nac+j] * b[i*nbc+k];
				}
				result[((nac)*i)+j] = sum/value;
      		}
		}
		return 1;
	}
	else
		printf("\n \n Error en multmatrixIDLValue no coinciden nac y nbf!!!! ..\n\n");
	return 0;
}

/**
 *
 * */
int multmatrixIDLValueSigma(REAL *a,int naf,int nac,REAL *b,int nbf,int nbc,REAL *result,int *fil,int *col, REAL * sigma){

   int i,j,k;
   REAL sum;

	if(naf==nbc){
		(*fil)=nbf;
		(*col)=nac;

		for ( i = 0; i < nbf; i++){
				sum=0;
				for ( k = 0;  k < naf; k++){
						sum += (((a[k] * b[i*nbc+k])))/sigma[k];
				}
				result[i] = sum;
		}
		return 1;
	}
	else
		printf("\n \n Error en multmatrixIDLValue no coinciden nac y nbf!!!! ..\n\n");
	return 0;
}

/**
 * @param REAL * A
 * @param int f
 * @param int c
 * @param REAL * result
 * */
void totalParcialf(REAL * A, int f,int c,REAL * result){

	int i,j;
	REAL sum;
	for(i=0;i<c;i++){
		sum = 0;
		for(j=0;j<f;j++){
			sum+=A[j*c+i];
		}
		result[i] = sum;
	}
}

/**
 * @param REAL * A
 * @param int f
 * @param int c
 * @param int p
 * @param REAL * result
 * */
void totalParcialMatrixf(REAL * A, int f,int c,int p,REAL *result){

	int i,j,k;
	REAL sum;
	for(i=0;i<f;i++)
		for(j=0;j<c;j++){
			sum=0;
			for(k=0;k<p;k++)
				sum+=A[i*c+j+f*c*k];
			result[i*c+j] = sum;
		}
}

/**
 * @param PRECISION * a
 * @param int naf
 * @param int nac
 * @param PRECISION * b
 * @param int nbf
 * @param int nbc
 * @param PRECISION * result
 * @param int * fil
 * @param int * col
 *
 * Multiply matrix "a"(naf,nac) with matrix "b" (nbf,nbc). Algebraic matrix multiplication style, that is,
 * columns of "a" by rows of "b". The result is stored in "result"(fil,col).
 * The size of columns of "a", nac, must be the same of rows of "b", nbf.
 * */
int multmatrix(PRECISION *a,int naf,int nac, PRECISION *b,int nbf,int nbc,PRECISION *result,int *fil,int *col){

    int i,j,k;
    PRECISION sum;

	if(nac==nbf){
		(*fil)=naf;
		(*col)=nbc;

		for ( i = 0; i < naf; i++)
		    for ( j = 0; j < nbc; j++){
				sum=0;
				for ( k = 0;  k < nbf; k++){
					sum += a[i*nac+k] * b[k*nbc+j];
				}
				result[(*col)*i+j] = sum;
      		}
		return 1;
	}
	return 0;

}

/**
 * @param REAL * a
 * @param int naf
 * @param int nac
 * @param REAL *b
 * @param int nbf
 * @param int nbc
 * @param REAL * result
 * @param int * fil
 * @param int * col
 * @param REAL value
 * */
int multmatrix_transpose(REAL *a,int naf,int nac, REAL *b,int nbf,int nbc,REAL *result,int *fil,int *col,REAL value){

    int i,j,k;
    REAL sum;

	if(nac==nbc){
		(*fil)=naf;
		(*col)=nbf;

		for ( i = 0; i < naf; i++){
		    for ( j = 0; j < nbf; j++){
				sum=0;
				for ( k = 0;  k < nbc; k++){
					sum += a[i*nac+k] * b[j*nbc+k];
				}

				result[(*col)*i+j] = (sum)*value;
     		}

		}
		return 1;
	}else{
		printf("\n \n Error en multmatrix_transpose no coinciden nac y nbc!!!! ..\n\n");
	}

	return 0;
}

/**
 * @param REAL * a
 * @param int naf
 * @param int nac
 * @param REAL *b
 * @param int nbf
 * @param int nbc
 * @param REAL * result
 * @param int * fil
 * @param int * col
 * @param REAL weigth
 * @param REAL * sigma
 *
 * */
int multmatrix_transpose_sigma(REAL *a,int naf,int nac, REAL *b,int nbf,int nbc,REAL *result,int *fil,int *col,REAL weigth, REAL * sigma){

    int i,j,k;
    REAL sum;

	if(nac==nbc){
		(*fil)=naf;
		(*col)=nbf;

		for ( i = 0; i < naf; i++){
		    for ( j = 0; j < nbf; j++){
				sum=0;
				for ( k = 0;  k < nbc; k++){
						sum += (a[i*nac+k] * b[j*nbc+k]) * (weigth/sigma[k]);
				}

				result[(*col)*i+j] = sum;
     		}

		}
		return 1;
	}else{
		printf("\n \n Error en multmatrix_transpose no coinciden nac y nbc!!!! ..\n\n");
	}

	return 0;
}
//Media de un vector de longitud numl

/**
 * @param int nspectro
 */
int CalculaNfree(int nspectro)
{
	int nfree;
	nfree = 0;

	nfree = (nspectro * NPARMS) - NTERMS;

	return nfree;
}

/**
 * @param const char * path
 * */
int isDirectory(const char *path) {
   struct stat statbuf;
   if (stat(path, &statbuf) != 0)
       return 0;
   return S_ISDIR(statbuf.st_mode);
}

/**
 * @param tpuntero * cabeza
 * @param char * fileName
 * */
void insert_in_linked_list (tpuntero *cabeza, char * fileName){
    tpuntero nuevo;
    nuevo = malloc(sizeof(tnodo));
    strcpy(nuevo->d_name,fileName);
    nuevo->next = *cabeza;
    *cabeza = nuevo;
}

/**
 * @param tpuntero  cabeza
 * @param char * fileName
 * */
int checkNameInLista(tpuntero cabeza,char * fileName){
	int found = 0;
    while(cabeza != NULL && !found){ //Mientras cabeza no sea NULL
		if(strcmp(cabeza->d_name,fileName)==0)
			found = 1;
		else
        	cabeza = cabeza->next; //Pasamos al siguiente nodo
    }
	return found;
}

/**
 * @param tpuntero * cabeza
 * */

void deleteList(tpuntero *cabeza){
    tpuntero actual; //Puntero auxiliar para eliminar correctamente la lista

    while(*cabeza != NULL){ //Mientras cabeza no sea NULL
        actual = *cabeza; //Actual toma el valor de cabeza
        *cabeza = (*cabeza)->next; //Cabeza avanza 1 posicion en la lista
        free(actual); //Se libera la memoria de la posicion de Actual (el primer nodo), y cabeza queda apuntando al que ahora es el primero
    }
}


#pragma once

#include "mexObjectiveFunction.h"

#include <nlopt.h>
#include <mex.h>

struct mexConstraintFunction
{
  static double con_fun(unsigned n, const double *x, double *gradient, void *d_);
  static void vcon_fun(unsigned m, double *result, unsigned n, const double* x, double* grad, void* f_data);

  bool isvector;
  nlopt_func fun;   // (unsigned n, const double *x, double *fc, void *func_data)
  nlopt_mfunc mfun; // (unsigned m, double *result, unsigned n, const double *x, double *fc, void *func_data)

  mxArray *prhs[2];          // feval mexMatlabCall input arguments for objective function evaluation

  nlopt_opt &opt;
  mxArray *&lasterror;        // mexObjectiveFunction's to store trapped MException
  bool &stop;                 // mexObjectiveFunction's to flag if user issued a stop

  mexConstraintFunction(mxArray * mxFun, mexObjectiveFunction &data);
  ~mexConstraintFunction();
private:
  double evalFun(unsigned n, const double *x, double *gradient);
  void evalVecFun(unsigned m, unsigned n, const double* x, double *c, double* Cgrad);
  bool call_matlab_feval_with_trap(int nlhs, mxArray *plhs[], const int n, const double *x);
};


// Copyright 2019 Victor Hugo Schulz

// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at

//     http://www.apache.org/licenses/LICENSE-2.0

// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef STAR_IDENTIFICATION_H
#define STAR_IDENTIFICATION_H

extern "C"{
#include <cblas.h>
}
#include <algorithm>
#include <limits>
#include <vector>
#include <string.h>

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "structures_st.h"
#include "centroiding.h"
#include "standard_structures.h"

namespace st {

enum Stmode {REFERENCE, REFERENCE_BIN, BINARY, BINARY_INDEX, K};

class StarIdentification
{
public:
    StarIdentification();
    ~StarIdentification();
    std_str::Sky identifyStars(std_str::Sky csky);
    void reload();

    StarIdConfiguration config;
private:
    void bit_descriptor (unsigned long *bucket, unsigned int bucket_size, std::vector<cv::Point2d> &translated);
    void initialize_catalog(void *ptr, size_t size, std::string filename);
    void translate_rotate (std::vector<cv::Point2d> &translated, double &nn_dist, std::vector<std_str::Star> stars, uint ref);
    int normalized_coordinate (double pixel_coordinate);
    int cell(double x, double y);
    void descriptor(std::vector<int> &desc, std::vector<cv::Point2d> &translated);
    void pixel_to_unit_vector(cv::Point3d *unit_vector, cv::Point2d star);
    int array_intersection(int* array0, int size0, int* array1, int size1, int max_element);
    int binSearch(double what);
    int binSearch2(float what, float *data, int len);
    int classify(int *max, int *desc_size, std::vector<cv::Point2d> &translated);
    int classifyK(int *max, int *desc_size, std::vector<cv::Point2d> &translated, double nn_angle);
    int classify_binary(int *max, int *desc_size, std::vector<cv::Point2d> &translated, double nn_angle);
    int classify_binary_index(int *max, int *desc_size, std::vector<cv::Point2d> &translated, double nn_angle);
    int classify_binary_ref(int *max, int *desc_size, std::vector<cv::Point2d> &translated);
    void classifier(std::vector<Candidate> &candidates, std::vector<std_str::Star> stars);
    cv::Point3d cross(cv::Point3d a, cv::Point3d b);
    double vector_angle(cv::Point3d a, cv::Point3d b);
    void filter(std::vector<Candidate> & cluster, double fov);
    void cluster (std::vector<std::vector<Candidate> > &output, std::vector<Candidate> &candidates, double v_fov);
    std::vector<Candidate>* verify (std::vector<std::vector<Candidate> > &clustered);
    double fov(double size, double focus);
    std_str::Star to_star(Candidate cd);
    std::vector<std_str::Star> format_output(std::vector<Candidate> *verified);
    void loadConfig();
    void loadDatabase();

    double **ref_cat;
    unsigned long long **bin_cat;
    short **lut_cat;
    float **lut_nn_cat;
    short *index;

    Stmode mode;
};

}

#endif // STAR_IDENTIFICATION_H


#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <galpy_potentials.h>
// Reflex Motion potential - based on Moving Object Potential
// 3 arguments: amp, t0, tf
void constrain_range2(double * d) {
  // Constrains index to be within interpolation range
  if (*d < 0) *d = 0.0;
  if (*d > 1) *d = 1.0;
}
double ReflexMotionRforce(double R,double z, double phi,
				   double t,
				   struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,obj_z,obj_R,obj_phi,RF;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range2(&d_ind);
  // Interpolate x, y, z
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  obj_z= gsl_spline_eval(*(potentialArgs->spline1d+2),d_ind,
			 *(potentialArgs->acc1d+2));
  // Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  obj_R = pow(pow(obj_x,2) + pow(obj_y,2),0.5);
  obj_phi = atan2(obj_y,obj_x);
  // Calculate R force
  RF= calcRforce(obj_R,obj_z,obj_phi,t,potentialArgs->nwrapped,
		 potentialArgs->wrappedPotentialArg);
  return -amp*RF*(cos(phi)*cos(obj_phi)+sin(phi)*sin(obj_phi));
}

double ReflexMotionzforce(double R,double z,double phi,
				      double t,
				      struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,obj_z, obj_R, obj_phi;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range2(&d_ind);
  // Interpolate x, y, z
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  obj_z= gsl_spline_eval(*(potentialArgs->spline1d+2),d_ind,
			 *(potentialArgs->acc1d+2));
  // Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  obj_R = pow(pow(obj_x,2) + pow(obj_y,2),0.5);
  obj_phi = atan2(obj_y,obj_x);

  // Calculate z force
  return -amp * calczforce(obj_R,obj_z,obj_phi,t,potentialArgs->nwrapped,
			   potentialArgs->wrappedPotentialArg);
}

double ReflexMotionphiforce(double R,double z,double phi,
					double t,
					struct potentialArg * potentialArgs){
  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,obj_z, obj_R, obj_phi, RF;
  double * args= potentialArgs->args;
  //Get args
  amp= *args;
  t0= *(args+1);
  tf= *(args+2);
  d_ind= (t-t0)/(tf-t0);
  x= R*cos(phi);
  y= R*sin(phi);
  constrain_range2(&d_ind);
  // Interpolate x, y, z
  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
			 *(potentialArgs->acc1d+1));
  obj_z= gsl_spline_eval(*(potentialArgs->spline1d+2),d_ind,
			 *(potentialArgs->acc1d+2));
  // Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  obj_R = pow(pow(obj_x,2) + pow(obj_y,2),0.5);
  obj_phi = atan2(obj_y,obj_x);

  // Calculate phiforce
  RF= calcRforce(obj_R,obj_z,obj_phi,t,potentialArgs->nwrapped,
		 potentialArgs->wrappedPotentialArg);
  return amp*RF*(sin(phi)*cos(obj_phi)-cos(phi)*sin(obj_phi));
}

// double MovingObjectPotentialPlanarRforce(double R, double phi,
// 				      double t,
// 				      struct potentialArg * potentialArgs){
//  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,Rdist,RF;
//  double * args= potentialArgs->args;
  //Get args
//  amp= *args;
//  t0= *(args+1);
//  tf= *(args+2);
//  d_ind= (t-t0)/(tf-t0);
//  x= R*cos(phi);
//  y= R*sin(phi);
//  constrain_range(&d_ind);
  // Interpolate x, y
//  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
//  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
//			 *(potentialArgs->acc1d+1));
//  Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  // Calculate R force
//  RF= calcPlanarRforce(Rdist, phi, t, potentialArgs->nwrapped,
//		       potentialArgs->wrappedPotentialArg);
//  return -amp*RF*(cos(phi)*(obj_x-x)+sin(phi)*(obj_y-y))/Rdist;
//}

// double MovingObjectPotentialPlanarphiforce(double R, double phi,
//					double t,
//					struct potentialArg * potentialArgs){
//  double amp,t0,tf,d_ind,x,y,obj_x,obj_y,Rdist,RF;
//  double * args= potentialArgs->args;
  // Get args
//  amp= *args;
//  t0= *(args+1);
//  tf= *(args+2);
//  d_ind= (t-t0)/(tf-t0);
//  x= R*cos(phi);
//  y= R*sin(phi);
//  constrain_range(&d_ind);
  // Interpolate x, y
//  obj_x= gsl_spline_eval(*potentialArgs->spline1d,d_ind,*potentialArgs->acc1d);
//  obj_y= gsl_spline_eval(*(potentialArgs->spline1d+1),d_ind,
//			 *(potentialArgs->acc1d+1));
//  Rdist= pow(pow(x-obj_x, 2)+pow(y-obj_y, 2), 0.5);
  // Calculate phiforce
//  RF= calcPlanarRforce(Rdist, phi, t, potentialArgs->nwrapped,
//		       potentialArgs->wrappedPotentialArg);
//  return -amp*RF*R*(cos(phi)*(obj_y-y)-sin(phi)*(obj_x-x))/Rdist;
//}


#include <lapacke.h>
int main() { LAPACKE_dlapy2(0, 0); }


/*! \file Exceptions.h
 *  \brief header file that defines exceptions
 */
// Function.h

#ifndef NBODYEXCEPTION_H
#define NBODYEXCEPTION_H

#include <exception>
#include <string>
#include <utility>

#include <gsl/gsl_errno.h>

namespace Math
{

class gsl_error : public std::exception {

private:
	int gsl_errno;
	std::string errmsg;

public:
	gsl_error(int gsl_errno) :
		gsl_errno(gsl_errno),
		errmsg(gsl_strerror(gsl_errno))
	{
	}

	int get_gsl_errno() {
		return gsl_errno;
	}

	const char *what() const noexcept
	{
		return errmsg.c_str();
	}

};

template<typename Func, typename ... Args>
void gsl_invoke(Func &&f, Args ... args)
{
	int status = f(std::forward<Args>(args)...);
	if (status != GSL_SUCCESS) {
		throw gsl_error(status);
	}
}

}

#endif


/**
 *
 * @file zcgels.c
 *
 *  PLASMA computational routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Emmanuel Agullo
 * @date 2010-11-15
 * @precisions mixed zc -> ds
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <lapacke.h>
#include "common.h"

#define PLASMA_zlag2c(_descA, _descSB)                \
  plasma_parallel_call_4(plasma_pzlag2c,              \
                         PLASMA_desc,      (_descA),  \
                         PLASMA_desc,      (_descSB), \
                         PLASMA_sequence*, sequence,  \
                         PLASMA_request*,  request)

#define PLASMA_clag2z(_descSA, _descB)                \
  plasma_parallel_call_4(plasma_pclag2z,              \
                         PLASMA_desc,      (_descSA), \
                         PLASMA_desc,      (_descB),  \
                         PLASMA_sequence*, sequence,  \
                         PLASMA_request*,  request)

#define PLASMA_zlange(_norm, _descA, _result, _work)   \
  _result = 0;                                         \
  plasma_parallel_call_6(plasma_pzlange,               \
                         PLASMA_enum,      (_norm),    \
                         PLASMA_desc,      (_descA),   \
                         double*,          (_work),    \
                         double*,          &(_result), \
                         PLASMA_sequence*, sequence,   \
                         PLASMA_request*,  request);

#define PLASMA_zlacpy(_descA, _descB)                        \
  plasma_parallel_call_5(plasma_pzlacpy,                     \
                         PLASMA_enum,      PlasmaUpperLower, \
                         PLASMA_desc,      (_descA),         \
                         PLASMA_desc,      (_descB),         \
                         PLASMA_sequence*, sequence,         \
                         PLASMA_request*,  request)

#define PLASMA_zgeadd(_alpha, _descA, _descB)           \
  plasma_parallel_call_5(plasma_pzgeadd,                \
                         PLASMA_Complex64_t, (_alpha), \
                         PLASMA_desc,        (_descA), \
                         PLASMA_desc,        (_descB), \
                         PLASMA_sequence*,   sequence, \
                         PLASMA_request*,    request)

/***************************************************************************//**
 *
 * @ingroup PLASMA_Complex64_t
 *
 *  PLASMA_zcgels - Solves overdetermined or underdetermined linear systems involving an M-by-N
 *  matrix A using the QR or the LQ factorization of A.  It is assumed that A has full rank.
 *  The following options are provided:
 *
 *  # trans = PlasmaNoTrans and M >= N: find the least squares solution of an overdetermined
 *    system, i.e., solve the least squares problem: minimize || B - A*X ||.
 *
 *  # trans = PlasmaNoTrans and M < N:  find the minimum norm solution of an underdetermined
 *    system A * X = B.
 *
 *  Several right hand side vectors B and solution vectors X can be handled in a single call;
 *  they are stored as the columns of the M-by-NRHS right hand side matrix B and the N-by-NRHS
 *  solution matrix X.
 *
 *  PLASMA_zcgels first attempts to factorize the matrix in COMPLEX and use this
 *  factorization within an iterative refinement procedure to produce a
 *  solution with COMPLEX*16 normwise backward error quality (see below).
 *  If the approach fails the method switches to a COMPLEX*16
 *  factorization and solve.
 *
 *  The iterative refinement is not going to be a winning strategy if
 *  the ratio COMPLEX performance over COMPLEX*16 performance is too
 *  small. A reasonable strategy should take the number of right-hand
 *  sides and the size of the matrix into account. This might be done
 *  with a call to ILAENV in the future. Up to now, we always try
 *  iterative refinement.
 *
 *  The iterative refinement process is stopped if ITER > ITERMAX or
 *  for all the RHS we have: RNRM < N*XNRM*ANRM*EPS*BWDMAX
 *  where:
 *
 *  - ITER is the number of the current iteration in the iterative refinement process
 *  - RNRM is the infinity-norm of the residual
 *  - XNRM is the infinity-norm of the solution
 *  - ANRM is the infinity-operator-norm of the matrix A
 *  - EPS is the machine epsilon returned by DLAMCH('Epsilon').
 *
 *  Actually, in its current state (PLASMA 2.1.0), the test is slightly relaxed.
 *
 *  The values ITERMAX and BWDMAX are fixed to 30 and 1.0D+00 respectively.
 *
 *  We follow Bjorck's algorithm proposed in "Iterative Refinement of Linear
 *  Least Squares solutions I", BIT, 7:257-278, 1967.
 *
 *******************************************************************************
 *
 * @param[in] trans
 *          Intended usage:
 *          = PlasmaNoTrans:   the linear system involves A;
 *          = PlasmaConjTrans: the linear system involves A**H.
 *          Currently only PlasmaNoTrans is supported.
 *
 * @param[in] M
 *          The number of rows of the matrix A. M >= 0.
 *
 * @param[in] N
 *          The number of columns of the matrix A. N >= 0.
 *
 * @param[in] NRHS
 *          The number of right hand sides, i.e., the number of columns of the matrices B and X.
 *          NRHS >= 0.
 *
 * @param[in] A
 *          The M-by-N matrix A. This matrix is not modified.
 *
 * @param[in] LDA
 *          The leading dimension of the array A. LDA >= max(1,M).
 *
 * @param[in] B
 *          The M-by-NRHS matrix B of right hand side vectors, stored columnwise. Not modified.
 *
 * @param[in] LDB
 *          The leading dimension of the array B. LDB >= MAX(1,M,N).
 *
 * @param[out] X
 *          If return value = 0, the solution vectors, stored columnwise.
 *          if M >= N, rows 1 to N of X contain the least squares solution vectors; the residual
 *          sum of squares for the solution in each column is given by the sum of squares of the
 *          modulus of elements N+1 to M in that column;
 *          if M < N, rows 1 to N of X contain the minimum norm solution vectors;
 *
 * @param[in] LDX
 *          The leading dimension of the array X. LDX >= MAX(1,M,N).
 *
 * @param[out] ITER
 *          The number of the current iteration in the iterative refinement process
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 *******************************************************************************
 *
 * @sa PLASMA_zcgels_Tile
 * @sa PLASMA_zcgels_Tile_Async
 * @sa PLASMA_dsgels
 * @sa PLASMA_zgels
 *
 ******************************************************************************/
int PLASMA_zcgels(PLASMA_enum trans, int M, int N, int NRHS,
                  PLASMA_Complex64_t *A, int LDA,
                  PLASMA_Complex64_t *B, int LDB,
                  PLASMA_Complex64_t *X, int LDX, int *ITER)
{
    int i, j;
    int NB, NBNB, MT, NT, NTRHS;
    int status;
    PLASMA_desc  descA;
    PLASMA_desc  descB;
    PLASMA_desc *descT;
    PLASMA_desc  descX;
    plasma_context_t *plasma;
    PLASMA_sequence *sequence = NULL;
    PLASMA_request request = PLASMA_REQUEST_INITIALIZER;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_zcgels", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }

    *ITER = 0;

    /* Check input arguments */
    if (trans != PlasmaNoTrans   &&
        trans != PlasmaConjTrans &&
        trans != PlasmaTrans )
    {
        plasma_error("PLASMA_zcgels", "illegal value of trans");
        return -1;
    }
    if (trans != PlasmaNoTrans) {
        plasma_error("PLASMA_zcgels", "only PlasmaNoTrans supported");
        return PLASMA_ERR_NOT_SUPPORTED;
    }
    if (M < 0) {
        plasma_error("PLASMA_zcgels", "illegal value of M");
        return -2;
    }
    if (N < 0) {
        plasma_error("PLASMA_zcgels", "illegal value of N");
        return -3;
    }
    if (NRHS < 0) {
        plasma_error("PLASMA_zcgels", "illegal value of NRHS");
        return -4;
    }
    if (LDA < max(1, M)) {
        plasma_error("PLASMA_zcgels", "illegal value of LDA");
        return -6;
    }
    if (LDB < max(1, max(M, N))) {
        plasma_error("PLASMA_zcgels", "illegal value of LDB");
        return -9;
    }
    if (LDX < max(1, max(M, N))) {
        plasma_error("PLASMA_zcgels", "illegal value of LDX");
        return -10;
    }
    /* Quick return */
    if (min(M, min(N, NRHS)) == 0) {
        for (i = 0; i < max(M, N); i++)
            for (j = 0; j < NRHS; j++)
                B[j*LDB+i] = 0.0;
        return PLASMA_SUCCESS;
    }

    /* Tune NB & IB depending on M, N & NRHS; Set NBNB */
    status = plasma_tune(PLASMA_FUNC_ZCGELS, M, N, NRHS);
    if (status != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zcgels", "plasma_tune() failed");
        return status;
    }

    /* Set MT, NT & NTRHS */
    NB    = PLASMA_NB;
    NBNB  = NB*NB;
    NT    = (N%NB==0) ? (N/NB) : (N/NB+1);
    MT    = (M%NB==0) ? (M/NB) : (M/NB+1);
    NTRHS = (NRHS%NB==0) ? (NRHS/NB) : (NRHS/NB+1);
    printf("M %d, N %d, NRHS %d, NB %d, MT %d, NT %d, NTRHS %d\n", M, N, NRHS, NB, MT, NT, NTRHS);

    plasma_sequence_create(plasma, &sequence);

    descA = plasma_desc_init(
                PlasmaComplexDouble,
                NB, NB, NBNB,
                M, N, 0, 0, M, N);

    if (M >= N) {
        descB = plasma_desc_init(
            PlasmaComplexDouble,
            NB, NB, NBNB,
            M, NRHS, 0, 0, M, NRHS);

        descX = plasma_desc_init(
            PlasmaComplexDouble,
            NB, NB, NBNB,
            M, NRHS, 0, 0, M, NRHS);

    }
    else {
        descB = plasma_desc_init(
            PlasmaComplexDouble,
            NB, NB, NBNB,
            N, NRHS, 0, 0, N, NRHS);

        descX = plasma_desc_init(
            PlasmaComplexDouble,
            NB, NB, NBNB,
            N, NRHS, 0, 0, N, NRHS);
    }

    /* DOUBLE PRECISION INITIALIZATION */
    /* Allocate memory for matrices in block layout */
    if (plasma_desc_mat_alloc(&descA) || plasma_desc_mat_alloc(&descB) || plasma_desc_mat_alloc(&descX)) {
        plasma_error("PLASMA_zcgels", "plasma_shared_alloc() failed");
        plasma_desc_mat_free(&descA);
        plasma_desc_mat_free(&descB);
        plasma_desc_mat_free(&descX);
        return PLASMA_ERR_OUT_OF_RESOURCES;
    }

    plasma_parallel_call_5(plasma_pzlapack_to_tile,
        PLASMA_Complex64_t*, A,
        int, LDA,
        PLASMA_desc, descA,
        PLASMA_sequence*, sequence,
        PLASMA_request*, &request);

    plasma_parallel_call_5(plasma_pzlapack_to_tile,
        PLASMA_Complex64_t*, B,
        int, LDB,
        PLASMA_desc, descB,
        PLASMA_sequence*, sequence,
        PLASMA_request*, &request);

    /* Allocate workspace */
    PLASMA_Alloc_Workspace_zgels_Tile(M, N, &descT);

    /* Call the native interface */
    status = PLASMA_zcgels_Tile_Async(PlasmaNoTrans, &descA, descT, &descB, &descX, ITER,
                                      sequence, &request);

    if (status == PLASMA_SUCCESS) {
        plasma_parallel_call_5(plasma_pztile_to_lapack,
            PLASMA_desc, descX,
            PLASMA_Complex64_t*, X,
            int, LDX,
            PLASMA_sequence*, sequence,
            PLASMA_request*, &request);
    }
    plasma_dynamic_sync();

    PLASMA_Dealloc_Handle_Tile(&descT);
    plasma_sequence_destroy(plasma, sequence);
    plasma_desc_mat_free(&descA);
    plasma_desc_mat_free(&descB);
    plasma_desc_mat_free(&descX);
    return status;
}

/***************************************************************************//**
 *
 * @ingroup PLASMA_Complex64_t_Tile
 *
 *  PLASMA_zcgels_Tile - Solves overdetermined or underdetermined linear system of equations
 *  using the tile QR or the tile LQ factorization and mixed-precision iterative refinement.
 *  Tile equivalent of PLASMA_zcgesv().
 *  Operates on matrices stored by tiles.
 *  All matrices are passed through descriptors.
 *  All dimensions are taken from the descriptors.
 *
 *******************************************************************************
 *
 * @param[in] trans
 *          Intended usage:
 *          = PlasmaNoTrans:   the linear system involves A;
 *          = PlasmaConjTrans: the linear system involves A**H.
 *          Currently only PlasmaNoTrans is supported.
 *
 * @param[in,out] A
 *          - If the iterative refinement converged, A is not modified;
 *          - otherwise, it fell back to double precision solution, and
 *          on exit the M-by-N matrix A contains:
 *          if M >= N, A is overwritten by details of its QR factorization as returned by
 *                     PLASMA_zgeqrf;
 *          if M < N, A is overwritten by details of its LQ factorization as returned by
 *                      PLASMA_zgelqf.
 *
 * @param[out] T
 *          On exit:
 *          - if the iterative refinement converged, T is not modified;
 *          - otherwise, it fell back to double precision solution,
 *          and then T is an auxiliary factorization data.
 *
 * @param[in,out] B
 *          On entry, the M-by-NRHS matrix B of right hand side vectors, stored columnwise;
 *          On exit, if return value = 0, B is overwritten by the solution vectors, stored
 *          columnwise:
 *          if M >= N, rows 1 to N of B contain the least squares solution vectors; the residual
 *          sum of squares for the solution in each column is given by the sum of squares of the
 *          modulus of elements N+1 to M in that column;
 *          if M < N, rows 1 to N of B contain the minimum norm solution vectors;
 *
 * @param[in] B
 *          The descriptor of the M-by-NRHS matrix B of right hand side vectors, stored columnwise. Not modified.
 *
 * @param[in,out] X
 *          On entry, it's only the descriptor where to store the result.
 *          On exit, if return value = 0, X is the solution vectors, stored columnwise:
 *          if M >= N, rows 1 to N of X contain the least squares solution vectors; the residual
 *          sum of squares for the solution in each column is given by the sum of squares of the
 *          modulus of elements N+1 to M in that column;
 *          if M < N, rows 1 to N of X contain the minimum norm solution vectors;
 *
 * @param[out] ITER
 *          If > 0, ITER is the number of the current iteration in the iterative refinement process.
 *          -ITERMAX-1, if the refinment step failed and the double precision factorization has been used.
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *
 *******************************************************************************
 *
 * @sa PLASMA_zcgels
 * @sa PLASMA_zcgels_Tile_Async
 * @sa PLASMA_dsgels_Tile
 * @sa PLASMA_zgels_Tile
 *
 ******************************************************************************/
int PLASMA_zcgels_Tile(PLASMA_enum trans, PLASMA_desc *A, PLASMA_desc *T,
                       PLASMA_desc *B, PLASMA_desc *X, int *ITER)
{
    plasma_context_t *plasma;
    PLASMA_sequence *sequence = NULL;
    PLASMA_request request = PLASMA_REQUEST_INITIALIZER;
    int status;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_zcgels_Tile", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    plasma_sequence_create(plasma, &sequence);
    status = PLASMA_zcgels_Tile_Async(trans, A, T, B, X, ITER, sequence, &request);
    if (status != PLASMA_SUCCESS)
        return status;
    plasma_dynamic_sync();
    status = sequence->status;
    plasma_sequence_destroy(plasma, sequence);
    return status;
}

/***************************************************************************//**
 *
 * @ingroup PLASMA_Complex64_t_Tile_Async
 *
 *  PLASMA_zcgels_Tile_Async - Solves overdetermined or underdetermined linear
 *  system of equations using the tile QR or the tile LQ factorization and
 *  mixed-precision iterative refinement.
 *  Non-blocking equivalent of PLASMA_zcgels_Tile().
 *  May return before the computation is finished.
 *  Allows for pipelining of operations at runtime.
 *
 *******************************************************************************
 *
 * @param[in] sequence
 *          Identifies the sequence of function calls that this call belongs to
 *          (for completion checks and exception handling purposes).
 *
 * @param[out] request
 *          Identifies this function call (for exception handling purposes).
 *
 *******************************************************************************
 *
 * @sa PLASMA_zcgels
 * @sa PLASMA_zcgels_Tile
 * @sa PLASMA_dsgels_Tile_Async
 * @sa PLASMA_zgels_Tile_Async
 *
 ******************************************************************************/
int PLASMA_zcgels_Tile_Async(PLASMA_enum trans, PLASMA_desc *A, PLASMA_desc *T,
                             PLASMA_desc *B, PLASMA_desc *X, int *ITER,
                             PLASMA_sequence *sequence, PLASMA_request *request)
{
    int M, N, NRHS, NB, NBNB, MT, NT, NTRHS;
    PLASMA_desc descA;
    PLASMA_desc descT;
    PLASMA_desc descB;
    PLASMA_desc descX;
    plasma_context_t *plasma;
    double *work;

    const int itermax = 30;
    const double bwdmax = 1.0;
    const PLASMA_Complex64_t negone = -1.0;
    const PLASMA_Complex64_t one = 1.0;
    int iiter;
    double Anorm, cte, eps, Rnorm, Xnorm;
    *ITER=0;

    plasma = plasma_context_self();
    if (plasma == NULL) {
        plasma_fatal_error("PLASMA_zcgels_Tile", "PLASMA not initialized");
        return PLASMA_ERR_NOT_INITIALIZED;
    }
    if (sequence == NULL) {
        plasma_fatal_error("PLASMA_zcgels_Tile", "NULL sequence");
        return PLASMA_ERR_UNALLOCATED;
    }
    if (request == NULL) {
        plasma_fatal_error("PLASMA_zcgels_Tile", "NULL request");
        return PLASMA_ERR_UNALLOCATED;
    }
    /* Check sequence status */
    if (sequence->status == PLASMA_SUCCESS)
        request->status = PLASMA_SUCCESS;
    else
        return plasma_request_fail(sequence, request, PLASMA_ERR_SEQUENCE_FLUSHED);

    /* Check descriptors for correctness */
    if (plasma_desc_check(A) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zcgels_Tile", "invalid first descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descA = *A;
    }
    if (plasma_desc_check(T) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zcgels_Tile", "invalid second descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descT = *T;
    }
    if (plasma_desc_check(B) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zcgels_Tile", "invalid third descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descB = *B;
    }
    if (plasma_desc_check(X) != PLASMA_SUCCESS) {
        plasma_error("PLASMA_zcgels_Tile", "invalid fourth descriptor");
        return PLASMA_ERR_ILLEGAL_VALUE;
    } else {
        descX = *X;
    }
    /* Check input arguments */
    if (descA.nb != descA.mb || descB.nb != descB.mb || descX.nb != descX.mb) {
        plasma_error("PLASMA_zcgels_Tile", "only square tiles supported");
        return PLASMA_ERR_ILLEGAL_VALUE;
    }
    if (trans != PlasmaNoTrans) {
        plasma_error("PLASMA_zcgels_Tile", "only PlasmaNoTrans supported");
        return PLASMA_ERR_NOT_SUPPORTED;
    }
    /* Quick return  - currently NOT equivalent to LAPACK's:
    if (min(M, min(N, NRHS)) == 0) {
        for (i = 0; i < max(M, N); i++)
            for (j = 0; j < NRHS; j++)
                B[j*LDB+i] = 0.0;
        return PLASMA_SUCCESS;
    }
    */

    if (0 == 0) {
    // START SPECIFIC

    /* Set M, M, NRHS, NB, MT, NT & NTRHS */
    M    = descA.lm;
    N    = descA.ln;
    NRHS = descB.ln;
    NB   = descA.nb;
    NBNB = NB*NB;

    MT = (M%NB==0) ? (M/NB) : (M/NB+1);
    NT = (N%NB==0) ? (N/NB) : (N/NB+1);
    NTRHS = (NRHS%NB==0) ? (NRHS/NB) : (NRHS/NB+1);
    printf("M %d, N %d, NRHS %d, NB %d, MT %d, NT %d, NTRHS %d\n", M, N, NRHS, NB, MT, NT, NTRHS);

    work = (double *)plasma_shared_alloc(plasma, PLASMA_SIZE, PlasmaRealDouble);
    if (work == NULL) {
        plasma_error("PLASMA_zcgesv", "plasma_shared_alloc() failed");
        plasma_shared_free(plasma, work);
        return PLASMA_ERR_OUT_OF_RESOURCES;
    }

    PLASMA_desc descR = plasma_desc_init(
        PlasmaComplexDouble,
        NB, NB, NBNB,
        M, NRHS, 0, 0, M, NRHS);

    if (plasma_desc_mat_alloc(&descR)) {
        plasma_error("PLASMA_zcgesv", "plasma_shared_alloc() failed");
        plasma_desc_mat_free(&descR);
        plasma_shared_free(plasma, work);
        return PLASMA_ERR_OUT_OF_RESOURCES;
    }

    PLASMA_desc descSA = plasma_desc_init(
            PlasmaComplexFloat,
            NB, NB, NBNB,
            M, N, 0, 0, M, N);

    PLASMA_desc descST = plasma_desc_init(
            PlasmaComplexFloat,
            IB, NB, IBNB,
            M, N, 0, 0, M, N);

    PLASMA_desc descSX = plasma_desc_init(
            PlasmaComplexFloat,
            NB, NB, NBNB,
            M, NRHS, 0, 0, M, NRHS);

    /* Allocate memory for single precision matrices in block layout */
    if (plasma_desc_mat_alloc(&descSA) || plasma_desc_mat_alloc(&descST) || plasma_desc_mat_alloc(&descSX)) {
        plasma_error("PLASMA_zcgesv", "plasma_shared_alloc() failed");
        plasma_desc_mat_free(&descSA);
        plasma_desc_mat_free(&descST);
        plasma_desc_mat_free(&descSX);
        plasma_desc_mat_free(&descR);
        plasma_shared_free(plasma, work);
        return PLASMA_ERR_OUT_OF_RESOURCES;
    }

    /* Compute some constants */
    PLASMA_zlange(PlasmaInfNorm, descA, Anorm, work);
    eps = LAPACKE_dlamch_work('e');

    printf("Anorm=%e, cte=%e\n", Anorm, cte);

    /* Convert B from double precision to single precision and store
       the result in SX. */
    PLASMA_zlag2c(descB, descSX);
    if (sequence->status != PLASMA_SUCCESS)
        return plasma_request_fail(sequence, request, PLASMA_ERR_SEQUENCE_FLUSHED);

    /* Convert A from double precision to single precision and store
       the result in SA. */
    PLASMA_zlag2c(descA, descSA);
    if (sequence->status != PLASMA_SUCCESS)
        return plasma_request_fail(sequence, request, PLASMA_ERR_SEQUENCE_FLUSHED);

    if (descSA.m >= descSA.n) {

        /* Compute the QR factorization of SA */
        printf("Facto\n"); fflush(stdout);
        plasma_parallel_call_4(plasma_pcgeqrf,
            PLASMA_desc, descSA,
            PLASMA_desc, descST,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        printf("Solve\n"); fflush(stdout);
        plasma_parallel_call_5(plasma_pcunmqr,
            PLASMA_desc, descSA,
            PLASMA_desc, descSX,
            PLASMA_desc, descST,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_9(plasma_pctrsm,
            PLASMA_enum, PlasmaLeft,
            PLASMA_enum, PlasmaUpper,
            PLASMA_enum, PlasmaNoTrans,
            PLASMA_enum, PlasmaNonUnit,
            PLASMA_Complex32_t, 1.0,
            PLASMA_desc, plasma_desc_submatrix(descSA, 0, 0, descSA.n, descSA.n),
            PLASMA_desc, plasma_desc_submatrix(descSX, 0, 0, descSA.n, descSX.n),
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);
    }
    else {
        plasma_parallel_call_3(plasma_pztile_zero,
            PLASMA_desc, plasma_desc_submatrix(descSX, descSA.m, 0, descSA.n-descSA.m, descSX.n),
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_4(plasma_pcgelqf,
            PLASMA_desc, descSA,
            PLASMA_desc, descST,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_9(plasma_pctrsm,
            PLASMA_enum, PlasmaLeft,
            PLASMA_enum, PlasmaLower,
            PLASMA_enum, PlasmaNoTrans,
            PLASMA_enum, PlasmaNonUnit,
            PLASMA_Complex32_t, 1.0,
            PLASMA_desc, plasma_desc_submatrix(descSA, 0, 0, descSA.m, descSA.m),
            PLASMA_desc, plasma_desc_submatrix(descSX, 0, 0, descSA.m, descSX.n),
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_5(plasma_pcunmlq,
            PLASMA_desc, descSA,
            PLASMA_desc, descSX,
            PLASMA_desc, descST,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);
    }

    /* Convert SX back to double precision */
    PLASMA_clag2z(descSX, descX);

    /* Compute R = B - AX. */
    printf("R = B - Ax\n"); fflush(stdout);
    printf("R = B - Ax ... cpy\n"); fflush(stdout);
    PLASMA_zlacpy(descB,descR);
    printf("R = B - Ax ... gemm\n"); fflush(stdout);

    plasma_parallel_call_9(plasma_pzgemm,
        PLASMA_enum, PlasmaNoTrans,
        PLASMA_enum, PlasmaNoTrans,
        PLASMA_Complex64_t, negone,
        PLASMA_desc, descA,
        PLASMA_desc, descX,
        PLASMA_Complex64_t, one,
        PLASMA_desc, descR,
        PLASMA_sequence*, sequence,
        PLASMA_request*, request);

    /* Check whether the NRHS normwise backward error satisfies the
       stopping criterion. If yes return. Note that ITER=0 (already set). */
    printf("Norm of X and R\n"); fflush(stdout);
    PLASMA_zlange(PlasmaInfNorm, descX, Xnorm, work);
    PLASMA_zlange(PlasmaInfNorm, descR, Rnorm, work);

    /* Wait the end of Anorm, Xnorm and Bnorm computations */
    plasma_dynamic_sync();

    cte = Anorm*eps*((double) N)*bwdmax;
    if (Rnorm < Xnorm * cte){
        /* The NRHS normwise backward errors satisfy the
           stopping criterion. We are good to exit. */
        plasma_desc_mat_free(&descSA);
        plasma_desc_mat_free(&descST);
        plasma_desc_mat_free(&descSX);
        plasma_desc_mat_free(&descR);
        plasma_shared_free(plasma, work);
        return PLASMA_SUCCESS;
    }

    printf("Rnorm=%e, Xnorm * cte=%e, Rnorm=%e, cte=%e\n", Rnorm, Xnorm * cte, Rnorm, cte);

    /* Iterative refinement */
    for (iiter = 0; iiter < itermax; iiter++){

        /* Convert R from double precision to single precision
           and store the result in SX. */
        PLASMA_zlag2c(descR, descSX);

        /* Solve the system SA*SX = SR */
        if (descSA.m >= descSA.n) {

            plasma_parallel_call_5(plasma_pcunmqr,
                PLASMA_desc, descSA,
                PLASMA_desc, descSX,
                PLASMA_desc, descST,
                PLASMA_sequence*, sequence,
                PLASMA_request*, request);

            plasma_parallel_call_9(plasma_pctrsm,
                PLASMA_enum, PlasmaLeft,
                PLASMA_enum, PlasmaUpper,
                PLASMA_enum, PlasmaNoTrans,
                PLASMA_enum, PlasmaNonUnit,
                PLASMA_Complex32_t, 1.0,
                PLASMA_desc, plasma_desc_submatrix(descSA, 0, 0, descSA.n, descSA.n),
                PLASMA_desc, plasma_desc_submatrix(descSX, 0, 0, descSA.n, descSX.n),
                PLASMA_sequence*, sequence,
                PLASMA_request*, request);
        } else {
            plasma_parallel_call_9(plasma_pctrsm,
                PLASMA_enum, PlasmaLeft,
                PLASMA_enum, PlasmaLower,
                PLASMA_enum, PlasmaNoTrans,
                PLASMA_enum, PlasmaNonUnit,
                PLASMA_Complex32_t, 1.0,
                PLASMA_desc, plasma_desc_submatrix(descSA, 0, 0, descSA.m, descSA.m),
                PLASMA_desc, plasma_desc_submatrix(descSX, 0, 0, descSA.m, descSX.n),
                PLASMA_sequence*, sequence,
                PLASMA_request*, request);

            plasma_parallel_call_5(plasma_pcunmlq,
                PLASMA_desc, descSA,
                PLASMA_desc, descSX,
                PLASMA_desc, descST,
                PLASMA_sequence*, sequence,
                PLASMA_request*, request);
        }

        /* Convert SX back to double precision and update the current
           iterate. */
        PLASMA_clag2z(descSX, descR);
        PLASMA_zgeadd(one, descR, descX);

        /* Compute R = B - AX. */
        PLASMA_zlacpy(descB,descR);
        plasma_parallel_call_9(plasma_pzgemm,
            PLASMA_enum, PlasmaNoTrans,
            PLASMA_enum, PlasmaNoTrans,
            PLASMA_Complex64_t, negone,
            PLASMA_desc, descA,
            PLASMA_desc, descX,
            PLASMA_Complex64_t, one,
            PLASMA_desc, descR,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        /* Check whether the NRHS normwise backward errors satisfy the
           stopping criterion. If yes, set ITER=IITER>0 and return. */
        PLASMA_zlange(PlasmaInfNorm, descX, Xnorm, work);
        PLASMA_zlange(PlasmaInfNorm, descR, Rnorm, work);

        /* Wait the end of Xnorm and Bnorm computations */
        plasma_dynamic_sync();

        printf("Rnorm=%e, Xnorm * cte=%e, Rnorm=%e, cte=%e\n", Rnorm, Xnorm * cte, Rnorm, cte);

        if (Rnorm < Xnorm * cte){
            /* The NRHS normwise backward errors satisfy the
               stopping criterion. We are good to exit. */
            *ITER = iiter;

            plasma_desc_mat_free(&descSA);
            plasma_desc_mat_free(&descST);
            plasma_desc_mat_free(&descSX);
            plasma_desc_mat_free(&descR);
            plasma_shared_free(plasma, work);
            return PLASMA_SUCCESS;
        }
    }

    /* We have performed ITER=itermax iterations and never satisified
       the stopping criterion, set up the ITER flag accordingly and
       follow up on double precision routine. */
    *ITER = -itermax - 1;

    plasma_desc_mat_free(&descSA);
    plasma_desc_mat_free(&descST);
    plasma_desc_mat_free(&descSX);
    plasma_desc_mat_free(&descR);
    plasma_shared_free(plasma, work);

    printf("Go back DOUBLE\n");
    // END SPECIFIC
    }

    /* Single-precision iterative refinement failed to converge to a
       satisfactory solution, so we resort to double precision. */
    PLASMA_zlacpy(descB, descX);

    if (descA.m >= descA.n) {
        plasma_parallel_call_4(plasma_pzgeqrf,
            PLASMA_desc, descA,
            PLASMA_desc, descT,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_5(plasma_pzunmqr,
            PLASMA_desc, descA,
            PLASMA_desc, descX,
            PLASMA_desc, descT,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_9(plasma_pztrsm,
            PLASMA_enum, PlasmaLeft,
            PLASMA_enum, PlasmaUpper,
            PLASMA_enum, PlasmaNoTrans,
            PLASMA_enum, PlasmaNonUnit,
            PLASMA_Complex64_t, 1.0,
            PLASMA_desc, plasma_desc_submatrix(descA, 0, 0, descA.n, descA.n),
            PLASMA_desc, plasma_desc_submatrix(descX, 0, 0, descA.n, descX.n),
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);
    }
    else {
        plasma_parallel_call_3(plasma_pztile_zero,
            PLASMA_desc, plasma_desc_submatrix(descX, descA.m, 0, descA.n-descA.m, descX.n),
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_4(plasma_pzgelqf,
            PLASMA_desc, descA,
            PLASMA_desc, descT,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_9(plasma_pztrsm,
            PLASMA_enum, PlasmaLeft,
            PLASMA_enum, PlasmaLower,
            PLASMA_enum, PlasmaNoTrans,
            PLASMA_enum, PlasmaNonUnit,
            PLASMA_Complex64_t, 1.0,
            PLASMA_desc, plasma_desc_submatrix(descA, 0, 0, descA.m, descA.m),
            PLASMA_desc, plasma_desc_submatrix(descX, 0, 0, descA.m, descX.n),
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);

        plasma_parallel_call_5(plasma_pzunmlq,
            PLASMA_desc, descA,
            PLASMA_desc, descX,
            PLASMA_desc, descT,
            PLASMA_sequence*, sequence,
            PLASMA_request*, request);
    }
    return PLASMA_SUCCESS;
}


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

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_movstat.h>

typedef double ringbuf_type_t;

#include "ringbuf.c"

typedef struct
{
  double sum;     /* current window sum */
  ringbuf *rbuf;  /* ring buffer storing current window */
} sumacc_state_t;

static size_t
sumacc_size(const size_t n)
{
  size_t size = 0;

  size += sizeof(sumacc_state_t);
  size += ringbuf_size(n);

  return size;
}

static int
sumacc_init(const size_t n, void * vstate)
{
  sumacc_state_t * state = (sumacc_state_t *) vstate;

  state->sum = 0.0;

  state->rbuf = (ringbuf *) ((unsigned char *) vstate + sizeof(sumacc_state_t));
  ringbuf_init(n, state->rbuf);

  return GSL_SUCCESS;
}

static int
sumacc_insert(const double x, void * vstate)
{
  sumacc_state_t * state = (sumacc_state_t *) vstate;

  if (ringbuf_is_full(state->rbuf))
    {
      /* subtract oldest element from sum */
      state->sum -= ringbuf_peek_back(state->rbuf);
    }

  /* add new element to sum and ring buffer */
  state->sum += x;
  ringbuf_insert(x, state->rbuf);

  return GSL_SUCCESS;
}

static int
sumacc_delete(void * vstate)
{
  sumacc_state_t * state = (sumacc_state_t *) vstate;

  if (!ringbuf_is_empty(state->rbuf))
    {
      state->sum -= ringbuf_peek_back(state->rbuf);
      ringbuf_pop_back(state->rbuf);
    }

  return GSL_SUCCESS;
}

static int
sumacc_get(void * params, double * result, const void * vstate)
{
  const sumacc_state_t * state = (const sumacc_state_t *) vstate;
  (void) params;
  *result = state->sum;
  return GSL_SUCCESS;
}

static const gsl_movstat_accum sum_accum_type =
{
  sumacc_size,
  sumacc_init,
  sumacc_insert,
  sumacc_delete,
  sumacc_get
};

const gsl_movstat_accum *gsl_movstat_accum_sum = &sum_accum_type;


#ifndef LASER_SCANNER_H_WSZJIY40
#define LASER_SCANNER_H_WSZJIY40

#include <cmath>
#include <gsl/gsl>
#include <sens_loc/camera_models/concepts.h>
#include <sens_loc/math/constants.h>
#include <sens_loc/math/coordinate.h>
#include <sens_loc/math/scaling.h>
#include <stdexcept>
#include <type_traits>

namespace sens_loc::camera_models {

namespace detail {
template <typename Real>
Real get_d_phi(int width) noexcept {
    static_assert(std::is_floating_point_v<Real>);
    // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
    return Real(2.) * math::pi<Real> / Real(width);
}
}  // namespace detail

/// This struct contains the parameters for describing the equirectangular
/// projection of a laser scan.
///
/// The scan is expected to be 360° in horizontal direction and som
/// varying degree in vertical direction.
/// The conversions and projections are mainly conversion from a
/// spherical coordinate-system to a cartesian.
/// \tparam Real precision of the parameters and calculations.
///
/// \note the coordinate convention for spherical coordinates is as follows:
///   - \f$\varphi \in [0, 2 \pi] \mapsto u\f$ note that this wraps around
///   - \f$\theta \in [0, \pi] \mapsto v\f$ - not wrapping!
///   - it is possible to have a different range for \f$\theta\f$ but not for
///     \f$\varphi\f$.
///
/// \tparam Real floating point type that determines the precision of the
/// calculations.
/// \sa is_intrinsic_v
template <typename Real = float>
class equirectangular {
  public:
    static_assert(std::is_floating_point_v<Real>);
    using real_type = Real;

    /// Default initialize all parameters to zero.
    equirectangular() = default;

    /// Construct a equirectangular image for the full sphere.
    ///
    /// \param width width of the image, this maps to 360° field of vie
    /// horizontally which wraps around
    /// \param height height of the image, this
    /// maps to 180° field of view vertically which does __NOT__ wrap
    equirectangular(int width, int height) noexcept
        : _w(width)
        , _h(height)
        , d_phi(detail::get_d_phi<Real>(width))
        , d_theta(math::pi<Real> / Real(height))
        , theta_min(Real(0.)) {
        Expects(width > 0);
        Expects(height > 0);
        ensure_invariant();
    }

    /// Construct the model with a custom \f$\theta\f$-range. This model does
    /// __NOT__ map to the whole sphere, but is not a cylindrical coordinate
    /// system as well!
    /// \param width,height image dimensions
    /// \param theta_range minimum and maximum angle on the unit-sphere in
    /// vertical direction.
    equirectangular(int                       width,
                    int                       height,
                    math::numeric_range<Real> theta_range) noexcept
        : _w(width)
        , _h(height)
        , d_phi(detail::get_d_phi<Real>(width))
        , d_theta((theta_range.max - theta_range.min) / Real(height))
        , theta_min(theta_range.min) {
        Expects(width > 0);
        Expects(height > 0);
        Expects(theta_range.min >= 0.);
        Expects(theta_range.max <= math::pi<Real>);
        ensure_invariant();
    }

    /// Construct the model with a minimum angle \p theta_min and an angle
    /// increment \p d_theta.
    ///
    /// \param width,height image dimensions
    /// \param theta_min,d_theta vertical resolution configuration
    /// \throws if the \f$\theta\f$-angle would be out of the range with the
    /// configuration this constructor throws an exception.
    equirectangular(int width, int height, Real theta_min, Real d_theta)
        : _w(width)
        , _h(height)
        , d_phi(detail::get_d_phi<Real>(width))
        , d_theta(d_theta)
        , theta_min(theta_min) {
        const Real theta_max = theta_min + height * d_theta;
        if (theta_max > math::pi<Real>)
            throw std::invalid_argument("angle increment too big");
        ensure_invariant();
    }

    /// Return the width of the image corresponding to this intrinsic.
    [[nodiscard]] int w() const noexcept { return _w; }
    /// Return the height of the image corresponding to this intrinsic.
    [[nodiscard]] int h() const noexcept { return _h; }

    /// This methods calculates the inverse projection of the equirectangular
    /// model to get the direction of the lightray for the pixel at \p p.
    ///
    /// \tparam _Real either integer pixels or floating point for subpixels
    /// \param p non-negative pixel coordinates
    /// \post \f$\lVert result \rVert_2 = 1.\f$
    /// \returns normalized vector in camera coordinates - unit sphere
    /// coordinate
    /// \note if \p _Real is an integer-type the value is itself backprojected,
    /// which usually means the bottom left corner of the pixel and __NOT__
    /// its center!
    template <typename _Real = int>
    [[nodiscard]] math::sphere_coord<Real>
    pixel_to_sphere(const math::pixel_coord<_Real>& p) const noexcept;

    /// Project points in camera coordinates to pixel coordinates.
    /// \note if the point can not be projected (coordinate not in view) the
    /// pixel coordinate {-1, -1} is returned.
    /// \sa equirectangular::project_to_sphere
    template <typename _Real = Real>
    [[nodiscard]] math::pixel_coord<_Real>
    camera_to_pixel(const math::camera_coord<Real>& p) const noexcept;

  private:
    void ensure_invariant() const noexcept {
        Ensures(d_phi > Real(0.));
        Ensures(d_theta > Real(0.));
        Ensures(theta_min >= Real(0.));

        // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
        Ensures(std::abs(d_phi * _w - Real(2.) * math::pi<Real>) < 0.00001);
        // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
        Ensures(theta_min + _h * d_theta <= math::pi<Real> + 0.00001);
    }

    int  _w        = 0;   ///< width of a laser-scan image
    int  _h        = 0;   ///< height of a laser-scan image.
    Real d_phi     = 0.;  ///< Angle increment in u-direction.
    Real d_theta   = 0.;  ///< Angle increment in v-direction.
    Real theta_min = 0.;  ///< Smallest angle in v-direction.
};

template <typename Real>
template <typename _Real>
inline math::sphere_coord<Real>
equirectangular<Real>::pixel_to_sphere(const math::pixel_coord<_Real>& p) const
    noexcept {
    static_assert(std::is_arithmetic_v<_Real>);
    Expects(p.u() >= Real(0.0));
    Expects(p.v() < Real(w()));
    Expects(p.u() >= Real(0.0));
    Expects(p.v() < Real(h()));

    const Real phi   = p.u() * d_phi - math::pi<Real>;
    const Real theta = theta_min + (p.v() * d_theta);

    Ensures(phi >= -math::pi<Real>);
    Ensures(phi <= math::pi<Real>);
    Ensures(theta >= Real(0.));
    Ensures(theta <= math::pi<Real>);

    using std::cos;
    using std::sin;
    math::sphere_coord<Real> s{sin(theta) * cos(phi), sin(theta) * sin(phi),
                               cos(theta)};
    // NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
    Ensures(std::abs(s.norm() - Real(1.0)) < 0.000001);
    return s;
}

template <typename Real>
template <typename _Real>
inline math::pixel_coord<_Real>
equirectangular<Real>::camera_to_pixel(const math::camera_coord<Real>& p) const
    noexcept {
    static_assert(std::is_arithmetic_v<_Real>);

    const Real r = std::sqrt(p.X() * p.X() + p.Y() * p.Y() + p.Z() * p.Z());
    if (r == Real(0.0))
        return {_Real(-1), _Real(-1)};

    const Real theta = std::acos(p.Z() / r);
    const Real phi   = std::atan2(p.Y(), p.X());

    Ensures(r >= Real(0.0));
    Ensures(phi >= -math::pi<Real>);
    Ensures(phi <= math::pi<Real>);
    Ensures(theta >= Real(0.));
    Ensures(theta <= math::pi<Real>);

    const _Real u = gsl::narrow_cast<_Real>((phi + math::pi<Real>) / d_phi);
    const _Real v = gsl::narrow_cast<_Real>(theta / d_theta);

    if (u < _Real(0.0) || u > gsl::narrow_cast<Real>(w()) || v < _Real(0.0) ||
        v > gsl::narrow_cast<Real>(h()))
        return {_Real(-1), _Real(-1)};

    return {u, v};
}
}  // namespace sens_loc::camera_models

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


// ************************************************
// rand_helpers.h
// authors: Lee Howes and David B. Thomas
//
// Contains support code for the random number
// generation necessary for initialising the
// cuda simulations correctly.
//
// Ziggurat code taken from Marsaglia's
// paper.
// ************************************************

#ifndef __rand_helpers_h
#define __rand_helpers_h

// RNG choices
#define USE_ZIGGURAT 0
#define USE_GSL 0

#include <math.h>
#include <assert.h>

#if USE_GSL
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#endif

unsigned Kiss()
{
	static unsigned z=362436069, w=521288629, jsr=123456789, jcong=380116160;

	z=36969*(z&65535)+(z>>16);
	w=18000*(w&65535)+(w>>16);
	unsigned mwc=(z<<16)+w;
	jsr^=(jsr<<17);
	jsr^=(jsr>>13);
	jsr^=(jsr<<5);
	jcong=69069*jcong+1234567;
	return (mwc^jcong)+jsr;
}

double Rand()
{
	unsigned long long x=Kiss();
	x=(x<<32)|Kiss();
	return x*5.4210108624275221703311375920553e-20;
}

#if USE_GSL
const gsl_rng_type **t, **t0;
gsl_rng *rng;
bool initialised = false;

void initRand()
{
    gsl_rng_env_setup();

    t0 = gsl_rng_types_setup ();

    printf ("Available generators:\n");

    for (t = t0; *t != 0; t++)
    {
        printf ("%s\n", (*t)->name);
        if( strcmp("mt19937_1999", (*t)->name) == 0 ) break;
    }

    rng = gsl_rng_alloc (*t);
}

double RandN()
{
    return gsl_ran_gaussian_ziggurat  (rng, 1.0);

}
#else

#if USE_ZIGGURAT

/* Period parameters */
#define CPU_MT_N 624
#define CPU_MT_M 397
#define MATRIX_A 0x9908b0dfUL   /* constant vector a */
#define CPU_MT_UPPER_MASK 0x80000000UL /* most significant w-r bits */
#define CPU_MT_LOWER_MASK 0x7fffffffUL /* least significant r bits */

// MT
unsigned long init[4]={0x123, 0x234, 0x345, 0x456}, length=4;

static unsigned long mt[CPU_MT_N]; /* the array for the state vector  */
static int mti=CPU_MT_N+1; /* mti==N+1 means mt[N] is not initialized */

/* initializes mt[N] with a seed */
void init_genrand(unsigned long s)
{
    mt[0]= s & 0xffffffffUL;
    for (mti=1; mti<CPU_MT_N; mti++) {
        mt[mti] =
	    (1812433253UL * (mt[mti-1] ^ (mt[mti-1] >> 30)) + mti);
        /* See Knuth TAOCP Vol2. 3rd Ed. P.106 for multiplier. */
        /* In the previous versions, MSBs of the seed affect   */
        /* only MSBs of the array mt[].                        */
        /* 2002/01/09 modified by Makoto Matsumoto             */
        mt[mti] &= 0xffffffffUL;
        /* for >32 bit machines */
    }
}

/* initialize by an array with array-length */
/* init_key is the array for initializing keys */
/* key_length is its length */
/* slight change for C++, 2004/2/26 */
void init_by_array(unsigned long init_key[], int key_length)
{
    int i, j, k;
    init_genrand(19650218UL);
    i=1; j=0;
    k = (CPU_MT_N>key_length ? CPU_MT_N : key_length);
    for (; k; k--) {
        mt[i] = (mt[i] ^ ((mt[i-1] ^ (mt[i-1] >> 30)) * 1664525UL))
          + init_key[j] + j; /* non linear */
        mt[i] &= 0xffffffffUL; /* for WORDSIZE > 32 machines */
        i++; j++;
        if (i>=CPU_MT_N) { mt[0] = mt[CPU_MT_N-1]; i=1; }
        if (j>=key_length) j=0;
    }
    for (k=CPU_MT_N-1; k; k--) {
        mt[i] = (mt[i] ^ ((mt[i-1] ^ (mt[i-1] >> 30)) * 1566083941UL))
          - i; /* non linear */
        mt[i] &= 0xffffffffUL; /* for WORDSIZE > 32 machines */
        i++;
        if (i>=CPU_MT_N) { mt[0] = mt[CPU_MT_N-1]; i=1; }
    }

    mt[0] = 0x80000000UL; /* MSB is 1; assuring non-zero initial array */
}

/* generates a random number on [0,0xffffffff]-interval */
unsigned long genrand_int32(void)
{
    unsigned long y;
    static unsigned long mag01[2]={0x0UL, MATRIX_A};
    /* mag01[x] = x * MATRIX_A  for x=0,1 */

    if (mti >= CPU_MT_N) { /* generate N words at one time */
        int kk;

        if (mti == CPU_MT_N+1)   /* if init_genrand() has not been called, */
            init_genrand(5489UL); /* a default initial seed is used */

        for (kk=0;kk<CPU_MT_N-CPU_MT_M;kk++) {
            y = (mt[kk]&CPU_MT_UPPER_MASK)|(mt[kk+1]&CPU_MT_LOWER_MASK);
            mt[kk] = mt[kk+CPU_MT_M] ^ (y >> 1) ^ mag01[y & 0x1UL];
        }
        for (;kk<CPU_MT_N-1;kk++) {
            y = (mt[kk]&CPU_MT_UPPER_MASK)|(mt[kk+1]&CPU_MT_LOWER_MASK);
            mt[kk] = mt[kk+(CPU_MT_M-CPU_MT_N)] ^ (y >> 1) ^ mag01[y & 0x1UL];
        }
        y = (mt[CPU_MT_N-1]&CPU_MT_UPPER_MASK)|(mt[0]&CPU_MT_LOWER_MASK);
        mt[CPU_MT_N-1] = mt[CPU_MT_M-1] ^ (y >> 1) ^ mag01[y & 0x1UL];

        mti = 0;
    }

    y = mt[mti++];

    /* Tempering */
    y ^= (y >> 11);
    y ^= (y << 7) & 0x9d2c5680UL;
    y ^= (y << 15) & 0xefc60000UL;
    y ^= (y >> 18);

    return y;
}

double genrand_real2(void)
{
    return genrand_int32()*(1.0/4294967296.0);
    /* divided by 2^32 */
}

double genrand_real3(void)
{
    return (((double)genrand_int32()) + 0.5)*(1.0/4294967296.0);
    /* divided by 2^32 */
}

// ZIGGURAT
#define abs(X) abs((int)X)
//#define SHR3 (jz=jsr, jsr^=(jsr<<13), jsr^=(jsr>>17), jsr^=(jsr<<5),jz+jsr)
#define SHR3 genrand_int32()
//#define UNI (.5 + (signed) SHR3 * .2328306e-9)
#define UNI genrand_real3()
#define RandN() (hz=SHR3, iz=hz&127, (abs(hz)<kn[iz])? hz*wn[iz] : nfix())

static unsigned long iz,jz,jsr=123456789,kn[128],ke[256];
static long hz; static float wn[128],fn[128], we[256],fe[256];

float nfix(void) { /*provides RNOR if #define cannot */
    const float r = 3.442620f; static float x, y;
    for(;;){ x=hz*wn[iz];
        if(iz==0){
            do{
                x=-log(UNI)*0.2904764;
                y=-log(UNI);
            } while(y+y<x*x);
            return (hz>0)? r+x : -r-x;
        }
        if( fn[iz]+UNI*(fn[iz-1]-fn[iz]) < exp(-.5*x*x) ) return x;
        hz=SHR3; iz=hz&127;if(abs(hz)<kn[iz]) return (hz*wn[iz]);
    }
}

/*--------This procedure sets the seed and creates the tables------*/
void initRand() {
    unsigned long jsrseed = 123456789;
    const double m1 = 2147483648.0, m2 = 4294967296.;
    double dn=3.442619855899,tn=dn,vn=9.91256303526217e-3, q;
    double de=7.697117470131487, te=de, ve=3.949659822581572e-3;
    int i; jsr=jsrseed;

    /* Tables for RNOR: */ q=vn/exp(-.5*dn*dn);
    kn[0]=(dn/q)*m1; kn[1]=0;
    wn[0]=q/m1; wn[127]=dn/m1;
    fn[0]=1.; fn[127]=exp(-.5*dn*dn);
    for(i=126;i>=1;i--) {
        dn=sqrt(-2.*log(vn/dn+exp(-.5*dn*dn)));
        kn[i+1]=(dn/tn)*m1; tn=dn;
        fn[i]=exp(-.5*dn*dn); wn[i]=dn/m1;
    }

    /* Tables for REXP */ q = ve/exp(-de);
    ke[0]=(de/q)*m2; ke[1]=0;
    we[0]=q/m2; we[255]=de/m2;
    fe[0]=1.; fe[255]=exp(-de);
    for(i=254;i>=1;i--) {
        de=-log(ve/de+exp(-de));
        ke[i+1]= (de/te)*m2; te=de;
        fe[i]=exp(-de); we[i]=de/m2;
    }

    init_by_array(init, length);
}

#else

void initRand()
{

}

double RandN()
{
	static bool cached=false;
	static double cn;

	if(cached){
		cached=false;
		return cn;
	}

	double a=std::sqrt(-2*std::log(Rand()));
	double b=6.283185307179586476925286766559*Rand();
	cn=std::sin(b)*a;
	cached=true;
	return std::cos(b)*a;
}
#endif // USE_ZIGGURAT
#endif // USE_GSL

double MakeChi2Scale(unsigned N)
{
	const double chic1 = std::sqrt ( std::sqrt (1.0 - 1.0 / N));
	const double chic2 = std::sqrt (1.0 - chic1 * chic1);
	return chic1+chic2*RandN();
}

#endif


// The top most header - contains switches and constants
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <gsl/gsl_rng.h>

#define Pi   acos(-1.0)

#define real double

// Cell shape and size
//#define circle
#define square
#define Dia 40  // Cell radius or square side
#define thickness 40

// clamped or hinged MTs

int iprint, iter, t;
char datadir[64] ;