#include "../../def_submodule.hpp"

#include "../../../../../type/math/coord.hpp"

using namespace ::math::linear::vector;

#include <boost/python.hpp>

typedef GS_DDMRM::S_IceRay::S_type::GT_int               GTs_int;
typedef GS_DDMRM::S_IceRay::S_type::GT_size               GTs_size;
typedef GS_DDMRM::S_IceRay::S_type::GT_scalar             GTs_scalar;
//typedef GS_DDMRM::S_IceRay::S_type::S_color::GT_scalar    GTs_color;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_int2D    GTs_cell2D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_int3D    GTs_cell3D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_int4D    GTs_cell4D;

typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_size2D    GTs_size2D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_size3D    GTs_size3D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_size4D    GTs_size4D;

typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar2D  GTs_coord2D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar3D  GTs_coord3D;
typedef GS_DDMRM::S_IceRay::S_type::S_coord::GT_scalar4D  GTs_coord4D;

namespace
 {

  template < typename N_number, unsigned N_dimension>
   N_number get( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, N_dimension>::T_typedef const&v, int P_index )
    {
     if( P_index >= 0 && P_index < v.size() )
      {
       return v.at( P_index );
      }

     PyErr_SetString( PyExc_IndexError, "index out of range" );
     return 0;
    }

   template < typename N_scalar, typename N_number, unsigned N_dimension>
    void set( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_scalar, N_dimension>::T_typedef & v, int P_index, N_number value )
     {
      if(P_index >= 0 && P_index < v.size())
       {
        v.at( P_index ) = value;
        return;
       }

       PyErr_SetString(PyExc_IndexError, "index out of range" );
       return;
     }

   template < typename N_number >
    typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, 2 >::T_typedef &
    load2( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 2 >::T_typedef & v, N_number x, N_number y )
     {
      v[0] = x;
      v[1] = y;
      return v;
     }
   template < typename N_number >
    typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, 3 >::T_typedef &
    load3( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 3 >::T_typedef & v, N_number x, N_number y, N_number z )
     {
      v[0] = x;
      v[1] = y;
      v[2] = z;
      return v;
     }

   template < typename N_number >
    typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, 4 >::T_typedef &
    load4( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 4 >::T_typedef & v, N_number x, N_number y, N_number z, N_number t )
     {
      v[0] = x;
      v[1] = y;
      v[2] = z;
      v[3] = t;
      return v;
     }

   template < typename N_number, unsigned dimension_number >
    typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, dimension_number >::T_typedef &
    fill( typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, dimension_number >::T_typedef & v, N_number x )
     {
      ::math::linear::vector::fill( v, x );
      return v;
     }

    template < typename N_number >
     typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model< N_number, 3 >::T_typedef
      GFs_cross
       (
         typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 3 >::T_typedef const& P_left
        ,typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 3 >::T_typedef const& P_right
       )
       {
        typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, 3 >::T_typedef Ir_right;
        ::math::linear::vector::cross( Ir_right, P_left, P_right );
        return Ir_right;
       }

    template < typename N_number, unsigned N_dimension >
     N_number
     GFs_dot
      (
        typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, N_dimension >::T_typedef const& P_left
       ,typename GS_DDMRM::S_IceRay::S_type::S_coord::GC__model<N_number, N_dimension >::T_typedef const& P_right
      )
      {
       return  ::math::linear::vector::dot( P_left, P_right );
      }

  }

void expose_math_type_coord_cell()
 {
//MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  boost::python::class_<GTs_cell2D>( "MathTypeCell2D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_int>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_int,GTs_int>( TODO ) )
    .def( boost::python::init<GTs_cell2D>() )

    .def("__getitem__", &get<GTs_int, 2 > )
    .def("__setitem__", &set<GTs_int, GTs_int, 2 > )
    .def("__setitem__", &set<GTs_int, GTs_scalar, 2 > )

    .def( "load",       &load2<GTs_size   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load2<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_size,2>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_size() )
    .def( GTs_size() * boost::python::self  )
    .def( boost::python::self / GTs_int() )
    .def( boost::python::self /= GTs_int() )
    .def( boost::python::self *= GTs_int() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::class_<GTs_cell3D>( "MathTypeCell3D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_size>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_size,GTs_size,GTs_size>( TODO ) )
   .def( boost::python::init<GTs_cell3D>() )

    .def("__getitem__", &get<GTs_int, 3 > )
    .def("__setitem__", &set<GTs_int, GTs_int, 3 > )
    .def("__setitem__", &set<GTs_int, GTs_scalar, 3 > )

    .def( "load",       &load3<GTs_int   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load3<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_int,3>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_int() )
    .def( boost::python::self /= GTs_int() )
    .def( boost::python::self *= GTs_int() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::class_<GTs_cell4D>( "MathTypeCell4D" )
    .def( boost::python::init<>() )
    //.def( boost::python::init<GTs_int>() )
    //.def( boost::python::init<GTs_scalar>() )
    //.def( boost::python::init<GTs_int,GTs_int,GTs_int,GTs_int>( TODO ) )
    .def( boost::python::init<GTs_cell4D>() )

    .def("__getitem__", &get<GTs_int, 4 > )
    .def("__setitem__", &set<GTs_int, GTs_int, 4 > )
    .def("__setitem__", &set<GTs_int, GTs_scalar, 4 > )

    .def( "load",       &load4<GTs_int    >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load4<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_int,   4>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_int() )
    .def( boost::python::self /= GTs_int() )
    .def( boost::python::self *= GTs_int() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
    ;
 }

void expose_math_type_coord_size()
 {
//MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  boost::python::class_<GTs_size2D>( "MathTypeSize2D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_size>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_size,GTs_size>( TODO ) )
    .def( boost::python::init<GTs_size2D>() )

    .def("__getitem__", &get<GTs_size, 2 > )
    .def("__setitem__", &set<GTs_size, GTs_size, 2 > )
    .def("__setitem__", &set<GTs_size, GTs_scalar, 2 > )

    .def( "load",       &load2<GTs_size   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load2<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_size,2>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_size() )
    .def( GTs_size() * boost::python::self  )
    .def( boost::python::self / GTs_size() )
    .def( boost::python::self /= GTs_size() )
    .def( boost::python::self *= GTs_size() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::class_<GTs_size3D>( "MathTypeSize3D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_size>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_size,GTs_size,GTs_size>( TODO ) )
   .def( boost::python::init<GTs_size3D>() )

    .def("__getitem__", &get<GTs_size, 3 > )
    .def("__setitem__", &set<GTs_size, GTs_size, 3 > )
    .def("__setitem__", &set<GTs_size, GTs_scalar, 3 > )

    .def( "load",       &load3<GTs_size   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load3<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_size,3>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_size() )
    .def( boost::python::self /= GTs_size() )
    .def( boost::python::self *= GTs_size() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::class_<GTs_size4D>( "MathTypeSize4D" )
    .def( boost::python::init<>() )
    //.def( boost::python::init<GTs_size>() )
    //.def( boost::python::init<GTs_scalar>() )
    //.def( boost::python::init<GTs_size,GTs_size,GTs_size,GTs_size>( TODO ) )
    .def( boost::python::init<GTs_size4D>() )

    .def("__getitem__", &get<GTs_size, 4 > )
    .def("__setitem__", &set<GTs_size, GTs_size, 4 > )
    .def("__setitem__", &set<GTs_size, GTs_scalar, 4 > )

    .def( "load",       &load4<GTs_size   >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load4<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_size,4>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_size() )
    .def( boost::python::self /= GTs_size() )
    .def( boost::python::self *= GTs_size() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
    ;
 }

// TODO: s * v, v * s, v / s, v+v, v-v
void expose_math_type_coord_scalar()
 {
//MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( math   );

  boost::python::class_<GTs_coord2D>( "MathTypeCoord2D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_scalar,GTs_scalar>( TODO ) )
    .def( boost::python::init<GTs_coord2D>() )

    .def("__getitem__", &get<GTs_scalar, 2 > )
    .def("__setitem__", &set<GTs_scalar, GTs_scalar, 2 > )

    .def( "load",        &load2<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",        &fill<GTs_scalar,2>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )

    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )

    .def( boost::python::self / GTs_scalar() )

    .def( boost::python::self /= GTs_scalar() )
    .def( boost::python::self *= GTs_scalar() )

    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
  ;

  boost::python::def("MathCoord2D_dot",  &GFs_dot<GTs_scalar, 2 > );

  boost::python::def("MathLinearVector2DDot",  &GFs_dot<GTs_scalar, 2 > );
  boost::python::def("MathLinearVector3DLength",  &::math::linear::vector::length<GTs_scalar,GTs_scalar, 4 > );

  boost::python::class_<GTs_coord3D>( "MathTypeCoord3D" )
    .def( boost::python::init<>() )
   //.def( boost::python::init<GTs_scalar>() )
   //.def( boost::python::init<GTs_scalar,GTs_scalar,GTs_scalar>( TODO ) )
   .def( boost::python::init<GTs_coord3D>() )

     .def( "__getitem__", &get<GTs_scalar, 3 > )
     .def( "__setitem__", &set<GTs_scalar, GTs_scalar, 3 > )
   //.def("__setitem__", &set<GTs_scalar, GTs_size, 3 > )

    .def( "load",        &load3<GTs_scalar >, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",        &fill<GTs_scalar,3>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )

    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )

    .def( boost::python::self / GTs_scalar() )

    .def( boost::python::self /= GTs_scalar() )
    .def( boost::python::self *= GTs_scalar() )

    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
   ;

  boost::python::def("MathCoord3D_dot",    &GFs_dot<GTs_scalar, 3 > );
  boost::python::def("MathCoord3D_cross",  &GFs_cross<GTs_scalar > );

  boost::python::def("MathLinearVector3DDot",  &GFs_dot<GTs_scalar, 2 > );
  boost::python::def("MathLinearVector3DLength",  &::math::linear::vector::length<GTs_scalar,GTs_scalar, 3 > );

  boost::python::class_<GTs_coord4D>( "MathTypeCoord4D" )
    .def( boost::python::init<>() )
  //.def( boost::python::init<GTs_scalar>() )
  //.def( boost::python::init<GTs_scalar,GTs_scalar,GTs_scalar,GTs_scalar>() )
   .def( boost::python::init<GTs_coord4D>() )

   .def("__getitem__", &get<GTs_scalar, 4 > )
   .def("__setitem__", &set<GTs_scalar, GTs_scalar, 4 > )

    .def( "load",       &load4<GTs_scalar>, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",       &load4<GTs_size>, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",       &fill<GTs_scalar,4>, boost::python::return_value_policy<boost::python::return_by_value>() )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )
    .def( boost::python::self * GTs_scalar() )
    .def( GTs_scalar() * boost::python::self  )
    .def( boost::python::self / GTs_scalar() )
    .def( boost::python::self /= GTs_scalar() )
    .def( boost::python::self *= GTs_scalar() )
    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
   ;

  boost::python::def("MathCoord4D_dot",  &GFs_dot<GTs_scalar, 4 > );

  boost::python::def("MathLinearVector4DDot",  &GFs_dot<GTs_scalar, 2 > );
  boost::python::def("MathLinearVector4DLength",  &::math::linear::vector::length<GTs_scalar,GTs_scalar, 4 > );

 }

void expose_math_type_coord_coord()
 {
  expose_math_type_coord_cell();
  expose_math_type_coord_size();
  expose_math_type_coord_scalar();
 }


/**
 All of this code is written by Aman Agrawal
 (Indian Institute of Technology, Delhi)
*/

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <Eigen/QR>
#include "time.h"

#include "genotype.h"
#include "mailman.h"
#include "arguments.h"
#include "helper.h"
#include "storage.h"

#if SSE_SUPPORT==1
	#define fastmultiply fastmultiply_sse
	#define fastmultiply_pre fastmultiply_pre_sse
#else
	#define fastmultiply fastmultiply_normal
	#define fastmultiply_pre fastmultiply_pre_normal
#endif

using namespace Eigen;
using namespace std;

// Storing in RowMajor Form
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixXdr;

//Intermediate Variables
int blocksize;
double *partialsums;
double *sum_op;
double *yint_e;
double *yint_m;
double **y_e;
double **y_m;

struct timespec t0;

clock_t total_begin = clock();

genotype g;
MatrixXdr geno_matrix; //(p,n)

int MAX_ITER;
int k,p,n;
int k_orig;

MatrixXdr c; //(p,k)
MatrixXdr x; //(k,n)
MatrixXdr v; //(p,k)
MatrixXdr means; //(p,1)
MatrixXdr stds; //(p,1)

options command_line_opts;

bool debug = false;
bool check_accuracy = false;
bool var_normalize=false;
int accelerated_em=0;
double convergence_limit;
bool memory_efficient = false;
bool missing=false;
bool fast_mode = true;
bool text_version = false;

void multiply_y_pre_fast(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){

	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
		sum_op[k_iter]=op.col(k_iter).sum();
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on premultiply"<<endl;
			cout << "Nops = " << Ncol_op << "\t" <<g.Nsegments_hori << endl;
			cout << "Segment size = " << g.segment_size_hori << endl;
			cout << "Matrix size = " <<g.segment_size_hori<<"\t" <<g.Nindv << endl;
			cout << "op = " <<  op.rows () << "\t" << op.cols () << endl;
		}
	#endif

	//TODO: Memory Effecient SSE FastMultipy

	for(int seg_iter=0;seg_iter<g.Nsegments_hori-1;seg_iter++){
		mailman::fastmultiply(g.segment_size_hori,g.Nindv,Ncol_op,g.p[seg_iter],op,yint_m,partialsums,y_m);
		int p_base = seg_iter*g.segment_size_hori;
		for(int p_iter=p_base; (p_iter<p_base+g.segment_size_hori) && (p_iter<g.Nsnp) ; p_iter++ ){
			for(int k_iter=0;k_iter<Ncol_op;k_iter++)
				res(p_iter,k_iter) = y_m[p_iter-p_base][k_iter];
		}
	}

	int last_seg_size = (g.Nsnp%g.segment_size_hori !=0 ) ? g.Nsnp%g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply(last_seg_size,g.Nindv,Ncol_op,g.p[g.Nsegments_hori-1],op,yint_m,partialsums,y_m);
	int p_base = (g.Nsegments_hori-1)*g.segment_size_hori;
	for(int p_iter=p_base; (p_iter<p_base+g.segment_size_hori) && (p_iter<g.Nsnp) ; p_iter++){
		for(int k_iter=0;k_iter<Ncol_op;k_iter++)
			res(p_iter,k_iter) = y_m[p_iter-p_base][k_iter];
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on premultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	for(int p_iter=0;p_iter<p;p_iter++){
 		for(int k_iter=0;k_iter<Ncol_op;k_iter++){
			res(p_iter,k_iter) = res(p_iter,k_iter) - (g.get_col_mean(p_iter)*sum_op[k_iter]);
			if(var_normalize)
				res(p_iter,k_iter) = res(p_iter,k_iter)/(g.get_col_std(p_iter));
 		}
 	}

}

void multiply_y_post_fast(MatrixXdr &op_orig, int Nrows_op, MatrixXdr &res,bool subtract_means){

	MatrixXdr op;
	op = op_orig.transpose();

	if(var_normalize && subtract_means){
		for(int p_iter=0;p_iter<p;p_iter++){
			for(int k_iter=0;k_iter<Nrows_op;k_iter++)
				op(p_iter,k_iter) = op(p_iter,k_iter) / (g.get_col_std(p_iter));
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Starting mailman on postmultiply"<<endl;
		}
	#endif

	int Ncol_op = Nrows_op;

	int seg_iter;
	for(seg_iter=0;seg_iter<g.Nsegments_hori-1;seg_iter++){
		mailman::fastmultiply_pre(g.segment_size_hori,g.Nindv,Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter],op,yint_e,partialsums,y_e);
	}
	int last_seg_size = (g.Nsnp%g.segment_size_hori !=0 ) ? g.Nsnp%g.segment_size_hori : g.segment_size_hori;
	mailman::fastmultiply_pre(last_seg_size,g.Nindv,Ncol_op, seg_iter * g.segment_size_hori, g.p[seg_iter],op,yint_e,partialsums,y_e);

	for(int n_iter=0; n_iter<n; n_iter++)  {
		for(int k_iter=0;k_iter<Ncol_op;k_iter++) {
			res(k_iter,n_iter) = y_e[n_iter][k_iter];
			y_e[n_iter][k_iter] = 0;
		}
	}

	#if DEBUG==1
		if(debug){
			print_time ();
			cout <<"Ending mailman on postmultiply"<<endl;
		}
	#endif

	if(!subtract_means)
		return;

	double *sums_elements = new double[Ncol_op];
 	memset (sums_elements, 0, Nrows_op * sizeof(int));

 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		double sum_to_calc=0.0;
 		for(int p_iter=0;p_iter<p;p_iter++)
 			sum_to_calc += g.get_col_mean(p_iter)*op(p_iter,k_iter);
 		sums_elements[k_iter] = sum_to_calc;
 	}
 	for(int k_iter=0;k_iter<Ncol_op;k_iter++){
 		for(int n_iter=0;n_iter<n;n_iter++)
 			res(k_iter,n_iter) = res(k_iter,n_iter) - sums_elements[k_iter];
 	}

}

void multiply_y_pre_naive_mem(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	for(int p_iter=0;p_iter<p;p_iter++){
		for(int k_iter=0;k_iter<Ncol_op;k_iter++){
			double temp=0;
			for(int n_iter=0;n_iter<n;n_iter++)
				temp+= g.get_geno(p_iter,n_iter,var_normalize)*op(n_iter,k_iter);
			res(p_iter,k_iter)=temp;
		}
	}
}

void multiply_y_post_naive_mem(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	for(int n_iter=0;n_iter<n;n_iter++){
		for(int k_iter=0;k_iter<Nrows_op;k_iter++){
			double temp=0;
			for(int p_iter=0;p_iter<p;p_iter++)
				temp+= op(k_iter,p_iter)*(g.get_geno(p_iter,n_iter,var_normalize));
			res(k_iter,n_iter)=temp;
		}
	}
}

void multiply_y_pre_naive(MatrixXdr &op, int Ncol_op ,MatrixXdr &res){
	res = geno_matrix * op;
}

void multiply_y_post_naive(MatrixXdr &op, int Nrows_op ,MatrixXdr &res){
	res = op * geno_matrix;
}

void multiply_y_post(MatrixXdr &op, int Nrows_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_post_fast(op,Nrows_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_post_naive_mem(op,Nrows_op,res);
		else
			multiply_y_post_naive(op,Nrows_op,res);
	}
}

void multiply_y_pre(MatrixXdr &op, int Ncol_op ,MatrixXdr &res,bool subtract_means){
    if(fast_mode)
        multiply_y_pre_fast(op,Ncol_op,res,subtract_means);
    else{
		if(memory_efficient)
			multiply_y_pre_naive_mem(op,Ncol_op,res);
		else
			multiply_y_pre_naive(op,Ncol_op,res);
	}
}

pair<double,double> get_error_norm(MatrixXdr &c){
	HouseholderQR<MatrixXdr> qr(c);
	MatrixXdr Q;
	Q = qr.householderQ() * MatrixXdr::Identity(p,k);
	MatrixXdr q_t(k,p);
	q_t = Q.transpose();
	MatrixXdr b(k,n);
	multiply_y_post(q_t,k,b,true);
	JacobiSVD<MatrixXdr> b_svd(b, ComputeThinU | ComputeThinV);
	MatrixXdr u_l,d_l,v_l;
	if(fast_mode)
        u_l = b_svd.matrixU();
    else
        u_l = Q * b_svd.matrixU();
	v_l = b_svd.matrixV();
	d_l = MatrixXdr::Zero(k,k);
	for(int kk=0;kk<k; kk++)
		d_l(kk,kk) = (b_svd.singularValues())(kk);

	MatrixXdr u_k,v_k,d_k;
	u_k = u_l.leftCols(k_orig);
	v_k = v_l.leftCols(k_orig);
	d_k = MatrixXdr::Zero(k_orig,k_orig);
	for(int kk =0 ; kk < k_orig ; kk++)
		d_k(kk,kk)  =(b_svd.singularValues())(kk);

	MatrixXdr b_l,b_k;
    b_l = u_l * d_l * (v_l.transpose());
    b_k = u_k * d_k * (v_k.transpose());

    if(fast_mode){
        double temp_k = b_k.cwiseProduct(b).sum();
        double temp_l = b_l.cwiseProduct(b).sum();
        double b_knorm = b_k.norm();
        double b_lnorm = b_l.norm();
        double norm_k = (b_knorm*b_knorm) - (2*temp_k);
        double norm_l = (b_lnorm*b_lnorm) - (2*temp_l);
        return make_pair(norm_k,norm_l);
    }
    else{
        MatrixXdr e_l(p,n);
        MatrixXdr e_k(p,n);
        for(int p_iter=0;p_iter<p;p_iter++){
            for(int n_iter=0;n_iter<n;n_iter++){
                e_l(p_iter,n_iter) = g.get_geno(p_iter,n_iter,var_normalize) - b_l(p_iter,n_iter);
                e_k(p_iter,n_iter) = g.get_geno(p_iter,n_iter,var_normalize) - b_k(p_iter,n_iter);
            }
        }

        double ek_norm = e_k.norm();
        double el_norm = e_l.norm();
        return make_pair(ek_norm,el_norm);
    }
}

MatrixXdr run_EM_not_missing(MatrixXdr &c_orig){

	#if DEBUG==1
		if(debug){
			print_time ();
			cout << "Enter: run_EM_not_missing" << endl;
		}
	#endif

	MatrixXdr c_temp(k,p);
	MatrixXdr c_new(p,k);
	c_temp = ( (c_orig.transpose()*c_orig).inverse() ) * (c_orig.transpose());

	#if DEBUG==1
		if(debug){
			print_timenl ();
		}
	#endif

	MatrixXdr x_fn(k,n);
	multiply_y_post(c_temp,k,x_fn,true);

	#if DEBUG==1
		if(debug){
			print_timenl ();
		}
	#endif

	MatrixXdr x_temp(n,k);
	x_temp = (x_fn.transpose()) * ((x_fn*(x_fn.transpose())).inverse());
	multiply_y_pre(x_temp,k,c_new,true);

	#if DEBUG==1
		if(debug){
			print_time ();
			cout << "Exiting: run_EM_not_missing" << endl;
		}
	#endif

	return c_new;
}

MatrixXdr run_EM_missing(MatrixXdr &c_orig){

	MatrixXdr c_new(p,k);

	MatrixXdr mu(k,n);

	// E step
	MatrixXdr c_temp(k,k);
	c_temp = c_orig.transpose() * c_orig;

	MatrixXdr T(k,n);
	MatrixXdr c_fn;
	c_fn = c_orig.transpose();
	multiply_y_post(c_fn,k,T,false);

	MatrixXdr M_temp(k,1);
	M_temp = c_orig.transpose() *  means;

	for(int j=0;j<n;j++){
		MatrixXdr D(k,k);
		MatrixXdr M_to_remove(k,1);
		D = MatrixXdr::Zero(k,k);
		M_to_remove = MatrixXdr::Zero(k,1);
		for(int i=0;i<g.not_O_j[j].size();i++){
			int idx = g.not_O_j[j][i];
			D = D + (c_orig.row(idx).transpose() * c_orig.row(idx));
			M_to_remove = M_to_remove + (c_orig.row(idx).transpose()*g.get_col_mean(idx));
		}
		mu.col(j) = (c_temp-D).inverse() * ( T.col(j) - M_temp + M_to_remove);
	}

	// M step

	MatrixXdr mu_temp(k,k);
	mu_temp = mu * mu.transpose();
	MatrixXdr T1(p,k);
	MatrixXdr mu_fn;
	mu_fn = mu.transpose();
	multiply_y_pre(mu_fn,k,T1,false);
	MatrixXdr mu_sum(k,1);
	mu_sum = MatrixXdr::Zero(k,1);
	mu_sum = mu.rowwise().sum();

	for(int i=0;i<p;i++){
		MatrixXdr D(k,k);
		MatrixXdr mu_to_remove(k,1);
		D = MatrixXdr::Zero(k,k);
		mu_to_remove = MatrixXdr::Zero(k,1);
		for(int j=0;j<g.not_O_i[i].size();j++){
			int idx = g.not_O_i[i][j];
			D = D + (mu.col(idx) * mu.col(idx).transpose());
			mu_to_remove = mu_to_remove + (mu.col(idx));
		}
		c_new.row(i) = (((mu_temp-D).inverse()) * (T1.row(i).transpose() -  ( g.get_col_mean(i) * (mu_sum-mu_to_remove)))).transpose();
		double mean;
		mean = g.get_col_sum(i);
		mean = mean -  (c_orig.row(i)*(mu_sum-mu_to_remove))(0,0);
		mean = mean * 1.0 / (n-g.not_O_i[i].size());
		g.update_col_mean(i,mean);
	}
	return c_new;
}

MatrixXdr run_EM(MatrixXdr &c_orig){

	if(missing)
		return run_EM_missing(c_orig);
	else
		return run_EM_not_missing(c_orig);
}

void print_vals(){

	HouseholderQR<MatrixXdr> qr(c);
	MatrixXdr Q;
	Q = qr.householderQ() * MatrixXdr::Identity(p,k);
	MatrixXdr q_t(k,p);
	q_t = Q.transpose();
	MatrixXdr b(k,n);
	multiply_y_post(q_t,k,b,true);
	JacobiSVD<MatrixXdr> b_svd(b, ComputeThinU | ComputeThinV);
	MatrixXdr u_l;
	u_l = b_svd.matrixU();
	MatrixXdr v_l;
	v_l = b_svd.matrixV();
	MatrixXdr u_k;
	MatrixXdr v_k,d_k;
	u_k = u_l.leftCols(k_orig);
	v_k = v_l.leftCols(k_orig);

	ofstream evec_file;
	evec_file.open((string(command_line_opts.OUTPUT_PATH)+string("evecs.txt")).c_str());
	evec_file<< std::setprecision(15) << Q*u_k << endl;
	evec_file.close();
	ofstream eval_file;
	eval_file.open((string(command_line_opts.OUTPUT_PATH)+string("evals.txt")).c_str());
	for(int kk =0 ; kk < k_orig ; kk++)
		eval_file << std::setprecision(15)<< (b_svd.singularValues())(kk)<<endl;
	eval_file.close();

	ofstream proj_file;
	proj_file.open((string(command_line_opts.OUTPUT_PATH) + string("projections.txt")).c_str());
	proj_file << std::setprecision(15)<< v_k<<endl;
	proj_file.close();
	if(debug){
		ofstream c_file;
		c_file.open((string(command_line_opts.OUTPUT_PATH)+string("cvals.txt")).c_str());
		c_file<<c<<endl;
		c_file.close();

		d_k = MatrixXdr::Zero(k_orig,k_orig);
		for(int kk =0 ; kk < k_orig ; kk++)
			d_k(kk,kk)  =(b_svd.singularValues())(kk);
		MatrixXdr x_k;
		x_k = d_k * (v_k.transpose());
		ofstream x_file;
		x_file.open((string(command_line_opts.OUTPUT_PATH) + string("xvals.txt")).c_str());
		x_file<<x_k.transpose()<<endl;
		x_file.close();
	}
}

int main(int argc, char const *argv[]){

	clock_t io_begin = clock();
    clock_gettime (CLOCK_REALTIME, &t0);

	pair<double,double> prev_error = make_pair(0.0,0.0);
	double prevnll=0.0;

	parse_args(argc,argv);

	//TODO: Memory Effecient Version of Mailman

	memory_efficient = command_line_opts.memory_efficient;
	text_version = command_line_opts.text_version;
    fast_mode = command_line_opts.fast_mode;
	missing = command_line_opts.missing;

	if(text_version){
		if(fast_mode)
			g.read_txt_mailman(command_line_opts.GENOTYPE_FILE_PATH,missing);
		else
			g.read_txt_naive(command_line_opts.GENOTYPE_FILE_PATH,missing);
	}
	else{
		g.read_plink(command_line_opts.GENOTYPE_FILE_PATH,missing,fast_mode);
	}

	//TODO: Implement these codes.
	if(missing && !fast_mode){
		cout<<"Missing version works only with mailman i.e. fast mode\n EXITING..."<<endl;
		exit(-1);
	}
	if(fast_mode && memory_efficient){
		cout<<"Memory effecient version for mailman EM not yet implemented"<<endl;
		cout<<"Ignoring Memory effecient Flag"<<endl;
	}
	if(missing && var_normalize){
		cout<<"Missing version works only without variance normalization\n EXITING..."<<endl;
		exit(-1);
	}

    MAX_ITER =  command_line_opts.max_iterations ;
	k_orig = command_line_opts.num_of_evec ;
	debug = command_line_opts.debugmode ;
	check_accuracy = command_line_opts.getaccuracy;
	var_normalize = command_line_opts.var_normalize;
	accelerated_em = command_line_opts.accelerated_em;
	k = k_orig + command_line_opts.l;
	k = (int)ceil(k/10.0)*10;
	command_line_opts.l = k - k_orig;
	p = g.Nsnp;
	n = g.Nindv;
	convergence_limit = command_line_opts.convergence_limit;
	bool toStop=false;
	if(convergence_limit!=-1)
		toStop=true;
	srand((unsigned int) time(0));
	c.resize(p,k);
	x.resize(k,n);
	v.resize(p,k);
	means.resize(p,1);
	stds.resize(p,1);

	if(!fast_mode && !memory_efficient){
		geno_matrix.resize(p,n);
		g.generate_eigen_geno(geno_matrix,var_normalize);
	}

	clock_t io_end = clock();

	//TODO: Initialization of c with gaussian distribution
	c = MatrixXdr::Random(p,k);

	// Initial intermediate data structures
	blocksize = k;
	int hsegsize = g.segment_size_hori; 	// = log_3(n)
	int hsize = pow(3,hsegsize);
	int vsegsize = g.segment_size_ver; 		// = log_3(p)
	int vsize = pow(3,vsegsize);

	partialsums = new double [blocksize];
	sum_op = new double[blocksize];
	yint_e = new double [hsize*blocksize];
	yint_m = new double [hsize*blocksize];
	memset (yint_m, 0, hsize*blocksize * sizeof(double));
	memset (yint_e, 0, hsize*blocksize * sizeof(double));

	y_e  = new double*[g.Nindv];
	for (int i = 0 ; i < g.Nindv ; i++) {
		y_e[i] = new double[blocksize];
		memset (y_e[i], 0, blocksize * sizeof(double));
	}

	y_m = new double*[hsegsize];
	for (int i = 0 ; i < hsegsize ; i++)
		y_m[i] = new double[blocksize];

	for(int i=0;i<p;i++){
		means(i,0) = g.get_col_mean(i);
		stds(i,0) = g.get_col_std(i);
	}

	ofstream c_file;
	if(debug){
		c_file.open((string(command_line_opts.OUTPUT_PATH)+string("cvals_orig.txt")).c_str());
		c_file<<c<<endl;
		c_file.close();
		printf("Read Matrix\n");
	}

	cout<<"Running on Dataset of "<<g.Nsnp<<" SNPs and "<<g.Nindv<<" Individuals"<<endl;

	#if SSE_SUPPORT==1
		if(fast_mode)
			cout<<"Using Optimized SSE FastMultiply"<<endl;
	#endif

	clock_t it_begin = clock();
	for(int i=0;i<MAX_ITER;i++){

		MatrixXdr c1,c2,cint,r,v;
		double a,nll;
		if(debug){
			print_time ();
			cout << "*********** Begin epoch " << i << "***********" << endl;
		}
		if(accelerated_em!=0){
			#if DEBUG==1
				if(debug){
					print_time();
					cout << "Before EM" << endl;
				}
			#endif
			c1 = run_EM(c);
			c2 = run_EM(c1);
			#if DEBUG==1
				if(debug){
					print_time();
					cout << "After EM but before acceleration" << endl;
				}
			#endif
			r = c1-c;
			v = (c2-c1) - r;
			a = -1.0 * r.norm() / (v.norm()) ;
			if(accelerated_em==1){
				if(a>-1){
					a=-1;
					cint=c2;
				}
				else {
					cint = c - 2*a*r + a*a*v;
					nll = get_error_norm(cint).second;
					if(i>0){
						while(nll>prevnll && a<-1){
							a = 0.5 * (a-1);
							cint = c - 2*a*r +(a*a*v);
							nll = get_error_norm(cint).second;
						}
					}
				}
				c = cint;
			}
			else if(accelerated_em==2){
				cint = c - 2*a*r + a*a*v;
				c = cint;
				// c = run_EM(cint);
			}
		}
		else{
			c = run_EM(c);
		}

		if ( accelerated_em == 1 || check_accuracy || toStop) {
			pair<double,double> e = get_error_norm(c);
			prevnll = e.second;
			if(check_accuracy)
				cout<<"Iteration "<<i+1<<"  "<<std::setprecision(15)<<e.first<<"  "<<e.second<<endl;
			if(abs(e.first-prev_error.first)<=convergence_limit){
				cout<<"Breaking after "<<i+1<<" iterations"<<endl;
				break;
			}
			prev_error = e;
		}
		if(debug){
			print_time ();
			cout << "*********** End epoch " << i << "***********" << endl;
		}

	}
	clock_t it_end = clock();

    print_vals();

	clock_t total_end = clock();
	double io_time = double(io_end - io_begin) / CLOCKS_PER_SEC;
	double avg_it_time = double(it_end - it_begin) / (MAX_ITER * 1.0 * CLOCKS_PER_SEC);
	double total_time = double(total_end - total_begin) / CLOCKS_PER_SEC;
	cout<<"IO Time:  "<< io_time << "\nAVG Iteration Time:  "<<avg_it_time<<"\nTotal runtime:   "<<total_time<<endl;

	delete[] sum_op;
	delete[] partialsums;
	delete[] yint_e;
	delete[] yint_m;

	for (int i  = 0 ; i < hsegsize; i++)
		delete[] y_m [i];
	delete[] y_m;

	for (int i  = 0 ; i < g.Nindv; i++)
		delete[] y_e[i];
	delete[] y_e;

	return 0;
}


/*======================================================================
Copyright 2019 Google LLC

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

    https://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.
======================================================================*/

#include <cmath>

#include <Eigen/Dense>
#include "PCV_Types.h"

#include "Caster.h"

using namespace Eigen;

Caster::Caster(
            double _Kx, double _Ky, double _ang,
            double  _b, double  _r,
            double  _f, double _Mf, double _If,
            double _Ih, double _Ii, double _Is, double _It, double _Ij,
            double _Ns, double _Nt, double _Nw,
            double _px, double _py, double _Mp, double _Ip)
    :Lambda(M3Z), Mu(V3Z),
     Ce(M3I), Jdot(M3Z), A(M3Z), CC(V3Z), L_Pk(M3Z), Mu_Pk(V3Z)
{
 Kx = _Kx;   Ky = _Ky;
  b = _b;    bi= 1.0/b;   r = _r;    ri= 1.0/r;
  f = _f;    e = f-b;    Mf = _Mf;  If = _If;
 Ih = _Ih;  Ii = _Ii;    Is = _Is;  It = _It;    Ij = _Ij;
 Ns = _Ns;  Nt = _Nt;    Nw = _Nw;
 px = _px;  py = _py;    Mp = _Mp;  Ip = _Ip;

 Px = Kx + px;
 Py = Ky + py;
 P2  = pow(Px,2) + pow(Py,2);

 enc_offset = (long)(((M_PI_2 - _ang) * Ns * ENC_RAD2CNT)+0.5);

 Fill_A();
 Fill_L_Pk();
}

void
Caster::Fill_A()
{
  double mfe2Ifih= Mf*e*e + If + Ii + Ih;
  double IsNs    = Is * Ns;
  double ItNt    = It * Nt;
  double ItNt2   = ItNt*Nt;

  A(0,0) = mfe2Ifih + IsNs*Ns + ItNt;
  A(0,1) = (Ii - Ih - ItNt2) * Nw;
  A(0,2) = mfe2Ifih - IsNs - ItNt;

  A(1,0) = A(0,1);
  A(1,1) = Mf*r*r + (Ii + Ih + ItNt2)*Nw*Nw + Ij;
  A(1,2) = (Ii - Ih + ItNt ) * Nw;

  A(2,0) = A(0,2);
  A(2,1) = A(1,2);
  A(2,2) = mfe2Ifih + Is + It;
}

inline void
Caster::Fill_CC()
{
  register double  u_w = u + w;
  register double Mfre = Mf*r*e;

  CC(0) =  Mfre * v * u_w;
  CC(1) = -Mfre * pow(u_w,2);
  CC(2) =  CC[0];
}

void
Caster::Fill_L_Pk()
{
  L_Pk(0,0) =  Mp;
  L_Pk(0,1) =  0.0;
  L_Pk(0,2) = -Mp * Py;

  L_Pk(1,0) =  0.0;
  L_Pk(1,1) =  Mp;
  L_Pk(1,2) =  Mp * Px;

  L_Pk(2,0) = L_Pk(0,2);
  L_Pk(2,1) = L_Pk(1,2);
  L_Pk(2,2) = Ip + Mp*P2;
}

inline void
Caster::Fill_Mu_Pk()
{
  register double w2  = pow(w,2);

  Mu_Pk(0) = -Mp * Px * w2;
  Mu_Pk(1) = -Mp * Py * w2;
  Mu_Pk(2) =  0.0;
}

inline void
Caster::Fill_Ce()  // Ce (C_theta) (a.k.a. Ji )
{
  Ce(0,0) =  bi*s;
  Ce(0,1) = -bi*c;
  Ce(0,2) = -bi*(Kx*c + Ky*s) - 1.0;

  Ce(1,0) =  ri*c;
  Ce(1,1) =  ri*s;
  Ce(1,2) =  ri*(Kx*s - Ky*c);

  Ce(2,0) =  0.0;
  Ce(2,1) =  0.0;
  Ce(2,2) =  1.0;
}

inline void
Caster::Fill_Jdot()
{
  register double u_w = u + w;

  Jdot(0,0) =  b*c*u_w;
  Jdot(0,1) = -r*s*u_w;
  Jdot(0,2) =  Kx*w + b*c*u_w;

  Jdot(1,0) =  b*s*u_w;
  Jdot(1,1) =  r*c*u_w;
  Jdot(1,2) =  Ky*w + b*s*u_w;

  Jdot(2,0) =  0.0;
  Jdot(2,1) =  0.0;
  Jdot(2,2) =  0.0;
}

void
Caster::Fill_LM( double const _u,   // sigma dot
                 double const _v,   //  rho  dot
                 double const _w)   // theta dot
{

  static Vector3d   Qdot(3);

  // SET STATE
  Qdot[0] = u = _u;
  Qdot[1] = v = _v;
  Qdot[2] = w = _w;

  // FILL JACOBIANS
  Fill_Ce();
  Fill_Jdot();

  // FILL JT-SPACE CC VECTOR (NOTE: 'A' IS CONSTANT)
  Fill_CC();
  // FILL OP-SPACE CC VECTOR (NOTE: 'L_Pk' IS CONSTANT)
  Fill_Mu_Pk();

  //Lambda = L_Pk + (Cet * A * Ce)
  Lambda = L_Pk + Ce.transpose() * A * Ce;

  //Mu = Mu_Pk + [Cet * ( CC - (A*(Ce*(Jdot*Qdot))) )]
  Mu = Mu_Pk + Ce.transpose()*(CC-(A*(Ce*(Jdot*Qdot))));
}


#ifndef STAN_MATH_PRIM_FUN_LAMBERT_W_HPP
#define STAN_MATH_PRIM_FUN_LAMBERT_W_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/fun/boost_policy.hpp>
#include <boost/math/special_functions/lambert_w.hpp>

namespace stan {
namespace math {

/**
 * Compute the Lambert W function on W0 branch for a value x.
 *
 * @tparam T type of value
 * @param x value
 * @return value of the W0 branch of the Lambert W function for x
 * @throw std::domain_error if x is less than or equal to `-e^(-1)`
 */
template <typename T, require_arithmetic_t<T>* = nullptr>
inline double lambert_w0(const T& x) {
  return boost::math::lambert_w0(x, boost_policy_t<>());
}

/**
 * Compute the Lambert W function on W-1 branch for a value x.
 *
 * @tparam T type of value
 * @param x value
 * @return value of the W-1 branch of the Lambert W function for x
 * @throw std::domain_error if x is less than or equal to `-e^(-1)` or greater
 * than or equal to 0
 */
template <typename T, require_arithmetic_t<T>* = nullptr>
inline double lambert_wm1(const T& x) {
  return boost::math::lambert_wm1(x, boost_policy_t<>());
}

namespace internal {

/**
 * Structure to wrap lambert_w0() so it can be vectorized.
 *
 * @tparam T type of variable
 * @param x variable
 * @return value of the W0 branch of the Lambert W function at x.
 * @throw std::domain_error if x is less than or equal to `-e^(-1)`
 */
struct lambert_w0_fun {
  template <typename T>
  static inline T fun(const T& x) {
    return lambert_w0(x);
  }
};

/**
 * Structure to wrap lambert_wm1() so it can be vectorized.
 *
 * @tparam T type of variable
 * @param x variable
 * @return value of the W-1 branch of the Lambert W function at x.
 * @throw std::domain_error if x is less than or equal to `-e^(-1)` or greater
 * than or equal to 0
 */
struct lambert_wm1_fun {
  template <typename T>
  static inline T fun(const T& x) {
    return lambert_wm1(x);
  }
};
}  // namespace internal

/**
 * Vectorized version of lambert_w0().
 *
 * @tparam T type of container
 * @param x container
 * @return value of the W0 branch of the Lambert W function for each value in x
 * @throw std::domain_error if x is less than or equal to `-e^(-1)`
 */
template <typename T, require_not_stan_scalar_t<T>* = nullptr>
inline auto lambert_w0(const T& x) {
  return apply_scalar_unary<internal::lambert_w0_fun, T>::apply(x);
}

/**
 * Vectorized version of lambert_wm1().
 *
 * @tparam T type of container
 * @param x container
 * @return value of the W0 branch of the Lambert W function for each value in x
 * @throw std::domain_error if x is less than or equal to `-e^(-1)` or greater
 * than or equal to 0
 */
template <typename T, require_not_stan_scalar_t<T>* = nullptr>
inline auto lambert_wm1(const T& x) {
  return apply_scalar_unary<internal::lambert_wm1_fun, T>::apply(x);
}

}  // namespace math
}  // namespace stan

#endif


#ifndef POSITIONBASEDDYNAMICS_CPARTICLE_H
#define POSITIONBASEDDYNAMICS_CPARTICLE_H

#include <memory>
#include <vector>
#include <CVector3.hpp>
#include <Eigen/Dense>

namespace PBD {

    //TODO:: Make these external parameters
    const static double minVel = 0.001;
    const static double minRotVel = 0.00001;
    const static double velDamping = 1.0;

template<typename T_real=double, typename T_vector=Eigen::Vector3d, typename T_quaternion=Eigen::Quaterniond>
class CParticle
{
public:
    typedef std::shared_ptr< CParticle<T_real> > Ptr;

    typedef const std::shared_ptr< CParticle<T_real> > ConstPtr;

public:
    CParticle() = default;

    CParticle(const T_real& x, const T_real& y, const T_real& z, const T_real& mass, const T_real& size, const size_t& group=0):
            m_position( T_vector(x,y,z) ),
            m_predPosition( T_vector(x,y,z) ),
            m_velocity( T_vector(0,0,0) ),
            m_angularVelocity( T_vector(0,0,0) ),
            m_extForce( T_vector(0,0,0) ),
            m_mass(mass),
            m_massInv(1/mass),
            m_size(size),
            m_group(group)
    {
        m_orientation.setIdentity();
        m_predOrientation.setIdentity();
    }

    CParticle(const T_real& x,    const T_real& y,  const T_real& z,
              const T_real& vx,   const T_real& vy, const T_real& vz,
              const T_real& mass, const T_real& size,
              const size_t& group=0):
            m_position( T_vector(x,y,z) ),
            m_predPosition( T_vector(x,y,z) ),
            m_velocity( T_vector(vx,vy,vz) ),
            m_angularVelocity( T_vector(0,0,0) ),
            m_extForce( T_vector(0,0,0) ),
            m_mass(mass),
            m_massInv(1/mass),
            m_size(size),
            m_group(group)
    {
        m_orientation.setIdentity();
        m_predOrientation.setIdentity();
    }

    CParticle(const T_vector& pos,
              const T_vector& vel,
              const T_real& mass, const T_real& size,
              const size_t& group=0):
            m_position( pos ),
            m_predPosition( T_vector(0,0,0) ),
            m_velocity( vel ),
            m_angularVelocity( T_vector(0,0,0) ),
            m_mass(mass),
            m_massInv(1/mass),
            m_size(size),
            m_group(group)
    {
        m_orientation.setIdentity();
        m_predOrientation.setIdentity();
    }

    ~CParticle() = default;

    CParticle(const CParticle &p) :
            m_position(p.m_position),
            m_orientation(p.m_orientation),
            m_predPosition(p.m_predPosition),
            m_velocity(p.m_velocity),
            m_angularVelocity(p.m_angularVelocity),
            m_extForce(p.m_extForce),
            m_mass(p.m_mass),
            m_massInv(p.m_massInv),
            m_size(p.m_size),
            m_group(p.m_group),
            m_predOrientation(p.m_predOrientation)
    {
        m_predPosition = m_position;
    }

    CParticle(CParticle &&p) noexcept :
            m_position(std::move(p.m_position)),
            m_orientation(std::move(p.m_orientation)),
            m_predPosition(std::move(p.m_predPosition)),
            m_velocity(std::move(p.m_velocity)),
            m_angularVelocity(std::move(p.m_angularVelocity)),
            m_extForce(std::move(p.m_extForce)),
            m_mass(std::move(p.m_mass)),
            m_massInv(std::move(p.m_massInv)),
            m_size(std::move(p.m_size)),
            m_group(std::move(p.m_group)),
            m_predOrientation(std::move(p.m_predOrientation))
    {
        m_predPosition = m_position;
    }

    CParticle &operator=(const CParticle &p)
    {
        m_position = p.m_position;
        m_orientation = p.m_orientation;
        m_predPosition = p.m_predPosition;
        m_velocity = p.m_velocity;
        m_angularVelocity = p.m_angularVelocity;
        m_extForce = p.m_extForce;
        m_mass = p.getMass();
        m_massInv = p.getMassInv();
        m_size = p.m_size;
        m_group = p.m_group;
        m_predOrientation = p.m_predOrientation;
        return *this;
    }

    CParticle &operator=(CParticle &&p) noexcept
    {
        m_position = std::move(p.m_position);
        m_orientation = std::move(p.m_orientation);
        m_predPosition = std::move(p.m_predPosition);
        m_velocity = std::move(p.m_velocity);
        m_angularVelocity = std::move(m_angularVelocity);
        m_extForce = std::move(p.m_extForce);
        m_mass = std::move(p.getMass());
        m_massInv = std::move(p.getMassInv());
        m_size = std::move(p.m_size);
        m_group = std::move(p.m_group);
        m_predOrientation = std::move(p.m_predOrientation);
        return *this;
    }

    void setMass(const T_real &m)
    {
        m_mass = m;
        m_massInv = 1 / m_mass;
    }

    T_real getMass( ) const { return m_mass; }

    T_real getMassInv( ) const { return m_massInv; }

    void clearExtForces() { m_extForce = T_vector(0,0,0); }

    void symplecticEulerUpdate(T_real timeStep)
    {
        if (m_mass <= 0)
        {
            m_predPosition = m_position;
            m_predOrientation = m_orientation;
        }
        else
        {
            T_vector vel = m_velocity + m_extForce * timeStep * m_massInv;
            m_predPosition = m_position + vel * timeStep;

            T_real wNorm = m_angularVelocity.norm();
            if (wNorm > minRotVel)
            {
                T_vector axis = m_angularVelocity / wNorm * sin( wNorm * timeStep / 2.0 );
                T_real   angle = cos ( wNorm * timeStep / 2 );
                T_quaternion delta;
                delta.x() = axis.x();
                delta.y() = axis.y();
                delta.z() = axis.z();
                delta.w() = angle;
                m_predOrientation = delta * m_orientation;
            }
            else
            {
                m_predOrientation = m_orientation;
            }

        }
    }

    void updateVelocity(T_real timeStep)
    {
        if (m_mass > 0)
        {
            T_quaternion quat = m_predOrientation*m_orientation.inverse();
            Eigen::AngleAxisd aa( quat );

            m_velocity = ((m_predPosition - m_position) / timeStep) * PBD::velDamping;
            m_angularVelocity = aa.axis() * aa.angle() / timeStep;
        }
    }

    void updatePositionsWithPredPositions()
    {
        if (m_mass > 0)
        {
            m_position = m_predPosition;
            m_orientation = m_predOrientation;
        }
    }

    T_vector m_position;
    T_vector m_predPosition;
    T_quaternion m_orientation;
    T_quaternion m_predOrientation;
    T_vector m_velocity;
    T_vector m_angularVelocity;
    T_vector m_extForce;
    T_real m_size;
    size_t m_group;

protected:
    T_real m_mass;
    T_real m_massInv;

};

}

#endif


//
// Created by Linyuan Shi on 10/18/19.
// Copyright (c) 2019 University of Florida. All rights reserved.
//

#include <boost/multi_array.hpp>
#include <cmath>
#include <iostream>
using std::cerr;
using std::endl;

#include <fstream>
using std::istringstream;

#include <filesystem>

#include <vector>
using std::vector;

#include "atom.h"
#include "simulation_cell.h"
#include "grid_info.h"
#include "bin.h"
#include "color.h"

typedef boost::multi_array<bin, 3> grid_bin_type;

void read_xyz_file(vector<atom>& vec_atoms, grid_bin_type& grid_bins, const std::filesystem::path& path_structure,
	const simulation_cell& sc, const grid_info& gr_if, std::ostream& os) {

	unsigned int count = 2;
	constexpr size_t max_character_per_line = 512;
	size_t n_atoms;
	string line, element;
	double x, y, z; // atom position
	grid_bin_type::size_type xbx, yby, zbz; // indexes of the bin
	istringstream line_s;
	std::ifstream file(path_structure);
	const std::array<double, 3> gr_dr = gr_if.get_grid_size();
	const auto [ox, oy, oz] = sc.get_origin();
	const auto [vx, vy, vz] = sc.get_vectors();
	const auto [gr_dx, gr_dy, gr_dz] = gr_dr;
	const auto [gr_nx, gr_ny, gr_nz] = gr_if.get_grid_no();

	if (!file.is_open()) {
		cerr << Color::Red << "Failed to read file: " << path_structure.filename() << endl;
		cerr << "The program is aborted." << Color::None << endl;
		exit(EXIT_FAILURE);
	}
	else {
		os << "Begin to read xyz file: " << path_structure.filename() << endl;
	}
	file >> n_atoms;
	os << n_atoms << " atoms are in this system." << endl;
	file.ignore(max_character_per_line, '\n');
	file.ignore(max_character_per_line, '\n');
	if (!file) {
		cerr << Color::Red << "Incorrect xyz file format." << endl;
		cerr << "The program is aborted." << Color::None << endl;
		exit(EXIT_FAILURE);
	}
	vec_atoms.reserve(n_atoms);
	while (std::getline(file, line)) {
		++count;
		line_s.clear();
		line_s.str(line);
		line_s >> element >> x >> y >> z;

		// Color::None origin
		x -= ox;
		y -= oy;
		z -= oz;
		x = std::fmod(x, vx);
		y = std::fmod(y, vy);
		z = std::fmod(z, vz);
		if (x >= vx || x < 0) {
			cerr << Color::Red << path_structure << " Line " << count << " :atom is outside the cell at x direction." << endl;
			cerr << "The program is aborted." << Color::None << endl;
			exit(EXIT_FAILURE);
		}
		if (y >= vy || y < 0) {
			cerr << Color::Red << path_structure << " Line " << count << " :atom is outside the cell at y direction." << endl;
			cerr << "The program is aborted." << Color::None << endl;
			exit(EXIT_FAILURE);
		}
		if (z >= vz || z < 0) {
			cerr << Color::Red << path_structure << " Line " << count << " :atom is outside the cell at z direction." << endl;
			cerr << "The program is aborted." << Color::None << endl;
			exit(EXIT_FAILURE);
		}
		vec_atoms.emplace_back(element, x, y, z);

		// set bin values
		xbx = static_cast<grid_bin_type::size_type> (x / gr_dx);
		yby = static_cast<grid_bin_type::size_type> (y / gr_dy);
		zbz = static_cast<grid_bin_type::size_type> (z / gr_dz);

		if (xbx >= gr_nx || yby >= gr_ny || zbz >= gr_nz) {
			cerr << Color::Red << "The bin_index of bin is over the max number of bins on one direction." << endl;
			cerr << "The program is aborted." << Color::None << endl;
			exit(EXIT_FAILURE);
		}

		grid_bins[xbx][yby][zbz].add_atm_ptr(&vec_atoms.back());
	}
	if (n_atoms != vec_atoms.size()) {
		cerr << Color::Red << "The actual number of atoms: " << vec_atoms.size()
			<< "is different with the declared number: " << n_atoms << endl;
		cerr << "The program is aborted." << Color::None << endl;
		exit(EXIT_FAILURE);
	}
	os << "Finish reading xyz position file." << endl;
	file.close();
}


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/matching/cascade_hasher.h"

#include <Eigen/Core>
#include <glog/logging.h>
#include <stdint.h>

#include <algorithm>
#include <cmath>
#include <utility>
#include <vector>

#include "theia/matching/distance.h"
#include "theia/matching/feature_matcher.h"
#include "theia/matching/indexed_feature_match.h"
#include "theia/util/random.h"

namespace theia {

namespace {

void GetZeroMeanDescriptor(const std::vector<Eigen::VectorXf>& sift_desc,
                           Eigen::VectorXf* mean) {
  mean->setZero(sift_desc[0].size());
  for (int i = 0; i < sift_desc.size(); i++) {
    *mean += sift_desc[i];
  }
  *mean /= static_cast<double>(sift_desc.size());
}

}  // namespace

bool CascadeHasher::Initialize(const int num_dimensions_of_descriptor) {
  num_dimensions_of_descriptor_ = num_dimensions_of_descriptor;
  primary_hash_projection_.resize(kHashCodeSize, num_dimensions_of_descriptor_);

  // Initialize primary hash projection.
  for (int i = 0; i < kHashCodeSize; i++) {
    for (int j = 0; j < num_dimensions_of_descriptor; j++) {
      primary_hash_projection_(i, j) = rng_->RandGaussian(0.0, 1.0);
    }
  }

  // Initialize secondary hash projection.
  for (int i = 0; i < kNumBucketGroups; i++) {
    secondary_hash_projection_[i].resize(kNumBucketBits,
                                         num_dimensions_of_descriptor_);
    for (int j = 0; j < kNumBucketBits; j++) {
      for (int k = 0; k < num_dimensions_of_descriptor_; k++) {
        secondary_hash_projection_[i](j, k) = rng_->RandGaussian(0.0, 1.0);
      }
    }
  }

  return true;
}

void CascadeHasher::CreateHashedDescriptors(
    const std::vector<Eigen::VectorXf>& sift_desc,
    HashedImage* hashed_image) const {
  for (int i = 0; i < sift_desc.size(); i++) {
    // Use the zero-mean shifted descriptor.
    const auto descriptor = sift_desc[i] - hashed_image->mean_descriptor;
    auto& hash_code = hashed_image->hashed_desc[i].hash_code;

    // Compute hash code.
    const Eigen::VectorXf primary_projection =
        primary_hash_projection_ * descriptor;
    for (int j = 0; j < kHashCodeSize; j++) {
      hash_code[j] = primary_projection(j) > 0;
    }

    // Determine the bucket index for each group.
    for (int j = 0; j < kNumBucketGroups; j++) {
      uint16_t bucket_id = 0;
      const Eigen::VectorXf secondary_projection =
          secondary_hash_projection_[j] * descriptor;

      for (int k = 0; k < kNumBucketBits; k++) {
        bucket_id = (bucket_id << 1) + (secondary_projection(k) > 0 ? 1 : 0);
      }
      hashed_image->hashed_desc[i].bucket_ids[j] = bucket_id;
    }
  }
}

void CascadeHasher::BuildBuckets(HashedImage* hashed_image) const {
  for (int i = 0; i < kNumBucketGroups; i++) {
    // Add the descriptor ID to the proper bucket group and id.
    for (int j = 0; j < hashed_image->hashed_desc.size(); j++) {
      const uint16_t bucket_id = hashed_image->hashed_desc[j].bucket_ids[i];
      hashed_image->buckets[i][bucket_id].push_back(j);
    }
  }
}

// Steps:
//   1) Get zero mean descriptor.
//   2) Compute hash code and hash buckets.
//   3) Construct buckets.
HashedImage CascadeHasher::CreateHashedSiftDescriptors(
    const std::vector<Eigen::VectorXf>& sift_desc) const {
  HashedImage hashed_image;

  // Allocate the buckets even if no descriptors exist to fill them.
  hashed_image.buckets.resize(kNumBucketGroups);
  for (int i = 0; i < kNumBucketGroups; i++) {
    hashed_image.buckets[i].resize(kNumBucketsPerGroup);
  }

  if (sift_desc.size() == 0) {
    return hashed_image;
  }

  GetZeroMeanDescriptor(sift_desc, &hashed_image.mean_descriptor);

  // Allocate space for hash codes and bucket ids.
  hashed_image.hashed_desc.resize(sift_desc.size());

  // Allocate space for each bucket id.
  for (int i = 0; i < sift_desc.size(); i++) {
    hashed_image.hashed_desc[i].bucket_ids.resize(kNumBucketGroups);
  }

  // Create hash codes for each feature.
  CreateHashedDescriptors(sift_desc, &hashed_image);

  // Build the buckets.
  BuildBuckets(&hashed_image);
  return hashed_image;
}

// Matches images with a fast matching scheme based on the hash codes
// previously generated.
void CascadeHasher::MatchImages(
    const HashedImage& hashed_image1,
    const std::vector<Eigen::VectorXf>& descriptors1,
    const HashedImage& hashed_image2,
    const std::vector<Eigen::VectorXf>& descriptors2,
    const double lowes_ratio,
    std::vector<IndexedFeatureMatch>* matches) const {
  if (descriptors1.size() == 0 || descriptors2.size() == 0) {
    return;
  }

  static const int kNumTopCandidates = 10;
  const double sq_lowes_ratio = lowes_ratio * lowes_ratio;
  L2 l2_distance;

  // Reserve space for the matches.
  matches->reserve(
      static_cast<int>(std::min(descriptors1.size(), descriptors2.size())));

  // Preallocate the candidate descriptors container.
  std::vector<int> candidate_descriptors;
  candidate_descriptors.reserve(descriptors2.size());

  // Preallocated hamming distances. Each column indicates the hamming distance
  // and the rows collect the descriptor ids with that
  // distance. num_descriptors_with_hamming_distance keeps track of how many
  // descriptors have that distance.
  Eigen::MatrixXi candidate_hamming_distances(descriptors2.size(),
                                              kHashCodeSize + 1);
  Eigen::VectorXi num_descriptors_with_hamming_distance(kHashCodeSize + 1);

  // Preallocate the container for keeping euclidean distances.
  std::vector<std::pair<float, int> > candidate_euclidean_distances;
  candidate_euclidean_distances.reserve(kNumTopCandidates);

  // A preallocated vector to determine if we have already used a particular
  // feature for matching (i.e., prevents duplicates).
  std::vector<bool> used_descriptor(descriptors2.size());
  for (int i = 0; i < hashed_image1.hashed_desc.size(); i++) {
    candidate_descriptors.clear();
    num_descriptors_with_hamming_distance.setZero();
    candidate_euclidean_distances.clear();

    const auto& hashed_desc = hashed_image1.hashed_desc[i];

    // Accumulate all descriptors in each bucket group that are in the same
    // bucket id as the query descriptor.
    for (int j = 0; j < kNumBucketGroups; j++) {
      const uint16_t bucket_id = hashed_desc.bucket_ids[j];
      for (const auto& feature_id : hashed_image2.buckets[j][bucket_id]) {
        candidate_descriptors.emplace_back(feature_id);
        used_descriptor[feature_id] = false;
      }
    }

    // Skip matching this descriptor if there are not at least 2 candidates.
    if (candidate_descriptors.size() <= kNumTopCandidates) {
      continue;
    }

    // Compute the hamming distance of all candidates based on the comp hash
    // code. Put the descriptors into buckets corresponding to their hamming
    // distance.
    for (const int candidate_id : candidate_descriptors) {
      if (used_descriptor[candidate_id]) {
        continue;
      }
      used_descriptor[candidate_id] = true;
      const uint8_t hamming_distance =
          (hashed_desc.hash_code ^
           hashed_image2.hashed_desc[candidate_id].hash_code).count();
      candidate_hamming_distances(
          num_descriptors_with_hamming_distance(hamming_distance)++,
          hamming_distance) = candidate_id;
    }

    // Compute the euclidean distance of the k descriptors with the best hamming
    // distance.
    candidate_euclidean_distances.reserve(kNumTopCandidates);
    for (int j = 0; j < candidate_hamming_distances.cols(); j++) {
      for (int k = 0; k < num_descriptors_with_hamming_distance(j); k++) {
        const int candidate_id = candidate_hamming_distances(k, j);
        const float distance =
            l2_distance(descriptors2[candidate_id], descriptors1[i]);
        candidate_euclidean_distances.emplace_back(distance, candidate_id);
        if (candidate_euclidean_distances.size() > kNumTopCandidates) {
          break;
        }
      }
      if (candidate_euclidean_distances.size() > kNumTopCandidates) {
        break;
      }
    }

    // Find the top 2 candidates based on euclidean distance.
    std::partial_sort(candidate_euclidean_distances.begin(),
                      candidate_euclidean_distances.begin() + 2,
                      candidate_euclidean_distances.end());

    // Only add to output matches if it passes the ratio test.
    if (candidate_euclidean_distances[0].first >
        candidate_euclidean_distances[1].first * sq_lowes_ratio) {
      continue;
    }

    matches->emplace_back(i,
                          candidate_euclidean_distances[0].second,
                          candidate_euclidean_distances[0].first);
  }
}

}  // namespace theia


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
/* blockMatmul.cpp - Data-movement operations on arrays of slots
 */
#include <algorithm>
#include <NTL/BasicThreadPool.h>
#include "matmul.h"

// A class that implements (sparse) linear transformation over the base ring
template<class type> class blockMatmul_impl {
  PA_INJECT(type)
  const MatrixCacheType buildCache;
  std::unique_ptr<CachedzzxMatrix> zCache;
  std::unique_ptr<CachedDCRTMatrix> dCache;

  BlockMatMul<type>& mat;
  const EncryptedArrayDerived<type>& ea;

public:
  blockMatmul_impl(MatMulBase& _mat, MatrixCacheType tag)
    : buildCache(tag), mat(dynamic_cast< BlockMatMul<type>& >(_mat)),
      ea(_mat.getEA().getDerived(type()))
  {
    long n = ea.size() * ea.getDegree();
    if (buildCache==cachezzX)
      zCache.reset(new CachedzzxMatrix(NTL::INIT_SIZE, n));
    else if (buildCache==cacheDCRT)
      dCache.reset(new CachedDCRTMatrix(NTL::INIT_SIZE, n));
  }

  // Extract one "column" from a matrix that was built with buildLinPolyCoeffs
  bool shiftedColumnInDiag(zzX& zpoly, long k,
                           const std::vector< std::vector<RX> >& diag)
  {
    // extract "column" with index k and store it in cvev
    bool zero = true;
    vector<RX> cvec(ea.size());
    for (long j = 0; j < ea.size(); j++) {
      cvec[j] = diag[j][k];
      if (!NTL::IsZero(cvec[j])) zero = false;
    }
    if (zero) { // all are zeros
      convert(zpoly, RX::zero());
      return true;
    }
    ea.encode(zpoly, cvec);

    if (k>0) {
      long p = ea.getContext().zMStar.getP();
      long m = ea.getContext().zMStar.getM();
      long d = ea.getDegree();
      long exp = PowerMod(mcMod(p, m), d-k, m); // apply inverse automorphism
      const auto& F = ea.getTab().getPhimXMod();
      RX rpoly1, rpoly2;

      convert(rpoly1, zpoly);
      plaintextAutomorph(rpoly2,rpoly1, exp, m, F);
      convert(zpoly, rpoly2);
    }
    return false;
  }

  // Process a single block diagonal with index idx, making calls
  // to the get(i,j) method and storing the result in the diag argument
  bool processDiagonal(std::vector< std::vector<RX> >& diag,
                       long idx, long nslots, long d)
  {
    bool zDiag = true;
    long nzLast = -1;
    std::vector<RX> entry1(d);
    mat_R entry(INIT_SIZE, d, d);
    for (long j = 0; j < nslots; j++) {
      bool zEntry = mat.get(entry, mcMod(j-idx, nslots), j);
      if (!zEntry && IsZero(entry)) zEntry=true; // zero is an empty entry too
      // get(...) returns true if the entry is empty, false otherwise

      assert(zEntry || (entry.NumRows() == d && entry.NumCols() == d));

      if (!zEntry) {    // non-empty entry
        zDiag = false;  // mark diagonal as non-empty

        // clear entries between last nonzero entry and this one
        for (long jj = nzLast+1; jj < j; jj++)
          for (long k = 0; k < d; k++)
            clear(diag[jj][k]);
	nzLast = j;

	// recode entry as a vector of polynomials
	for (long k = 0; k < d; k++) conv(entry1[k], entry[k]);

	ea.buildLinPolyCoeffs(diag[j], entry1);
      } // now diag[j] contains the lin poly coeffs
    }

    if (!zDiag) // clear trailing zero entries
      for (long jj = nzLast+1; jj < nslots; jj++)
	for (long k = 0; k < d; k++)
	  clear(diag[jj][k]);

    return zDiag;
  }

  // This code has a complexity of N+d (instead of N*d) where N is the
  // number of nonzero diagonal blocks. However, it requires space for
  // d extra ciphertexts
  void multiply(Ctxt* ctxt)
  {
    RBak bak; bak.save(); ea.getTab().restoreContext();

    const RXModulus& F = ea.getTab().getPhimXMod();
    const PAlgebra& zMStar = ea.getContext().zMStar;
    long p = zMStar.getP();
    long m = zMStar.getM();
    long nslots = ea.size();
    long d = ea.getDegree();

    std::vector< std::vector<RX> > diag(nslots);
    for (long j = 0; j < nslots; j++) diag[j].resize(d);

    std::vector<Ctxt> acc;
    std::unique_ptr<Ctxt> shCtxt;
    if (ctxt!=nullptr) { // we need to do an actual multiplication
      ctxt->cleanUp();   // not sure, but this may be a good idea
      acc.assign(d, Ctxt(ZeroCtxtLike, *ctxt));
      shCtxt.reset(new Ctxt(*ctxt));
    }

    // Check if we have the relevant constant in cache
    CachedzzxMatrix* zcp;
    CachedDCRTMatrix* dcp;
    mat.getCache(&zcp, &dcp);

    long lastShift = 0;
    bool sequential = (ea.dimension()==1) && ea.nativeDimension(0);
    // if just a single native dimension, then rotate adds only little noise

    for (long i = 0; i < nslots; i++) { // Process the diagonals one at a time
      bool zDiag = true;
      // For each diagonal i, we update the d accumulators y_0,..,y_{d-1}
      // with y_k += \sigma^{-k}(\lambda_{i,k}) * \rho^i(x)

      std::vector<zzX> zpoly(d);
      std::vector<zzX*> zzxPtr(d,nullptr);
      std::vector<DoubleCRT*> dcrtPtr(d,nullptr);

      if (dcp!=nullptr) for (long k=0; k<d; k++) { // DoubleCRT cache exists
          dcrtPtr[k] = (*dcp)[d*i +k].get();
          if (dcrtPtr[k]!=nullptr) zDiag = false;
      }
      else if (zcp!=nullptr) for (long k=0; k<d; k++) {// zzX but no DoubleCRT
          zzxPtr[k] = (*zcp)[d*i +k].get();
          if (zzxPtr[k]!=nullptr) zDiag = false;
      } else {
        zDiag = processDiagonal(diag, i, nslots, d);

        // extract the "columns" from diag and encode them in zpoly
        if (!zDiag) for (long k=0; k<d; k++) {
            if (!shiftedColumnInDiag(zpoly[k],k,diag))// compute the constant
            zzxPtr[k] = &(zpoly[k]);                  // returns true on zero
        }
      }
      if (zDiag) continue; // nothing to do for this diagonal
      // done preparing all the zzxPtr, dcrtPtr variables

      if (ctxt!=nullptr) {
        if (i > 0) { // rotate the ciphertext
          if (sequential)  // rotate the previous version
            ea.rotate(*shCtxt, i-lastShift);
          else {           // rotate the original ciphertext
            *shCtxt = *ctxt;
            ea.rotate(*shCtxt, i);
	  }
	  lastShift = i;
	} // if (i>0)

	// Depending on zzxPtr, dcrtPtr, update the accumulated sums
	for (long k=0; k<d; k++) if (dcrtPtr[k]!=nullptr || zzxPtr[k]!=nullptr) {
            Ctxt tmp1(*shCtxt);
            if (dcrtPtr[k] != nullptr) tmp1.multByConstant(*(dcrtPtr[k]));
            else                    tmp1.multByConstant(*(zzxPtr[k]));
            acc[k] += tmp1;
          }
      } // if (ctxt!=nullptr)

      // allocate constants and store in the cache, if needed
      if (buildCache==cachezzX) for (long k=0; k<d; k++) {
          (*zCache)[d*i +k].reset( new zzX(*(zzxPtr[k])) );
      }
      else if (buildCache==cacheDCRT) for (long k=0; k<d; k++) {
          (*dCache)[d*i +k].reset(new DoubleCRT(*(zzxPtr[k]),ea.getContext()));
      }
    } // end of i'th diagonal

    // Finally, compute the result as \sum_{k=0}^{d-1} \sigma_k(y_k)
    if (ctxt!=nullptr) {
      *ctxt = acc[0];
      for (long k = 1; k < d; k++) {
	acc[k].frobeniusAutomorph(k);
	*ctxt += acc[k];
      }
    }
    // "install" the cache (if needed)
    if (buildCache == cachezzX)
      mat.installzzxcache(zCache);
    else if (buildCache == cacheDCRT)
      mat.installDCRTcache(dCache);
  } // end of multiply(...)
};

// Wrapper functions around the implemenmtation class
static void block_matmul(Ctxt* ctxt,MatMulBase& mat,MatrixCacheType buildCache)
{
  MatMulLock locking(mat, buildCache);

  // If locking.getType()!=cacheEmpty then we really do need to
  // build the cache, and we also have the lock for it.

  if (locking.getType()==cacheEmpty && ctxt==nullptr) //  nothing to do
    return;

  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      blockMatmul_impl<PA_GF2> M(mat, locking.getType());
      M.multiply(ctxt);
      break;
    }
    case PA_zz_p_tag: {
      blockMatmul_impl<PA_zz_p> M(mat, locking.getType());
      M.multiply(ctxt);
      break;
    }
    default:
      throw std::logic_error("block_matmul: neither PA_GF2 nor PA_zz_p");
  }
}
void buildCache4BlockMatMul(MatMulBase& mat, MatrixCacheType buildCache)
{ block_matmul(nullptr, mat, buildCache); }

void blockMatMul(Ctxt& ctxt, MatMulBase& mat, MatrixCacheType buildCache)
{ block_matmul(&ctxt, mat, buildCache); }

// Applying matmul to plaintext, useful for debugging
template<class type>
class blockMatmul_pa_impl {
public:
  PA_INJECT(type)

  static void multiply(NewPlaintextArray& pa, BlockMatMul<type>& mat)
  {
    const EncryptedArrayDerived<type>& ea = mat.getEA().getDerived(type());
    const PAlgebra& zMStar = ea.getContext().zMStar;
    RBak bak; bak.save(); ea.getTab().restoreContext();

    long n = ea.size();
    long d = ea.getDegree();
    vector<RX>& data = pa.getData<type>();

    vector<RX> res(n);
    for (long j = 0; j < n; j++) {
      vec_R acc, tmp, tmp1;
      mat_R val;

      acc.SetLength(d);
      for (long i = 0; i < n; i++) {
         if (!mat.get(val, i, j)) {
            VectorCopy(tmp1, data[i], d);
            mul(tmp, tmp1, val);
            add(acc, acc, tmp);
         }
      }
      conv(res[j], acc);
    }
    data = res;
  }
};
void blockMatMul(NewPlaintextArray& pa, MatMulBase& mat)
{
  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      BlockMatMul<PA_GF2>& mat1= dynamic_cast< BlockMatMul<PA_GF2>& >(mat);
      blockMatmul_pa_impl<PA_GF2>::multiply(pa, mat1);
      break;
    }
    case PA_zz_p_tag: {
      BlockMatMul<PA_zz_p>& mat1= dynamic_cast< BlockMatMul<PA_zz_p>& >(mat);
      blockMatmul_pa_impl<PA_zz_p>::multiply(pa, mat1);
      break;
    }
    default:
      throw std::logic_error("blockMatMul: neither PA_GF2 nor PA_zz_p");
  }
}


/*=========================================================================*/
/*                                                                         */
/* @file            Optimizer.cpp                                          */
/* @author          Chirantan Ekbote (ekbote@seas.harvard.edu)             */
/* @date            2012/11/14                                             */
/* @version         0.3                                                    */
/* @brief           Optimizer for generating bright and dark images        */
/*                                                                         */
/*=========================================================================*/

#ifdef USE_OPENMP
#include <omp.h>
#endif
#include <Eigen/SVD>

#include "../inc/Optimizer.h"

#define ROW(n)	1*n,	2*n,	3*n,	4*n,	3*n,	2*n,	1*n

namespace pvrtex {
// Optimization window information
const int Optimizer::window_width_ = 11;
const int Optimizer::window_height_ = 11;
const int Optimizer::matrix_rows_ = 121;
const int Optimizer::matrix_cols_ = 8;
const int Optimizer::offset_x_ = 4;
const int Optimizer::offset_y_ = 4;

// Weight matrices
const float Optimizer::kTopLeft[] =
{
  ROW(0.0625f), 0, 0, 0, 0,	// 1/16
  ROW(0.125f), 0, 0, 0, 0,	// 2/16
  ROW(0.1875f), 0, 0, 0, 0,	// 3/16
  ROW(0.25f), 0, 0, 0, 0,		// 4/16
  ROW(0.1875f), 0, 0, 0, 0,	// 3/16
  ROW(0.125f), 0, 0, 0, 0,	// 2/16
  ROW(0.0625f), 0, 0, 0, 0,	// 1/16
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0
};
const float Optimizer::kTopRight[] =
{
  0, 0, 0, 0, ROW(0.0625f),	// 1/16
  0, 0, 0, 0, ROW(0.125f),	// 2/16
  0, 0, 0, 0, ROW(0.1875f),	// 3/16
  0, 0, 0, 0, ROW(0.25f),		// 4/16
  0, 0, 0, 0, ROW(0.1875f),	// 3/16
  0, 0, 0, 0, ROW(0.125f),	// 2/16
  0, 0, 0, 0, ROW(0.0625f),	// 1/16
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0
};
const float Optimizer::kBottomLeft[] =
{
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  ROW(0.0625f), 0, 0, 0, 0,	// 1/16
  ROW(0.125f), 0, 0, 0, 0,	// 2/16
  ROW(0.1875f), 0, 0, 0, 0,	// 3/16
  ROW(0.25f), 0, 0, 0, 0,		// 4/16
  ROW(0.1875f), 0, 0, 0, 0,	// 3/16
  ROW(0.125f), 0, 0, 0, 0,	// 2/16
  ROW(0.0625f), 0, 0, 0, 0,	// 1/16
};
const float Optimizer::kBottomRight[] =
{
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  ROW(0), 0, 0, 0, 0,
  0, 0, 0, 0, ROW(0.0625f),	// 1/16
  0, 0, 0, 0, ROW(0.125f),	// 2/16
  0, 0, 0, 0, ROW(0.1875f),	// 3/16
  0, 0, 0, 0, ROW(0.25f),		// 4/16
  0, 0, 0, 0, ROW(0.1875f),	// 3/16
  0, 0, 0, 0, ROW(0.125f),	// 2/16
  0, 0, 0, 0, ROW(0.0625f)	// 1/16
};

Optimizer::Optimizer(const Eigen::MatrixXi &o, Eigen::MatrixXi &d,
                     Eigen::MatrixXi &b, SOLVER s, util::DATA_FORMAT f) :

  dark_(d),
  bright_(b),
  orig_(o),
  solv_(s),
  format_(f)
{
  red_ = Eigen::MatrixXi(orig_.rows(), orig_.cols());
  green_ = Eigen::MatrixXi(orig_.rows(), orig_.cols());
  blue_ = Eigen::MatrixXi(orig_.rows(), orig_.cols());
}

Optimizer::~Optimizer() {
}

void Optimizer::ComputeUpdateVector() {
  Eigen::MatrixXi comp = util::ModulateImage(util::Upscale4x4(dark_, format_),
                                             util::Upscale4x4(bright_, format_),
                                             mod_);
#ifdef USE_OPENMP
#pragma omp parallel for
#endif
  for (int y = 0; y < orig_.rows(); ++y) {
    for (int x = 0; x < orig_.cols(); ++x) {
      Eigen::Vector3i diff;
      diff = (util::MakeColorVector(orig_(y,x), util::PVR888) -
              util::MakeColorVector(comp(y,x), util::PVR888));
      //        red_(y,x) = util::MakeRed(orig_(y,x));
      //        green_(y,x) = util::MakeGreen(orig_(y,x));
      //        blue_(y,x) = util::MakeBlue(orig_(y,x));
      red_(y,x) = diff(0);
      green_(y,x) = diff(1);
      blue_(y,x) = diff(2);
    }
  }
}

void Optimizer::OptimizeWindow(int j, int i) {
  Eigen::MatrixXf a(matrix_rows_, matrix_cols_);
  Eigen::MatrixXf w(matrix_rows_, matrix_cols_ / 2);
  Eigen::VectorXf red(matrix_rows_);
  Eigen::VectorXf green(matrix_rows_);
  Eigen::VectorXf blue(matrix_rows_);
  int idx, pixel_x, pixel_y;
  float m, distance;
  float r, g, b;

  /* Construct the optimization window */
  for (int y = 0; y < window_height_; ++y) {
    for (int x = 0; x < window_width_; ++x) {
      /* Get the position of the pixel we want to fetch */
      idx = y*window_width_ + x;
      pixel_x = util::Clamp(offset_x_*i-1 + x, 0, mod_.cols()-1);
      pixel_y = util::Clamp(offset_y_*j-1 + y, 0, mod_.rows()-1);

      /* Fetch the modulation value and the original color*/
      m = mod_(pixel_y, pixel_x);
      r = static_cast<float>(red_(pixel_y, pixel_x));
      g = static_cast<float>(green_(pixel_y, pixel_x));
      b = static_cast<float>(blue_(pixel_y, pixel_x));

      /* Fetch the distance weights and construct the matrix */
      distance = kTopLeft[idx];
      a(idx, 0) = distance * (1.0f - m);
      a(idx, 1) = distance * m;
      w(idx, 0) = distance;
      red(idx) = distance * r;
      green(idx) = distance * g;
      blue(idx) = distance * b;

      /* Top right pxel */
      distance = kTopRight[idx];
      a(idx, 2) = distance * (1.0f - m);
      a(idx, 3) = distance * m;
      w(idx, 1) = distance;
      red(idx) += distance * r;
      green(idx) += distance * g;
      blue(idx) += distance * b;

      /* Bottom left pixel */
      distance = kBottomLeft[idx];
      a(idx, 4) = distance * (1.0f - m);
      a(idx, 5) = distance * m;
      w(idx, 2) = distance;
      red(idx) += distance * r;
      green(idx) += distance * g;
      blue(idx) += distance * b;

      /* Bottom right pixel */
      distance = kBottomRight[idx];
      a(idx, 6) = distance * (1.0f - m);
      a(idx, 7) = distance * m;
      w(idx, 3) = distance;
      red(idx) += distance * r;
      green(idx) += distance * g;
      blue(idx) += distance * b;

    }
  }

  /* Solve for the best colors */
  Eigen::JacobiSVD<Eigen::MatrixXf> svd(a, Eigen::ComputeThinU |
                                        Eigen::ComputeThinV);
  Eigen::VectorXi optimal_red = svd.solve(red).cast<int>();
  Eigen::VectorXi optimal_green;
  Eigen::VectorXi optimal_blue;
  if (format_ == util::PVR444) {
    Eigen::JacobiSVD<Eigen::MatrixXf> svd_w(w, Eigen::ComputeThinU |
                                            Eigen::ComputeThinV);
    Eigen::Vector3i update;
    optimal_green = svd_w.solve(green).cast<int>();
    optimal_blue = svd_w.solve(blue).cast<int>();

    /* Update the dark and bright images */
    for (int x = 0; x < 2; ++x) {
      for (int y = 0; y < 2; ++y) {
        idx = 4*y + 2*x;
        update = Eigen::Vector3i(util::Clamp(optimal_red(idx), -32, 32),
                                 util::Clamp(optimal_green(idx/2), -32, 32),
                                 util::Clamp(optimal_blue(idx/2), -32, 32));
        dark_(j+y, i+x) = util::MakeRGB(
                              util::MakeColorVector(dark_(j+y, i+x), format_) +
                              update, format_);
        update(0) = util::Clamp(optimal_red(idx+1), -32, 32);
        bright_(j+y, i+x) = util::MakeRGB(
                                util::MakeColorVector(bright_(j+y, i+x), format_) +
                                    update, format_);

      }
    }
  } else {
    optimal_green = svd.solve(green).cast<int>();
    optimal_blue = svd.solve(blue).cast<int>();

    /* Update the dark and bright images */
    for (int x = 0; x < 2; ++x) {
      for (int y = 0; y < 2; ++y) {
        idx = 4*y + 2*x;
        dark_(j+y, i+x) = util::MakeRGB(
                              util::MakeColorVector(dark_(j+y, i+x), format_) +
                              Eigen::Vector3i(
                                  util::Clamp(optimal_red(idx), -32, 32),
                                  util::Clamp(optimal_green(idx), -32, 32),
                                  util::Clamp(optimal_blue(idx), -32, 32)),
                                        format_);
        bright_(j+y, i+x) = util::MakeRGB(
                                util::MakeColorVector(bright_(j+y, i+x), format_) +
                                Eigen::Vector3i(
                                    util::Clamp(optimal_red(idx+1), -32, 32),
                                    util::Clamp(optimal_green(idx+1), -32, 32),
                                    util::Clamp(optimal_blue(idx+1), -32, 32)),
                                          format_);
        //      dark_(j+y, i+x) = util::Make565RGB(Eigen::Vector3i(optimal_red(idx),
        //                                                         optimal_green(idx),
        //                                                         optimal_blue(idx)));
        //      bright_(j+y, i+x) = util::Make565RGB(Eigen::Vector3i(optimal_red(idx+1),
        //                                                           optimal_green(idx+1),
        //                                                           optimal_blue(idx+1)));

      }
    }

  }

}

void Optimizer::Optimize(const Eigen::MatrixXf &m) {
  mod_ = m;
  ComputeUpdateVector();
#ifdef USE_OPENMP
#pragma omp parallel for
#endif
  for (int j = 0; j < dark_.rows(); j+=2) {
    for (int i = 0; i < dark_.cols(); i+=2) {
      OptimizeWindow(j, i);
    }
  }
}

void Optimizer::WriteToFile(const char *filename) {

}

} /* namespace pvrtex */


#include "WSPRLog.hxx"
#include "SolarTime.hxx"
#include "TimeCorr.hxx"
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <list>
#include <math.h>
#include <set>
#include <vector>

// translate the timestamp field of each log entry into an INT
// representing the solar time in minutes (at a granularity of 15 min)

const int MINUTES_PER_BUCKET = 2;
const int BUCKETS_PER_TABLE = ((24 * 60) / MINUTES_PER_BUCKET);
const int BUCKETS_PER_HOUR = (60 / MINUTES_PER_BUCKET);

class myWSPRLog : public WSPRLog {
public:
  myWSPRLog(const std::string fname) : WSPRLog() {
    os.open(fname);
  }

  ~myWSPRLog() {
    os.close();
  }

  bool processEntry(WSPRLogEntry * ent) {
    if(ent == NULL) return false;

    // calculate solar time for rx, tx, and midpoint
    SolarTime tx_time(ent->dtime, ent->txgrid);
    SolarTime rx_time(ent->dtime, ent->rxgrid);
    float mid_hour = TimeCorr::circularMean(24.0, tx_time.getFHour(), rx_time.getFHour());

    ent->dtime = ((int) floor(mid_hour * 60.0)); // minutes past the hour

    ent->print(os);

    return false;
  }

private:
  std::ofstream os;
};

int main(int argc, char * argv[])
{
  bool input_gzipped;
  std::string in_name, out_name;
  namespace po = boost::program_options;

  po::options_description desc("Options:");

  desc.add_options()
    ("help", "help message")
    ("in", po::value<std::string>(&in_name)->required(), "WSPR log for baseline reports")
    ("out", po::value<std::string>(&out_name)->required(), "WSPR log for baseline reports");

  po::positional_options_description pos_opts ;
  pos_opts.add("in", 1);
  pos_opts.add("out", 1);

  po::variables_map vm;

  std::string what_am_i("Convert epoch timestamp to minutes past solar midnight\n");
  try {
    po::store(po::command_line_parser(argc, argv).options(desc)
	      .positional(pos_opts).run(), vm);

    if(vm.count("help")) {
      std::cout << what_am_i
		<< desc << std::endl;
      exit(-1);
    }

    po::notify(vm);
  }
  catch(po::required_option & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }
  catch(po::error & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }

  myWSPRLog wlog(out_name);

  wlog.readLog(in_name);
}


/*
 * Copyright 2017 MapD Technologies, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/**
 * @file		PopulateTableRandom.cpp
 * @author	Wei Hong <wei@map-d.com>
 * @brief		Populate a table with random data
 *
 * Copyright (c) 2014 MapD Technologies, Inc.  All rights reserved.
 **/

#include <iostream>
#include <string>
#include <cstring>
#include <cstdlib>
#include <cstdint>
#include <cfloat>
#include <exception>
#include <memory>
#include <random>
#include <boost/functional/hash.hpp>
#include "../Catalog/Catalog.h"
#include "../DataMgr/DataMgr.h"
#include "../Shared/sqltypes.h"
#include "../Fragmenter/Fragmenter.h"
#include "../Shared/measure.h"

using namespace std;
using namespace Catalog_Namespace;
using namespace Fragmenter_Namespace;

size_t random_fill_int16(int8_t* buf, size_t num_elems) {
  default_random_engine gen;
  uniform_int_distribution<int16_t> dist(INT16_MIN, INT16_MAX);
  int16_t* p = (int16_t*)buf;
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_int32(int8_t* buf, size_t num_elems) {
  default_random_engine gen;
  uniform_int_distribution<int32_t> dist(INT32_MIN, INT32_MAX);
  int32_t* p = (int32_t*)buf;
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_int64(int8_t* buf, size_t num_elems) {
  default_random_engine gen;
  uniform_int_distribution<int64_t> dist(INT64_MIN, INT64_MAX);
  int64_t* p = (int64_t*)buf;
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_float(int8_t* buf, size_t num_elems) {
  default_random_engine gen;
  uniform_real_distribution<float> dist(FLT_MIN, FLT_MAX);
  float* p = (float*)buf;
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_double(int8_t* buf, size_t num_elems) {
  default_random_engine gen;
  uniform_real_distribution<double> dist(DBL_MIN, DBL_MAX);
  double* p = (double*)buf;
  size_t hash = 0;
  for (size_t i = 0; i < num_elems; i++) {
    p[i] = dist(gen);
    boost::hash_combine(hash, p[i]);
  }
  return hash;
}

size_t random_fill_string(vector<string>& stringVec, size_t num_elems, int max_len, size_t& data_volumn) {
  string chars("abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ1234567890");
  default_random_engine gen;
  uniform_int_distribution<> char_dist(0, chars.size() - 1);
  uniform_int_distribution<> len_dist(0, max_len);
  size_t hash = 0;
  std::hash<std::string> string_hash;
  for (size_t n = 0; n < num_elems; n++) {
    int len = len_dist(gen);
    string s(len, ' ');
    for (int i = 0; i < len; i++)
      s[i] = chars[char_dist(gen)];
    // cout << "insert string: " << s << endl;
    stringVec[n] = s;
    boost::hash_combine(hash, string_hash(s));
    data_volumn += len;
  }
  return hash;
}

size_t random_fill_int8array(vector<vector<int8_t>>& stringVec, size_t num_elems, int max_len, size_t& data_volumn) {
  default_random_engine gen;
  uniform_int_distribution<int8_t> dist(INT8_MIN, INT8_MAX);
  uniform_int_distribution<> len_dist(0, max_len);
  size_t hash = 0;
  for (size_t n = 0; n < num_elems; n++) {
    int len = len_dist(gen);
    vector<int8_t> s(len);
    for (int i = 0; i < len; i++) {
      s[i] = dist(gen);
      boost::hash_combine(hash, s[i]);
    }
    stringVec[n] = s;
    data_volumn += len * sizeof(int8_t);
  }
  return hash;
}

size_t random_fill_int16array(vector<vector<int16_t>>& stringVec, size_t num_elems, int max_len, size_t& data_volumn) {
  default_random_engine gen;
  uniform_int_distribution<int16_t> dist(INT16_MIN, INT16_MAX);
  uniform_int_distribution<> len_dist(0, max_len / 2);
  size_t hash = 0;
  for (size_t n = 0; n < num_elems; n++) {
    int len = len_dist(gen);
    vector<int16_t> s(len);
    for (int i = 0; i < len; i++) {
      s[i] = dist(gen);
      boost::hash_combine(hash, s[i]);
    }
    stringVec[n] = s;
    data_volumn += len * sizeof(int16_t);
  }
  return hash;
}

size_t random_fill_int32array(vector<vector<int32_t>>& stringVec, size_t num_elems, int max_len, size_t& data_volumn) {
  default_random_engine gen;
  uniform_int_distribution<int32_t> dist(INT32_MIN, INT32_MAX);
  uniform_int_distribution<> len_dist(0, max_len / 4);
  size_t hash = 0;
  for (size_t n = 0; n < num_elems; n++) {
    int len = len_dist(gen);
    vector<int32_t> s(len);
    for (int i = 0; i < len; i++) {
      s[i] = dist(gen);
      boost::hash_combine(hash, s[i]);
    }
    stringVec[n] = s;
    data_volumn += len * sizeof(int32_t);
  }
  return hash;
}

#define MAX_TEXT_LEN 255

size_t random_fill(const ColumnDescriptor* cd, DataBlockPtr p, size_t num_elems, size_t& data_volumn) {
  size_t hash = 0;
  switch (cd->columnType.get_type()) {
    case kSMALLINT:
      hash = random_fill_int16(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(int16_t);
      break;
    case kINT:
      hash = random_fill_int32(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(int32_t);
      break;
    case kBIGINT:
    case kNUMERIC:
    case kDECIMAL:
      hash = random_fill_int64(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(int64_t);
      break;
    case kFLOAT:
      hash = random_fill_float(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(float);
      break;
    case kDOUBLE:
      hash = random_fill_double(p.numbersPtr, num_elems);
      data_volumn += num_elems * sizeof(double);
      break;
    case kVARCHAR:
    case kCHAR:
      if (cd->columnType.get_compression() == kENCODING_NONE)
        hash = random_fill_string(*p.stringsPtr, num_elems, cd->columnType.get_dimension(), data_volumn);
      break;
    case kTEXT:
      if (cd->columnType.get_compression() == kENCODING_NONE)
        hash = random_fill_string(*p.stringsPtr, num_elems, MAX_TEXT_LEN, data_volumn);
      break;
    case kTIME:
    case kTIMESTAMP:
      if (cd->columnType.get_dimension() == 0) {
        if (sizeof(time_t) == 4) {
          hash = random_fill_int32(p.numbersPtr, num_elems);
          data_volumn += num_elems * sizeof(int32_t);
        } else {
          hash = random_fill_int64(p.numbersPtr, num_elems);
          data_volumn += num_elems * sizeof(int64_t);
        }
      } else
        assert(false);  // not supported yet
      break;
    case kDATE:
      if (sizeof(time_t) == 4) {
        hash = random_fill_int32(p.numbersPtr, num_elems);
        data_volumn += num_elems * sizeof(int32_t);
      } else {
        hash = random_fill_int64(p.numbersPtr, num_elems);
        data_volumn += num_elems * sizeof(int64_t);
      }
      break;
    default:
      assert(false);
  }
  return hash;
}

vector<size_t> populate_table_random(const string& table_name, const size_t num_rows, const Catalog& cat) {
  const TableDescriptor* td = cat.getMetadataForTable(table_name);
  list<const ColumnDescriptor*> cds = cat.getAllColumnMetadataForTable(td->tableId, false, false, false);
  InsertData insert_data;
  insert_data.databaseId = cat.get_currentDB().dbId;
  insert_data.tableId = td->tableId;
  for (auto cd : cds) {
    insert_data.columnIds.push_back(cd->columnId);
  }
  insert_data.numRows = num_rows;
  vector<unique_ptr<int8_t>> gc_numbers;          // making sure input buffers get freed
  vector<unique_ptr<vector<string>>> gc_strings;  // making sure input vectors get freed
  vector<unique_ptr<vector<vector<int8_t>>>> gc_int8arrays;
  vector<unique_ptr<vector<vector<int16_t>>>> gc_int16arrays;
  vector<unique_ptr<vector<vector<int32_t>>>> gc_int32arrays;
  DataBlockPtr p{0};
  // now allocate space for insert data
  for (auto cd : cds) {
    if (cd->columnType.is_varlen()) {
      if (cd->columnType.get_compression() == kENCODING_NONE) {
        vector<string>* col_vec = new vector<string>(num_rows);
        gc_strings.push_back(unique_ptr<vector<string>>(col_vec));  // add to gc list
        p.stringsPtr = col_vec;
      }
    } else {
      int8_t* col_buf = static_cast<int8_t*>(malloc(num_rows * cd->columnType.get_size()));
      gc_numbers.push_back(unique_ptr<int8_t>(col_buf));  // add to gc list
      p.numbersPtr = col_buf;
    }
    insert_data.data.push_back(p);
  }

  // fill InsertData  with random data
  vector<size_t> col_hashs(cds.size());  // compute one hash per column for the generated data
  int i = 0;
  size_t data_volumn = 0;
  for (auto cd : cds) {
    col_hashs[i] = random_fill(cd, insert_data.data[i], num_rows, data_volumn);
    i++;
  }

  // now load the data into table
  auto ms = measure<>::execution([&]() { td->fragmenter->insertData(insert_data); });
  cout << "Loaded " << num_rows << " rows " << data_volumn << " bytes in " << ms << " ms. at "
       << (double)data_volumn / (ms / 1000.0) / 1e6 << " MB/sec." << std::endl;

  return col_hashs;
}


#ifndef PYTHONIC_INCLUDE_NUMPY_LOG10_HPP
#define PYTHONIC_INCLUDE_NUMPY_LOG10_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/log10.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    std::complex<T> log10(std::complex<T> const &val)
    {
      return std::log10(val);
    }
    template <class T>
    auto log10(T const &val) -> decltype(boost::simd::log10(val))
    {
      return boost::simd::log10(val);
    }
  }
#define NUMPY_NARY_FUNC_NAME log10
#define NUMPY_NARY_FUNC_SYM wrapper::log10
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif