#include "qbmove_multiple.hh"
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/common/PID.hh>
#include <stdio.h>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/math/gzmath.hh>
#include "position_stiffness_request.pb.h"

/* servo motor parameters */
#define J 0.001
#define b 0.01
#define Kt 0.8
#define Kb 1.3
#define R 2.3
#define L 4

//################################################################
/* matrixes used for state space representation */

#define Ts 0.001

double Ad[N][N] = { {1.0, 0.0008},
                    {0, 0.6299}
                  };

double Bd[N][U] = { {0.0001, 0.0004},
                    {0.2785, 0.8008}
                  };

double Cd[M][N] = { {1, 0} };

double Dd[M][U] = {{0, 0}};

//###############################################################
/* PID parameters in discrete domain */
#define Kp 0.001
#define Ki 0.0
#define Kd 0.008

using namespace gazebo;

int qbmovePlugin::stat_value = 1;

void qbmovePlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{

        		/* Store the pointer to the model */
        		this->model = _parent;

            /* Gazebo node used for communication */
            node = transport::NodePtr(new transport::Node());

            /* Initialize the node */
            node->Init();

            std::string model_name = this->model->GetName();

            std::string string_plugin = "model_qbmove_" + boost::to_string(stat_value);

            this->num_qbmove = stat_value;

            std::string joint_name = "";
            std::string submodel_name = "";

            submodel_name = "qbmove_" + boost::to_string(this->num_qbmove);

            joint_name = submodel_name + "::output_shaft_joint";

            this->joint = this->model->GetJoint(joint_name);

            joint_name = submodel_name + "::motor1_joint";

            this->joint_motor1 = this->model->GetJoint(joint_name);

            joint_name = submodel_name + "::motor2_joint";

            this->joint_motor2 = this->model->GetJoint(joint_name);

            /* strings used to define stiffness and position topics for Gazebo commands */
            std::string command_topic =  submodel_name + "/position_stiffness/command";

            std::string echo_topic =   submodel_name + "/pos_current/echo";

/*******/
            std::cout << command_topic << "\n";
            std::cout << echo_topic << "\n";

/********/
            /* target angle for motor 1 in radians */
        		this->q1_ref = 0.0;

            this->old_q1_ref = 0.0;

            this->old_value_position_1 = 0.0;

            /* target angle for motor 2 in radians */
            this->q2_ref = 0.0;

            this->old_q2_ref = 0.0;

            this->old_value_position_2 = 0.0;

            this->value_k_1 = 0.0;
            this->value2_k_1 = 0.0;

            this->tau_ext = 0.0;
            this->tau2_ext = 0.0;

            this->e_k_2 = 0.0;
            this->e_k_3 = 0.0;

            this->e2_k_2 = 0.0;
            this->e2_k_3 = 0.0;

            /* vectors initialization */
            /* state initialization */
            for(int i=0; i<N; i++)
            {
                  x1_k[i] = 0.0;
                  x2_k[i] = 0.0;
            }
            /* output initialization */
            for(int i=0; i<M; i++)
            {
                    y1_k[i] = 0.0;
                    y2_k[i] = 0.0;
            }
            /* input initialization */
            for(int i=0; i<U; i++)
            {
                    u1_k[i] = 0.0;
                    u2_k[i] = 0.0;
            }

            /* stiffness value initialization */
            this->val_stiffness = 0.0;

            /* position value initialization */
            this->val_position = 0.0;

            /* subscriber initialization */
            sub = node->Subscribe(command_topic, &qbmovePlugin::stiff_pos_Callback, this);

            /* publisher initialization */
            pub = node->Advertise<pos_current_echo_creator_msgs::msgs::PosCurrentEchoRequest>(echo_topic);

            /* static variable increase */
            stat_value++;

            /* Listen to the update event. This event is broadcast every simulation iteration */
            this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&qbmovePlugin::OnUpdate, this, _1));

}

void qbmovePlugin::stiff_pos_Callback(PositionStiffnessRequestPtr &msg)
{
             /* position and stiffness update */
          	 this->val_position = msg->position()*(3.14/180.0);
          	 this->val_stiffness = msg->stiffness()*(3.14/180.0);

}

void qbmovePlugin::ref_generation(double q_e, double q_d)
{
             /* q_e: link equilibrium reference */
             /* q_d: stiffness preset reference */

             /* saturation limit for reference values */
             double limit_value = 16434;
             /* conversion value from radians to ticks */
             double conversion_value = 65536.0/(4.0*3.14);

             /* q1_ref and q2_ref old values */
             this->old_q1_ref = this->q1_ref;
             this->old_q2_ref = this->q2_ref;

             /* conversion from radians to ticks */
             this->q_e = q_e * conversion_value ;
             this->q_d = q_d * conversion_value ;

             /* reference values for servomotors  */
             this->q1_ref = this->q_e + this->q_d;
      	     this->q2_ref = this->q_e - this->q_d;

             /* check if q1 reference value is over saturation limit*/
        	   if( this->q1_ref < - limit_value)
        	   {
        	         this->q1_ref = - limit_value;
        	   }

        	   if( this->q1_ref > limit_value)
        	   {
        	         this->q1_ref = limit_value;
        	   }

      	     this->q1_ref = 2*(this->q1_ref);

             /* check if q2 reference value is over saturation limit*/
      	     if(this->q2_ref < - limit_value)
      	     {
      	         this->q2_ref = - limit_value;
      	     }

      	     if( this->q2_ref > limit_value)
      	     {
      	         this->q2_ref = limit_value;
      	     }

      	     this->q2_ref = 2*(this->q2_ref);

}

double qbmovePlugin::pwm_modulation(double u_pid)
{
              /* PWM parameters */
      	      double dead_zone = 0.3;
              /* normalization factor after PWM modulation */
      	      double norm_factor = 1./(1. - 0.3);
              /* motor voltage input */
      	      double Vcc = 8;
              /* command saturation limit */
              int saturation_limit = 1.0;
              /* command given to motor after PWM modulation */
      	      double u_pwm = 0.0;

              /* command saturation */
              if( u_pid > saturation_limit )
            	{
            		   u_pwm = saturation_limit;
            	}

      	      if( u_pid < - saturation_limit )
      	      {
      		         u_pwm = - saturation_limit;
      	      }

              /* dead zone */
      	      if( u_pid > dead_zone )
      	      {
      		         u_pwm = u_pwm - dead_zone;
      	      }

      	      if( u_pid < - dead_zone )
      	      {
      		         u_pwm = u_pwm + dead_zone;
      	      }

      	      if(( u_pid >= - dead_zone) && ( u_pid <= dead_zone))
      	      {
      		         u_pwm = 0.0;
      	      }

      		    /* normalization factor */
      	      u_pwm = u_pwm * norm_factor;
      		    /* output value multiplied for tension value */
      	      u_pwm = u_pwm * Vcc;

              return u_pwm;

}

void qbmovePlugin::system_update(double* x, double* y, double u1, double u2)
{

              /* each motor is simulated as a state-space system */
              /* system update */

              /* output update */
              for(int i=0; i<M; i++)
      	      {
            		    y[i] = 0.0;

            		    for(int k=0; k<N; k++)
            		    {
            		    	   y[i]  += Cd[i][k]*x[k];
            		    }

      	       }

              /* state update */
              for(int i=0; i<N; i++)
      	      {
            		    x1[i] = 0.0;

            		    for(int j=0; j<N; j++)
            		    {
            		    	 x1[i] += Ad[i][j]*x[j];
            		    }
      		    }

          		for(int i=0; i<N; i++)
          		{
          		    x2[i] = Bd[i][0]*u1 + Bd[i][1]*u2 ;
          		 }

          		 for(int i=0; i<N; i++)
          		 {
          		    	x[i] = x1[i] + x2[i];
          		 }

}

double qbmovePlugin::friction_torque_computation(double position, int index)
{
                /* index : 1 for motor 1, 2 for motor 2 , 3 for the output shaft */
                /* variables names are the same of Simulink scheme */

                /* friction torque parameters */
                double friction_torque = 0.0;
      	        double z_max = 0.03;
      	        double static_friction_torque = 0.0;
                double sign1 = 0.0;
                /* output value from Switch1 block in the Simulink scheme */
      	        double switch1_out = 0.0;
                /* output value from Switch block in the Simulink scheme */
      	        double switch_out = 0.0;
                double old_value = 0.0;

                switch(index)
                {
                     case 1:
                            old_value = this->value_k_1;
                            static_friction_torque = 0.8;
                            break;

                     case 2:
                            old_value = this->value2_k_1;
                            static_friction_torque = 0.8;
                            break;

                     case 3:
                            old_value = this->value_k_1_output_shaft;
                            static_friction_torque = 0.01;
                            break;

                     default: break;

                }

                sign1 = ( position - old_value )/z_max;

		            if(sign1 >= -1)
                {
            	      switch1_out = old_value;
                }
                else
		           {
		               switch1_out = position + z_max;
	              }

            		if(sign1 > 1)
            		{
            		     switch_out = position  - z_max;
            		}
            	        else
            		{
            		     switch_out = switch1_out;
            		}

            		if(sign1 > 1)
            		{
            		     sign1 = 1;
            		}

            		if(sign1 < -1)
            		{
            		     sign1 = -1;
            		}

                switch(index)
                {
                      case 1:
		                     this->value_k_1 = switch_out;
                         break;

                      case 2:
                         this->value2_k_1 = switch_out;
                         break;

                      case 3:
                         this->value_k_1_output_shaft = switch_out;
                         break;

                      default: break;
                }

	            friction_torque = sign1 * static_friction_torque;

              return friction_torque;

}

void qbmovePlugin::OnUpdate(const common::UpdateInfo & /*_info*/)
{

              /* command after PWM modulation */
  	          double u_pwm = 0.0;

              /* deformation angle */
              double def_angle = 0.0;

              /* friction torque parameter */
  	          double friction_torque = 0.0;

  	          /* parameters used to compute motor 1 non linear spring torque  */
  	          double k1 = 0.022;
  	          double a1 = 6.85;

              /* parameters used to compute motor 2 non linear spring torque */
              double k2 = 0.022;
              double a2 = 6.85;

              /* descrete PID parameters */
              double P = Kp;
              double I = Ki;
              double D = Kd;

              /* motor 1 error at k-th step */
              double e_k;
              /* motor 2 error at k-th step */
              double e2_k;

              /* factor conversion used for magnetic sensors */
              double conversion_fact_2 = 65536./(2.*3.14);

              /* output value from magnetic sensor on motor 1*/
              double magn_sensor_output_q1 = 0.0;

              /* output value from magnetic sensor on motor 2*/
              double magn_sensor_output_q2 = 0.0;

              /* non linear spring torque on motor 1*/
              double torque1 = 0.0;

              /* non linear spring torque on motor 2*/
              double torque2 = 0.0;

              /* motor 1 current */
              double curr_mot_1 = 0.0;

              /* motor 2 current */
              double curr_mot_2 = 0.0;

              /* voltage input to motors */
              double Vcc = 8.0;

              /* output shaft angle */
              this->output_shaft_angle = this->joint->GetAngle(0).Radian();

              /* q1_ref e q2_ref are reference values for motor 1 and motor 2, respectively */
              /* q1_ref and q2_ref update */
              ref_generation(this->val_position, this->val_stiffness );

              /***************************************************************/
              /***                   MOTOR 1 DYNAMICS                        */
              /***************************************************************/

              /* motor 2 angle is sensed by a magnetic sensor */
              /* value considered is the one obtained from the previous step */
              magn_sensor_output_q1 = this->old_value_position_1 * conversion_fact_2;

              /* error in ticks */
              /* in realtà questo è l'errore al passo precedente */
              e_k  = this->old_q1_ref - magn_sensor_output_q1 ;

              /* PID command update */
              u1_k[0] = u1_k[0] + P*( e_k - this->e_k_2) + I*e_k + D*( e_k - 2*this->e_k_2 + this->e_k_3 );

              /* error values update */
	            this->e_k_3 = this->e_k_2;
	            this->e_k_2 = e_k;

              /* PWM computation */
              u_pwm = pwm_modulation( u1_k[0]);

              /* second input is the extern torque */
              u1_k[1] = this->tau_ext;

              /* old angular position used for error computation*/
              this->old_value_position_1 = y1_k[0];

              /* output and state system update */
              system_update(x1_k, y1_k, u_pwm, u1_k[1]);

              /* friction torque computation */
              friction_torque =  friction_torque_computation( y1_k[0] , 1);

              /* deformation angle between output shaft and motor 1 */
              def_angle = this->output_shaft_angle - y1_k[0];

              /* non linear spring torque computation */
              torque1 = k1*(sinh(a1*def_angle));

              /* motor 1 torque update */
              this->tau_ext =  torque1 - friction_torque;

              /***************************************************************/
              /***                   MOTOR 2 DYNAMICS                        */
              /***************************************************************/

              /* motor 2 angle is sensed by a magnetic sensor */
              /*  value considered is the one obtained from the previous step */
              magn_sensor_output_q2 = this->old_value_position_2 * conversion_fact_2;

              /* error in ticks */
              /* error computed for the previous step */
              e2_k  = this->old_q2_ref - magn_sensor_output_q2 ;

              /* PID command update */
              u2_k[0] = u2_k[0] + P*( e2_k - this->e2_k_2 ) + I*e2_k + D*( e2_k - 2*this->e2_k_2 + this->e2_k_3 );

              /* error values update */
	            this->e2_k_3 = this->e2_k_2;
	            this->e2_k_2 = e2_k;

              /* PWM modulation */
              u_pwm = pwm_modulation( u2_k[0] );

              /* second input is the extern torque */
              u2_k[1] = this->tau2_ext;

              /* old angular position used for error computation */
              this->old_value_position_2 = y2_k[0];

              /* output and state system update */
              system_update(x2_k, y2_k, u_pwm, u2_k[1] );

              /* friction torque computation */
              friction_torque =  friction_torque_computation( y2_k[0], 2 );

              /* deformation angle computation between output shaft and motor 2 */
              def_angle = this->output_shaft_angle - y2_k[0];

              /* non linear spring torque computation */
              torque2 = k2*(sinh(a2*def_angle));

              /* motor 2 torque update */
              this->tau2_ext =  torque2 - friction_torque;

              /***************************************************************/
              /***           OUTPUT SHAFT TORQUE COMPUTATION                 */
              /***************************************************************/

              /* friction torque computation */
              friction_torque =  friction_torque_computation( this->output_shaft_angle , 3);

              /* tau_L (output shaft torque) computation */
              this->tau_L = - torque1 - torque2 - friction_torque;

              /* output shaft torque update */
              this->joint->SetForce( 0, this->tau_L);

              /* motor 1 position update */
              this->a.SetFromRadian(y1_k[0]);
              this->joint_motor1->SetAngle(0, this->a);

              /* motor 2 position update */
              this->a.SetFromRadian(y2_k[0]);
              this->joint_motor2->SetAngle(0, this->a);

              /* motor 1 current computation */
              curr_mot_1 = Vcc/R - (Kb/R)*x1_k[1];

              /* motor 2 current computation */
              curr_mot_2 = Vcc/R - (Kb/R)*x2_k[1];

              /* echo values */
              /* 1st value: output shaft position */
              msg_echo.set_pos_out_shaft(this->output_shaft_angle);
              /* 2nd value: motor 1 position */
              msg_echo.set_pos_mot_1(y1_k[0]);
              /* 3rd value: motor 2 position */
              msg_echo.set_pos_mot_2(y2_k[0]);
              /* 4th value: motor 1 current */
              msg_echo.set_curr_mot_1(curr_mot_1);
              /* 5th value: motor 2 current */
              msg_echo.set_curr_mot_2(curr_mot_2);

              /* message publication in the Gazebo topic */
              pub->Publish(msg_echo);

}


/* Copyright (C) 2011 Ion Torrent Systems, Inc. All Rights Reserved */
#include <cassert>
#include <cmath>
#include <algorithm>
#include <deque>
#include <functional>
#include <iostream>
#include <iomanip>
#include <numeric>
#include <set>
#include <string>
#include <vector>
#include <fenv.h>
#include <armadillo>
#include "Mask.h"
#include "RawWells.h"
#include "mixed.h"
#include "bivariate_gaussian.h"
#include "armadillo_utils.h"
#include "MaskSample.h"
#include "BeadTracker.h"

using namespace std;
using namespace arma;

typedef pair<int,int>   well_coord;
typedef set<well_coord> well_set;

static void count_sample(filter_counts& counts, deque<float>& ppf, deque<float>& ssq, Mask& mask, RawWells& wells, const vector<int>& key_ionogram, const PolyclonalFilterOpts & opts);
well_set sample_lib(Mask& mask, int nsamp);
void calcMeanCovariance(mat new_sgma[2], vec new_mean[2], vec &new_alpha, const vec mean[2], const mat sgma[2], const vec& alpha, const deque<float>& ppf, const deque<float>& ssq, int option);

void make_filter(clonal_filter& filter, filter_counts& counts, Mask& mask, RawWells& wells, const vector<int>& key_ionogram, const PolyclonalFilterOpts & opts)
{
    // Make a clonality filter from a sample of reads from a RawWells file.
    // Record number of reads in sample that are caught by each filter.
    deque<float>  ppf;
    deque<float>  ssq;
    count_sample(counts, ppf, ssq, mask, wells, key_ionogram, opts);
    make_filter(filter, counts, ppf, ssq, opts);
}

void make_filter(clonal_filter& filter, filter_counts& counts, const deque<float>& ppf, const deque<float>& ssq, const PolyclonalFilterOpts & opts)
{
    // Make a clonality filter from ppf and ssq for a sample of reads.
    // Record number of putative clonal and mixed reads in the sample.
    vec  mean[2];
    mat  sigma[2];
    vec  alpha;
    bool converged = fit_normals(mean, sigma, alpha, ppf, ssq, opts);

    if(converged){
        bivariate_gaussian clonal(mean[0], sigma[0]);
        bivariate_gaussian mixed( mean[1], sigma[1]);
        filter = clonal_filter(clonal, mixed, mixed_ppf_cutoff(), converged);
    }

    if(converged){
        deque<float>::const_iterator p = ppf.begin();
        for(deque<float>::const_iterator s=ssq.begin(); s!=ssq.end(); ++p, ++s){
            if(filter.is_clonal(*p,*s, opts.mixed_stringency))
                ++counts._nclonal;
        }
        counts._nmixed = ppf.size() - counts._nclonal;
    }
}

static void count_sample(filter_counts& counts, deque<float>& ppf, deque<float>& ssq, Mask& mask, RawWells& wells, const vector<int>& key_ionogram, const PolyclonalFilterOpts & opts)
{
    // Take sample of reads from a RawWells file, and apply some simple
    // filters to identify problem reads.
    // Record number of reads in sample, and number of reads caught by
    // each filter.
    well_set sample = sample_lib(mask, counts._nsamp);
    WellData data;
    unsigned int nflows = wells.NumFlows();
    vector<float> nrm(nflows);
    int flow0 = opts.mixed_first_flow;
    int flow1 = opts.mixed_last_flow;
    wells.ResetCurrentRegionWell();

    // Some temporary code for comparing clonal filter in background model:
    ofstream out("basecaller_ppf_ssq.txt");
    assert(out);

    while(!wells.ReadNextRegionData(&data)){
        // Skip if this is not in the sample:
        well_coord wc(data.y, data.x);
        if(sample.find(wc) == sample.end())
            continue;

        // Skip wells with infinite signal:
        bool finite = all_finite(data.flowValues, data.flowValues+nflows);
        if(not finite){
            ++counts._ninf;
            continue;
        }

        // Key-normalize:
        float normalizer = ComputeNormalizerKeyFlows(data.flowValues, &key_ionogram[0], key_ionogram.size());
        transform(data.flowValues, data.flowValues+nflows, nrm.begin(), bind2nd(divides<float>(),normalizer));

        // Skip wells with bad key:
        bool good_key = key_is_good(nrm.begin(), key_ionogram.begin(), key_ionogram.end());
        if(not good_key){
            ++counts._nbad_key;
            continue;
        }

        // Skip possible super-mixed beads:
        float perc_pos = percent_positive(nrm.begin()+flow0, nrm.begin()+flow1);;
        if(perc_pos > mixed_ppf_cutoff()){
            ++counts._nsuper;
            continue;
        }

        // Record ppf and ssq:
        float sum_frac = sum_fractional_part(nrm.begin()+flow0, nrm.begin()+flow1);
        ppf.push_back(perc_pos);
        ssq.push_back(sum_frac);

        // Some temporary code for comparing clonal filter in background model:
        out << setw(6) << data.y
            << setw(6) << data.x
            << setw(8) << setprecision(2) << fixed << perc_pos
            << setw(8) << setprecision(2) << fixed << sum_frac
            << setw(8) << setprecision(2) << fixed << normalizer
            << endl;
    }
    assert(ppf.size() == ssq.size());
}

well_set sample_lib(Mask& mask, int nsamp)
{
    // Return a random sample of wells with library beads.
    MaskSample<uint32_t> lib_sample(mask, MaskLib, nsamp);

    well_set sample;
    int chip_width = mask.W();
    for(vector<uint32_t>::iterator i=lib_sample.Sample().begin(); i!=lib_sample.Sample().end(); ++i){
        int row = *i / chip_width;
        int col = *i % chip_width;
        sample.insert(make_pair(row,col));
    }

    return sample;
}

inline double square(double x)
{
    return x * x;
}

static bool test_convergence(const vec mean[2], const mat sgma[2], const vec& alpha, const vec new_mean[2], const mat new_sgma[2], const vec& new_alpha)
{
    double eps        = 1e-4;
    double mean_diff0  = max(max(new_mean[0] - mean[0]));
    double mean_diff1  = max(max(new_mean[1] - mean[1]));
    double sgma_diff0  = max(max(new_sgma[0]    - sgma[0]));
    double sgma_diff1  = max(max(new_sgma[1]    - sgma[1]));
    double sgma_diff   = max(sgma_diff0, sgma_diff1);
    double alpha_diff  = max(max(new_alpha   - alpha));
    double max_diff    = max(max(mean_diff0, mean_diff1), max(sgma_diff, alpha_diff));

    return max_diff < eps;
}

static void print_dist(vec mean[2], mat sgma[2], vec& alpha, bool converged, int iter, int option)
{
    cout << "Clonal Filter: fit_normals with option " << option << " at iteration" << setw(4) << iter << endl;
    cout << "convergence status: " << boolalpha << converged << endl;
    cout << "Mean of first cluster:" << endl;
    cout << mean[0] << endl;
    cout << "Mean of mixed cluster:"<< endl;
    cout << mean[1] << endl;
    cout << "Covariance of clonal cluster:" <<endl;
    cout << sgma[0] << endl;
    cout << "Covariance of mixed cluster:" <<endl;
    cout << sgma[1] << endl;
    cout << "fraction clonal vs mixed: " << alpha[0] << " & " << alpha[1] << endl;
    cout << endl;
}

static void init(vec mean[2], mat sgma[2], vec& alpha)
{
    for (int i = 0; i < 2; i++) {
        mean[i].set_size(2);
        sgma[i].set_size(2,2);
    }

    alpha.set_size(2);
    // Following intializations are based on average over largne number of runs.
    // Average parameters for clonal population:
    mean[0] << 0.48811  << 1.61692;
    sgma[0] << 0.004836 << 0.032828 << endr
            << 0.032828 << 0.548305 << endr;

    // Average parameters for mixed population:
    mean[1] << 0.693371 << 4.397405;
    sgma[1] = sgma[0];

    // Start by assuming that clonal and mixed populations are equinumerous:
    alpha.fill(0.5);
}

bool fit_normals(vec mean[2], mat sgma[2], vec& alpha, const deque<float>& ppf, const deque<float>& ssq, const PolyclonalFilterOpts & opts)
{
    bool converged = false;

    try {
        // Initial guesses for two normal distributions:
        init(mean, sgma, alpha);
        if (opts.verbose)
          print_dist(mean, sgma, alpha, converged, 0, opts.mixed_model_option);  // XXX

        // increased stablity of iterative method:
        // Add two points to the ppf, ssp deqeue corresponding to the a-priori cluster centers
        std::deque<float> my_ppf(ppf);
        my_ppf.push_front(mean[0][0]);
        my_ppf.push_front(mean[1][0]);
        std::deque<float> my_ssq(ssq);
        my_ssq.push_front(mean[0][1]);
        my_ssq.push_front(mean[1][1]);

        int  max_iters = opts.max_iterations;
        int iteration = 1;
        cout << "max_iters: " << max_iters << endl;
        bool not_pos_def = false;
        bool not_finite_params = false;
        for(; iteration<=max_iters and not converged; ++iteration){

            vec  new_mean[2];
            mat  new_sgma[2];
            vec  new_alpha;

            calcMeanCovariance(new_sgma, new_mean, new_alpha, mean, sgma, alpha, my_ppf, my_ssq, opts.mixed_model_option);

            if(not is_pos_def(sgma[0]) or not is_pos_def(sgma[1])) {
                not_pos_def = true;
                break;
            }

            // Test for convergence:
            if(not new_mean[0].is_finite() or not new_mean[1].is_finite() or not new_sgma[0].is_finite() or not new_sgma[1].is_finite() or not new_alpha.is_finite()) {
                not_finite_params = true;
                break;
            }

            converged = test_convergence(mean, sgma, alpha, new_mean, new_sgma, new_alpha);//ignore new_sigma2 comparison

            // Update parameters, forcing covariances to be the same for both distributions:
            alpha   = new_alpha;
            mean[0] = new_mean[0];
            mean[1] = new_mean[1];
            sgma[0] = new_sgma[0];
            sgma[1] = new_sgma[1];

            if (opts.verbose)
              print_dist(mean, sgma, alpha, converged, iteration, opts.mixed_model_option);
        }

        // Fallback position if failed to converge:
        if(not converged){
            // in case of singular covariance matrices on infinite means, don't use the mean and covariances at the last iteration
            if (not_pos_def || not_finite_params) {
              cout << "failed to converge to an acceptable filter with improper covariance and mean parameters for the clusters: default filtering used" << endl;
              init(mean, sgma, alpha);
            }
            else {
              cout << "failed to converge to an acceptable filter in " << max_iters << " iterations";
              if (opts.use_last_iter_params) {
                cout << ": using last iteration params for filtering" << endl;
              }
              else {
                cout << ": default filtering used" << endl;
                init(mean, sgma, alpha);
              }
            }
            converged = true;
        } else {
          cout << "converged to acceptable filter: using adapted filter at iteration " << (iteration-1) << endl;
        }
    }catch(const exception& ex){
        converged = false;
        cerr << "exception thrown during fit_normals()" << endl;
        cerr << ex.what() << endl;
    }catch(...){
        converged = false;
        cerr << "unknown exception thrown during fit_normals()" << endl;
    }

    return converged;
}

void calcMeanCovariance(mat new_sgma[2], vec new_mean[2], vec &new_alpha, const vec mean[2], const mat sgma[2], const vec& alpha, const deque<float>& ppf, const deque<float>& ssq, int option)
{
  switch(option){
    case 0:{//common covariance
        bivariate_gaussian clone_dist(mean[0], sgma[0]);
        bivariate_gaussian mixed_dist(mean[1], sgma[1]);

        // Re-estimate parameters for each distribution:
        int nsamp = ppf.size();
        vec sumw(2);
        sumw.fill(0.5);

        mat sum2(2,2);
        sum2.fill(0.0);

        vec   sum1[2];
        sum1[0].set_size(2);
        sum1[1].set_size(2);
        sum1[0].fill(0.0);
        sum1[1].fill(0.0);

        // Accumulate weighted sums for re-estimating moments:
        int savedExceptionFlags = fegetexcept();
        fedisableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW | FE_INEXACT | FE_UNDERFLOW); //possible division by zero in probability normalization for outliers
        for(int j=0; j<nsamp; ++j){
            // Skip reads outside the poisson range for ppf:
            if(mixed_ppf_cutoff() < ppf[j])
                continue;

            // Each read gets two weights, reflecting the likelyhoods of that
            // read being clonal or mixed.
            vec x(2);
            x << ppf[j] << ssq[j];
            vec q(2);
            q[0] = alpha[0] * clone_dist.pdf(x);
            q[1] = alpha[1] * mixed_dist.pdf(x);

            vec w = q / sum(q);

            // Skip outliers:
            if( not w.is_finite() )
              continue;

            // Running sums for moments are weighted:
            sumw         += w;
            sum1[0]      += w[0] * x;
            sum1[1]      += w[1] * x;
            sum2.at(0,0) += w[0] * square(ppf[j] - mean[0][0]);
            sum2.at(0,1) += w[0] * (ppf[j] - mean[0][0]) * (ssq[j] - mean[0][1]);
            sum2.at(1,1) += w[0] * square(ssq[j] - mean[0][1]);
        }
        feenableexcept(savedExceptionFlags);

        // New means:
        for(int j=0; j<2; ++j) {
            new_mean[j].set_size(2);
            new_mean[j] = sum1[j] / sumw[j];
        }

        // New covariance:
        mat new_sgma1(2,2);
        new_sgma1.at(0,0) = sum2.at(0,0) / sumw[0];
        new_sgma1.at(0,1) = sum2.at(0,1) / sumw[0];
        new_sgma1.at(1,0) = new_sgma1.at(0,1);
        new_sgma1.at(1,1) = sum2.at(1,1) / sumw[0];

        new_sgma[0] = new_sgma1;
        new_sgma[1] = new_sgma1;

        // New prior:
        new_alpha = sumw / nsamp;
    }
        break;
    case 7: {
        //same volume and shape, different orientation: lambda*D_k*A*(D_k)'
      bivariate_gaussian clone_dist(mean[0], sgma[0]);
      bivariate_gaussian mixed_dist(mean[1], sgma[1]);
      // Re-estimate parameters for each distribution:
      int nsamp = ppf.size();
      vec sumw(2);
      sumw.fill(0.0);

      mat sum2(2,2);
      sum2.fill(0.0);

      mat sum3(2,2);
      sum3.fill(0.0);

      vec   sum1[2];
      sum1[0].set_size(2);
      sum1[1].set_size(2);
      sum1[0].fill(0.0);
      sum1[1].fill(0.0);

      // Accumulate weighted sums for re-estimating moments:
      for(int j=0; j<nsamp; ++j){
          // Skip reads outside the poisson range for ppf:
          if(mixed_ppf_cutoff() < ppf[j])
              continue;

          // Each read gets two weights, reflecting the likelyhoods of that
          // read being clonal or mixed.
          vec x(2);
          x << ppf[j] << ssq[j];
          vec q(2);
          q[0] = alpha[0] * clone_dist.pdf(x);
          q[1] = alpha[1] * mixed_dist.pdf(x);
          vec w = q / sum(q);

          // Skip outliers:
          if(not w.is_finite())
              continue;

          // Running sums for moments are weighted:
          sumw         += w;
          sum1[0]      += w[0] * x;
          sum1[1]      += w[1] * x;
          sum2.at(0,0) += w[0] * square(ppf[j] - mean[0][0]);
          sum2.at(0,1) += w[0] * (ppf[j] - mean[0][0]) * (ssq[j] - mean[0][1]);

          sum2.at(1,1) += w[0] * square(ssq[j] - mean[0][1]);
          sum3.at(0,0) += w[1] * square(ppf[j] - mean[1][0]);
          sum3.at(0,1) += w[1] * (ppf[j] - mean[1][0]) * (ssq[j] - mean[1][1]);

          sum3.at(1,1) += w[1] * square(ssq[j] - mean[1][1]);
      }
      sum2.at(1,0) =  sum2.at(0,1);
      sum3.at(1,0) =  sum3.at(0,1);

      // New means:
      for(int j=0; j<2; ++j) {
          new_mean[j].set_size(2);
          new_mean[j] = sum1[j] / sumw[j];
      }

      //calculate eigen values and vectors for sum2 and sum3
      vec eigval_clonal;
      mat eigvec_clonal;
      eig_sym(eigval_clonal, eigvec_clonal, sum2);

      vec eigval_mixed;
      mat eigvec_mixed;
      eig_sym(eigval_mixed, eigvec_mixed, sum3);

      //maintain same ascending order between clonal and mixed: ascending order by default and should be no op
      //should we bail out if eigenvalue is complex
      if(eigval_clonal.at(0) > eigval_clonal.at(1)){
        double temp = eigval_clonal.at(0);
        eigval_clonal.at(0) = eigval_clonal.at(1);
        eigval_clonal.at(1) = temp;
        temp = eigvec_clonal.at(0, 0);
        eigvec_clonal.at(0, 0) = eigvec_clonal.at(0, 1);
        eigvec_clonal.at(0, 1) = temp;
        temp = eigvec_clonal.at(1, 0);
        eigvec_clonal.at(1, 0) = eigvec_clonal.at(1, 1);
        eigvec_clonal.at(1, 1) = temp;
      }
      if(eigval_mixed.at(0) > eigval_mixed.at(1)){
        double temp = eigval_mixed.at(0);
        eigval_mixed.at(0) = eigval_mixed.at(1);
        eigval_mixed.at(1) = temp;
        temp = eigvec_mixed.at(0, 0);
        eigvec_mixed.at(0, 0) = eigvec_mixed.at(0, 1);
        eigvec_mixed.at(0, 1) = temp;
        temp = eigvec_mixed.at(1, 0);
        eigvec_mixed.at(1, 0) = eigvec_mixed.at(1, 1);
        eigvec_mixed.at(1, 1) = temp;
      }
      //average out the eigenvalues
      vec eigval_sum = eigval_clonal + eigval_mixed;
      //calculate the new covariance
      new_sgma[0] = eigvec_clonal * diagmat(eigval_sum) * trans(eigvec_clonal) / (sumw[0] + sumw[1]);
      new_sgma[1] = eigvec_mixed * diagmat(eigval_sum) * trans(eigvec_mixed) / (sumw[0] + sumw[1]);

      // New prior:
      new_alpha = sumw / nsamp;

    }
        break;

    case 8:{//common volume
        bivariate_gaussian clone_dist(mean[0], sgma[0]);
        bivariate_gaussian mixed_dist(mean[1], sgma[1]);
        // Re-estimate parameters for each distribution:
        int nsamp = ppf.size();
        vec sumw(2);
        sumw.fill(0.0);

        mat sum2(2,2);
        sum2.fill(0.0);

        mat sum3(2,2);
        sum3.fill(0.0);

        vec   sum1[2];
        sum1[0].set_size(2);
        sum1[1].set_size(2);
        sum1[0].fill(0.0);
        sum1[1].fill(0.0);

        // Accumulate weighted sums for re-estimating moments:
        for(int j=0; j<nsamp; ++j){
            // Skip reads outside the poisson range for ppf:
            if(mixed_ppf_cutoff() < ppf[j])
                continue;

            // Each read gets two weights, reflecting the likelyhoods of that
            // read being clonal or mixed.
            vec x(2);
            x << ppf[j] << ssq[j];
            vec q(2);
            q[0] = alpha[0] * clone_dist.pdf(x);
            q[1] = alpha[1] * mixed_dist.pdf(x);
            vec w = q / sum(q);

            // Skip outliers:
            if(not w.is_finite())
                continue;

            // Running sums for moments are weighted:
            sumw         += w;
            sum1[0]      += w[0] * x;
            sum1[1]      += w[1] * x;
            sum2.at(0,0) += w[0] * square(ppf[j] - mean[0][0]);
            sum2.at(0,1) += w[0] * (ppf[j] - mean[0][0]) * (ssq[j] - mean[0][1]);

            sum2.at(1,1) += w[0] * square(ssq[j] - mean[0][1]);
            sum3.at(0,0) += w[1] * square(ppf[j] - mean[1][0]);
            sum3.at(0,1) += w[1] * (ppf[j] - mean[1][0]) * (ssq[j] - mean[1][1]);
            sum3.at(1,1) += w[1] * square(ssq[j] - mean[1][1]);
        }
        sum2.at(1,0) =  sum2.at(0,1);
        sum3.at(1,0) =  sum3.at(0,1);

        // New means:
        for(int j=0; j<2; ++j) {
            new_mean[j].set_size(2);
            new_mean[j] = sum1[j] / sumw[j];
        }

        vec det(2);
        det[0] = sqrt(sum2.at(0, 0)*sum2.at(1,1) - sum2.at(0, 1)*sum2.at(1,0));
        det[1] = sqrt(sum3.at(0, 0)*sum3.at(1,1) - sum3.at(0, 1)*sum3.at(1,0));
        double det_factor = (det[0] + det[1])/(sumw[0] + sumw[1]);

        // New covariance:
        mat new_sgma1(2,2);
        new_sgma1.at(0,0) = sum2.at(0,0) * det_factor / det[0];
        new_sgma1.at(0,1) = sum2.at(0,1) * det_factor / det[0];
        new_sgma1.at(1,0) = new_sgma1.at(0,1);
        new_sgma1.at(1,1) = sum2.at(1,1) * det_factor / det[0];

        mat new_sgma2(2,2);
        new_sgma2.at(0,0) = sum3.at(0,0) * det_factor / det[1];
        new_sgma2.at(0,1) = sum3.at(0,1) * det_factor / det[1];
        new_sgma2.at(1,0) = new_sgma2.at(0,1);
        new_sgma2.at(1,1) = sum3.at(1,1) * det_factor / det[1];

        new_sgma[0] = new_sgma1;
        new_sgma[1] = new_sgma2;

        // New prior:
        new_alpha = sumw / nsamp;
    }
        break;

    case 9:{//independent covariance
        bivariate_gaussian clone_dist(mean[0], sgma[0]);
        bivariate_gaussian mixed_dist(mean[1], sgma[1]);
        // Re-estimate parameters for each distribution:
        int nsamp = ppf.size();
        vec sumw(2);
        sumw.fill(0.0);

        mat sum2(2,2);
        sum2.fill(0.0);

        mat sum3(2,2);
        sum3.fill(0.0);

        vec   sum1[2];
        sum1[0].set_size(2);
        sum1[1].set_size(2);
        sum1[0].fill(0.0);
        sum1[1].fill(0.0);

        // Accumulate weighted sums for re-estimating moments:
        for(int j=0; j<nsamp; ++j){
            // Skip reads outside the poisson range for ppf:
            if(mixed_ppf_cutoff() < ppf[j])
                continue;

            // Each read gets two weights, reflecting the likelyhoods of that
            // read being clonal or mixed.
            vec x(2);
            x << ppf[j] << ssq[j];
            vec q(2);
            q[0] = alpha[0] * clone_dist.pdf(x);
            q[1] = alpha[1] * mixed_dist.pdf(x);
            vec w = q / sum(q);

            // Skip outliers:
            if(not w.is_finite())
                continue;

            // Running sums for moments are weighted:
            sumw         += w;
            sum1[0]      += w[0] * x;
            sum1[1]      += w[1] * x;
            sum2.at(0,0) += w[0] * square(ppf[j] - mean[0][0]);
            sum2.at(0,1) += w[0] * (ppf[j] - mean[0][0]) * (ssq[j] - mean[0][1]);
            sum2.at(1,1) += w[0] * square(ssq[j] - mean[0][1]);
            sum3.at(0,0) += w[1] * square(ppf[j] - mean[1][0]);
            sum3.at(0,1) += w[1] * (ppf[j] - mean[1][0]) * (ssq[j] - mean[1][1]);
            sum3.at(1,1) += w[1] * square(ssq[j] - mean[1][1]);
        }

        // New means:
        for(int j=0; j<2; ++j) {
            new_mean[j].set_size(2);
            new_mean[j] = sum1[j] / sumw[j];
        }

        // New covariance:
        mat new_sgma1(2,2);
        new_sgma1.at(0,0) = sum2.at(0,0) / sumw[0];
        new_sgma1.at(0,1) = sum2.at(0,1) / sumw[0];
        new_sgma1.at(1,0) = new_sgma1.at(0,1);
        new_sgma1.at(1,1) = sum2.at(1,1) / sumw[0];

        mat new_sgma2(2,2);
        new_sgma2.at(0,0) = sum3.at(0,0) / sumw[1];
        new_sgma2.at(0,1) = sum3.at(0,1) / sumw[1];
        new_sgma2.at(1,0) = new_sgma2.at(0,1);
        new_sgma2.at(1,1) = sum3.at(1,1) / sumw[1];
        new_sgma[0] = new_sgma1;
        new_sgma[1] = new_sgma2;

        // New prior:
        new_alpha = sumw / nsamp;
    }
        break;
  }

}

ostream& operator<<(ostream& out, const filter_counts& c)
{
    out << setw(8) << "infinite" << setw(12) << c. _ninf    << endl
        << setw(8) << "bad-key"  << setw(12) << c._nbad_key << endl
        << setw(8) << "high-ppf" << setw(12) << c._nsuper   << endl
        << setw(8) << "mixed"    << setw(12) << c._nmixed   << endl
        << setw(8) << "clonal"   << setw(12) << c._nclonal  << endl
        << setw(8) << "samples"  << setw(12) << c._nsamp    << endl;

    return out;
}

//lamda * D_k * A_k * (D_k)'

//alternative: using lamda from clonal

//Is it possible to try different models and find the one fitting the data best?


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_PLUS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_PLUS_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/traits.hpp>
#include <boost/simd/function/bitwise_ornot.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/shr.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/zero.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF ( plus_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE
    A0 operator()(const saturated_tag &, const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      return a0+a1;
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( plus_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_<bd::uint_<A0>, X>
                          , bs::pack_<bd::uint_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE
    A0 operator()(const saturated_tag &, const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      A0 const s = a0+a1;
      return s | genmask(s < a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( plus_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_<bd::int_<A0>, X>
                          , bs::pack_<bd::int_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE
    A0 operator()(const saturated_tag &, const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      using utype = bd::as_unsigned_t<A0>;
      using stype = typename A0::value_type;

      utype ux = bitwise_cast<utype>(a0);
      utype const uy = bitwise_cast<utype>(a1);
      utype const res = ux + uy;

      ux = shr(ux, sizeof(stype)*CHAR_BIT-1) + utype(Valmax<stype>());

      auto t = is_gez(bitwise_cast<A0>(bitwise_ornot(ux ^ uy, uy ^ res )));

      return bitwise_cast<A0>(if_else(t, ux, res));
    }
  };
} } }

#endif


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_DEALII_SOLVER_HPP
#define MFMG_DEALII_SOLVER_HPP

#include <mfmg/common/solver.hpp>

#include <deal.II/lac/trilinos_solver.h>

namespace mfmg
{
template <typename VectorType>
class DealIISolver final : public Solver<VectorType>
{
public:
  using vector_type = VectorType;

  DealIISolver(std::shared_ptr<Operator<vector_type> const> op,
               std::shared_ptr<boost::property_tree::ptree const> params);

  virtual ~DealIISolver() override = default;

  void apply(vector_type const &b, vector_type &x) const override;

private:
  dealii::SolverControl _solver_control;
  std::unique_ptr<dealii::TrilinosWrappers::SolverDirect> _solver;
  std::unique_ptr<dealii::TrilinosWrappers::PreconditionBase> _smoother;
};
} // namespace mfmg

#endif


// Copyright (C) 2013 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 "five_point_relative_pose.h"

#include <Eigen/Dense>

#include <cmath>
#include <ctime>
#include <vector>

using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::RowVector3d;
using Eigen::RowVector4d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;
using Eigen::VectorXd;

typedef Matrix<double, 10, 10> Matrix10d;

// Multiply two degree one polynomials of variables x, y, z.
// E.g. p1 = a[0]x + a[1]y + a[2]z + a[3]
// Output order: x^2 xy y^2 xz yz z^2 x y z 1 (GrevLex)
Matrix<double, 1, 10> MultiplyDegOnePoly(const RowVector4d& a,
                                         const RowVector4d& b) {
  Matrix<double, 1, 10> output;
  // x^2
  output(0) = a(0) * b(0);
  // xy
  output(1) = a(0) * b(1) + a(1) * b(0);
  // y^2
  output(2) = a(1) * b(1);
  // xz
  output(3) = a(0) * b(2) + a(2) * b(0);
  // yz
  output(4) = a(1) * b(2) + a(2) * b(1);
  // z^2
  output(5) = a(2) * b(2);
  // x
  output(6) = a(0) * b(3) + a(3) * b(0);
  // y
  output(7) = a(1) * b(3) + a(3) * b(1);
  // z
  output(8) = a(2) * b(3) + a(3) * b(2);
  // 1
  output(9) = a(3) * b(3);
  return output;
}

// Multiply a 2 deg poly (in x, y, z) and a one deg poly in GrevLex order.
// x^3 x^2y xy^2 y^3 x^2z xyz y^2z xz^2 yz^2 z^3 x^2 xy y^2 xz yz z^2 x y z 1
Matrix<double, 1, 20> MultiplyDegTwoDegOnePoly(const Matrix<double, 1, 10>& a,
                                               const RowVector4d& b) {
  Matrix<double, 1, 20> output;
  // x^3
  output(0) = a(0) * b(0);
  // x^2y
  output(1) = a(0) * b(1) + a(1) * b(0);
  // xy^2
  output(2) = a(1) * b(1) + a(2) * b(0);
  // y^3
  output(3) = a(2) * b(1);
  // x^2z
  output(4) = a(0) * b(2) + a(3) * b(0);
  // xyz
  output(5) = a(1) * b(2) + a(3) * b(1) + a(4) * b(0);
  // y^2z
  output(6) = a(2) * b(2) + a(4) * b(1);
  // xz^2
  output(7) = a(3) * b(2) + a(5) * b(0);
  // yz^2
  output(8) = a(4) * b(2) + a(5) * b(1);
  // z^3
  output(9) = a(5) * b(2);
  // x^2
  output(10) = a(0) * b(3) + a(6) * b(0);
  // xy
  output(11) = a(1) * b(3) + a(6) * b(1) + a(7) * b(0);
  // y^2
  output(12) = a(2) * b(3) + a(7) * b(1);
  // xz
  output(13) = a(3) * b(3) + a(6) * b(2) + a(8) * b(0);
  // yz
  output(14) = a(4) * b(3) + a(7) * b(2) + a(8) * b(1);
  // z^2
  output(15) = a(5) * b(3) + a(8) * b(2);
  // x
  output(16) = a(6) * b(3) + a(9) * b(0);
  // y
  output(17) = a(7) * b(3) + a(9) * b(1);
  // z
  output(18) = a(8) * b(3) + a(9) * b(2);
  // 1
  output(19) = a(9) * b(3);
  return output;
}

Matrix<double, 1, 20> GetDeterminantConstraint(
    const Matrix<double, 1, 4> null_space[3][3]) {
  // Singularity constraint.
  const Matrix<double, 1, 20> determinant =
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space[0][1], null_space[1][2]) -
          MultiplyDegOnePoly(null_space[0][2], null_space[1][1]),
          null_space[2][0]) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space[0][2], null_space[1][0]) -
          MultiplyDegOnePoly(null_space[0][0], null_space[1][2]),
          null_space[2][1]) +
      MultiplyDegTwoDegOnePoly(
          MultiplyDegOnePoly(null_space[0][0], null_space[1][1]) -
          MultiplyDegOnePoly(null_space[0][1], null_space[1][0]),
          null_space[2][2]);
  return determinant;
}

// Shorthand for multiplying the Essential matrix with its transpose.
Matrix<double, 1, 10> EETranspose(
    const Matrix<double, 1, 4> null_space[3][3], int i, int j) {
  return MultiplyDegOnePoly(null_space[i][0], null_space[j][0]) +
      MultiplyDegOnePoly(null_space[i][1], null_space[j][1]) +
      MultiplyDegOnePoly(null_space[i][2], null_space[j][2]);
}

// Builds the trace constraint: EEtE - 1/2 trace(EEt)E = 0
Matrix<double, 9, 20> GetTraceConstraint(
    const Matrix<double, 1, 4> null_space[3][3]) {
  Matrix<double, 9, 20> trace_constraint;

  // Comput EEt.
  Matrix<double, 1, 10> eet[3][3];
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      eet[i][j] = 2 * EETranspose(null_space, i, j);
    }
  }

  // Compute the trace.
  const Matrix<double, 1, 10> trace = eet[0][0] + eet[1][1] + eet[2][2];

  // Multiply EEt with E.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      trace_constraint.row(3 * i + j) =
          MultiplyDegTwoDegOnePoly(eet[i][0], null_space[0][j]) +
          MultiplyDegTwoDegOnePoly(eet[i][1], null_space[1][j]) +
          MultiplyDegTwoDegOnePoly(eet[i][2], null_space[2][j]) -
          0.5 * MultiplyDegTwoDegOnePoly(trace, null_space[i][j]);
    }
  }

  return trace_constraint;
}

Matrix<double, 10, 20> BuildConstraintMatrix(
    const Matrix<double, 1, 4> null_space[3][3]) {
  Matrix<double, 10, 20> constraint_matrix;
  constraint_matrix.block<9, 20>(0, 0) = GetTraceConstraint(null_space);
  constraint_matrix.row(9) = GetDeterminantConstraint(null_space);
  return constraint_matrix;
}

// Implementation of Nister from "An Efficient Solution to the Five-Point
// Relative Pose Problem"
bool FivePointRelativePose(const std::vector<Vector2d>& image1_points,
                           const std::vector<Vector2d>& image2_points,
                           std::vector<Matrix3d>* essential_matrices) {

  // Step 1. Create the nx9 matrix containing epipolar constraints.
  //   Essential matrix is a linear combination of the 4 vectors spanning the
  //   null space of this matrix.
  MatrixXd epipolar_constraint(image1_points.size(), 9);
  for (int i = 0; i < image1_points.size(); i++) {
    // Fill matrix with the epipolar constraint from q'_t*E*q = 0. Where q is
    // from the first image, and q' is from the second.
    epipolar_constraint.row(i) <<
        image2_points[i].x() * image1_points[i].x(),
        image2_points[i].y() * image1_points[i].x(),
        image1_points[i].x(),
        image2_points[i].x() * image1_points[i].y(),
        image2_points[i].y() * image1_points[i].y(),
        image1_points[i].y(),
        image2_points[i].x(),
        image2_points[i].y(),
        1.0;
  }

  Matrix<double, 9, 4> null_space;

  // Extract the null space from a minimal sampling (using LU) or non-minimal
  // sampling (using SVD).
  if (image1_points.size() == 5) {
    const Eigen::FullPivLU<MatrixXd> lu(epipolar_constraint);
    if (lu.dimensionOfKernel() != 4) {
      return false;
    }
    null_space = lu.kernel();
  } else {
    const Eigen::JacobiSVD<MatrixXd> svd(
        epipolar_constraint.transpose() * epipolar_constraint,
        Eigen::ComputeFullV);
    null_space = svd.matrixV().rightCols<4>();
  }

  const Matrix<double, 1, 4> null_space_matrix[3][3] = {
    { null_space.row(0), null_space.row(3), null_space.row(6) },
    { null_space.row(1), null_space.row(4), null_space.row(7) },
    { null_space.row(2), null_space.row(5), null_space.row(8) }
  };

  // Step 2. Expansion of the epipolar constraints on the determinant and trace.
  const Matrix<double, 10, 20> constraint_matrix =
      BuildConstraintMatrix(null_space_matrix);

  // Step 3. Eliminate part of the matrix to isolate polynomials in z.
  Eigen::FullPivLU<Matrix10d> c_lu(constraint_matrix.block<10, 10>(0, 0));
  Matrix10d eliminated_matrix =
      c_lu.solve(constraint_matrix.block<10, 10>(0, 10));

  Matrix10d action_matrix = Matrix10d::Zero();
  action_matrix.block<3, 10>(0, 0) = eliminated_matrix.block<3, 10>(0, 0);
  action_matrix.row(3) = eliminated_matrix.row(4);
  action_matrix.row(4) = eliminated_matrix.row(5);
  action_matrix.row(5) = eliminated_matrix.row(7);
  action_matrix(6, 0) = -1.0;
  action_matrix(7, 1) = -1.0;
  action_matrix(8, 3) = -1.0;
  action_matrix(9, 6) = -1.0;

  Eigen::EigenSolver<Matrix10d> eigensolver(action_matrix);
  const auto& eigenvectors = eigensolver.eigenvectors();
  const auto& eigenvalues = eigensolver.eigenvalues();

  // Now that we have x, y, and z we need to substitute them back into the null
  // space to get a valid essential matrix solution.
  for (int i = 0; i < 10; i++) {
    // Only consider real solutions.
    if (eigenvalues(i).imag() != 0) {
      continue;
    }
    Matrix3d ematrix;
    Map<Matrix<double, 9, 1> >(ematrix.data()) =
        null_space * eigenvectors.col(i).tail<4>().real();
    essential_matrices->emplace_back(ematrix);
  }

  return essential_matrices->size() > 0;
}


#include <stdio.h>
#include <inttypes.h>
#include <boost/shared_ptr.hpp>
#include <Eigen/Dense>

#include <bot_param/param_client.h>
#include <bot_frames_cpp/bot_frames_cpp.hpp>

#include <pronto_utils/pronto_vis.hpp> // visualize pt clds
#include <pronto_utils/pronto_lcm.hpp> // unpack lidar to xyz

#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/bot_core.hpp"
#include <ConciseArgs>

using namespace std;

// set all unlikely returns to this range (same produced by sensor)
#define MAX_RANGE 60.0
// all ranges shorter than this are assumed to be with the head
#define ASSUMED_HEAD 0.3//0.3
// all ranges longer than this are assumed to be free
#define ASSUMED_FAR 2.0// 2.0
// set all collisions to this range
#define COLLISION_RANGE 0.0

class Pass{
  public:
    Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_,
         std::string lidar_channel_, double delta_threshold_);

    ~Pass(){
    }
  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    bool verbose_;
    std::string lidar_channel_;
    void lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg);

    // Output filter lidar:
    bot_core::planar_lidar_t lidar_msgout_;
    double delta_threshold_;

    BotParam* botparam_;
    bot::frames* frames_cpp_;
};

Pass::Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_,
         std::string lidar_channel_, double delta_threshold_):
    lcm_(lcm_), verbose_(verbose_),
    lidar_channel_(lidar_channel_), delta_threshold_(delta_threshold_){

  botparam_ = bot_param_new_from_server(lcm_->getUnderlyingLCM(), 0);
  frames_cpp_= new bot::frames(lcm_, botparam_);
  lcm_->subscribe( lidar_channel_ ,&Pass::lidarHandler,this);

  cout << "Finished setting up\n";
}

void Pass::lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg){
  // Make Local copy to later output
  lidar_msgout_ = *msg;
  std::vector<float> original_ranges =  lidar_msgout_.ranges;

  Eigen::Isometry3d scan_to_local; // transform used to do conversion
  frames_cpp_->get_trans_with_utime("SCAN", "body"  , msg->utime, scan_to_local);

  // 1. Convert scan into simple XY point cloud:
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
  double minRange =-100.0; // consider everything - don't remove any points
  double maxRange = 100.0;
  double validBeamAngles[] ={-10,10};
  convertLidar(msg->ranges, msg->nranges, msg->rad0,
      msg->radstep, scan_cloud, minRange, maxRange,
      validBeamAngles[0], validBeamAngles[1]);

  if (scan_cloud->points.size() !=  msg->nranges ){
    std::cout << "npoints and nranges are not equal\n";
    std::cout << scan_cloud->points.size() << "\n";
    std::cout << msg->nranges << "\n";
    exit(-1);
  }

  // 2. Project the scan into camera frame:
  Eigen::Isometry3f pose_f = scan_to_local.cast<float>();
  Eigen::Quaternionf pose_quat(pose_f.rotation());
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZRGB> ());
  pcl::transformPointCloud (*scan_cloud, *cloud_transformed,
      pose_f.translation(), pose_quat);
  /// cloud_transformed now contains the lidar returns as 3d points in the local frame

  for( unsigned int i = 0; i < lidar_msgout_.ranges.size()  ; i++ ){

    // Short Range Points:
    if( original_ranges[i] < ASSUMED_HEAD){
      lidar_msgout_.ranges[i] = COLLISION_RANGE;
    }

    // heuristic filtering of the weak intensity lidar returns
    if (( lidar_msgout_.intensities[i] < 2000 ) && ( original_ranges[i] < 2) ){
      lidar_msgout_.ranges[i] = MAX_RANGE;
    }

    // Edge effect filter
    if ( (i>0) && (i<lidar_msgout_.ranges.size()) ){
      float right_diff = fabs(original_ranges[i] - original_ranges[i-1]);
      float left_diff = fabs(original_ranges[i] - original_ranges[i+1]);
      if (( right_diff > delta_threshold_) || (left_diff > delta_threshold_ )){
        // cout << i<< ": " << right_diff << " and " << left_diff <<"\n";
        if (original_ranges[i] < 2){
          lidar_msgout_.ranges[i] = MAX_RANGE;
        }
      }
    }

    /*
    // Box filter around the robot (temporary until urdf improves)
    if ((cloud_transformed->points[i].x < 0.65) &&
        ( fabs (cloud_transformed->points[i].y) < 0.75)){
      lidar_msgout_.ranges[i] = COLLISION_RANGE;
    }
    */

  }
//  lidar_msgout_.ranges = ranges;

  lcm_->publish( (lidar_channel_ + "_FREE") , &lidar_msgout_);
}

int main( int argc, char** argv ){
  ConciseArgs parser(argc, argv, "lidar-passthrough");
  bool verbose=false;
  string lidar_channel="SCAN";
  double threshold = 0.03;
  parser.add(verbose, "v", "verbose", "Verbosity");
  parser.add(lidar_channel, "l", "lidar_channel", "Incoming LIDAR channel");
  parser.add(threshold, "t", "threshold", "Lidar threshold [lower removes more points]");
  parser.parse();
  cout << verbose << " is verbose\n";
  cout << lidar_channel << " is lidar_channel\n";

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  Pass app(lcm,verbose,lidar_channel, threshold);
  cout << "Ready to filter lidar points" << endl << "============================" << endl;
  while(0 == lcm->handle());
  return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <vector>
#include <Eigen/Dense>

#include <dpMM/gpuMatrix.hpp>
#include <dpMM/sphere.hpp>
#include <dpMM/clGMMDataGpu.hpp>
#include <dpMM/timer.hpp>

using namespace Eigen;
using std::vector;

extern  void Log_p_gpu(double *p, double *q, double *Rs, uint32_t *z,
  uint32_t K, uint32_t N, double *x);
extern void meanInTpS2_gpu(double *d_p, double *d_mu_karch, double *d_q,
    uint32_t *d_z , uint32_t N, uint32_t K);
extern void sufficientStatisticsOnTpS2_gpu(double *d_p, double *d_Rnorths,
    double *d_q, uint32_t *d_z , uint32_t N, uint32_t k0, uint32_t K,
    double *d_SSs);
extern void sphereGMMPdf(double *d_p, double *d_Rnorths, double * d_q,
    double *d_invSigmas, double *d_logNormalizer, double *d_logPi,
    double* d_logPdf, uint32_t N, uint32_t K);

extern  void Log_p_gpu(float *p, float *q, float *Rs, uint32_t *z,
  uint32_t K, uint32_t N, float *x);
extern void meanInTpS2_gpu(float *d_p, float *d_mu_karch, float *d_q,
    uint32_t *d_z , uint32_t N, uint32_t K);
extern void sufficientStatisticsOnTpS2_gpu(float *d_p, float *d_Rnorths,
    float *d_q, uint32_t *d_z , uint32_t N, uint32_t k0, uint32_t K,
    float *d_SSs);
extern void sphereGMMPdf(float *d_p, float *d_Rnorths, float * d_q,
    float *d_invSigmas, float *d_logNormalizer, float *d_logPi,
    float* d_logPdf, uint32_t N, uint32_t K);

template<typename T>
class ClTGMMDataGpu : public ClGMMDataGpu<T>
{
protected:
  GpuMatrix<T> d_q_; // normals on GPU
//  GpuMatrix<uint32_t> d_z_; // indicators on GPU
//  GpuMatrix<T> d_x_; // points in tangent plane (dim. is 1 less than q)

  //uint32_t K_; // number of different tangent planes
  GpuMatrix<T> d_ps_; // points around which we have tangent spaces
  GpuMatrix<T> d_muKarch_; // karcher means
//  GpuMatrix<T> d_Ss_; // sufficient statistics in tangent spaces
  GpuMatrix<T> d_northRps_; // rotations from tangent spaces to north

//  GpuMatrix<T> d_pdfs_; //
//  GpuMatrix<T> d_logPi_; //
//  GpuMatrix<T> d_logNormalizers_; //
//  GpuMatrix<T> d_invSigmas_; //

  Matrix<T,Dynamic,Dynamic> ps_;

  Sphere<T> sphere_;

public:
  ClTGMMDataGpu(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& q,
      const spVectorXu& z, uint32_t K);
  ~ClTGMMDataGpu(){;};
  virtual void init(const Matrix<T,Dynamic,Dynamic>& ps);

  /* normal in tangent space around p rotate to the north pole
   * -> the last dimension will always be 0
   * -> return only first 2 dims
   */
  void Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
      const VectorXu& z, Matrix<T,Dynamic,Dynamic>& x, int32_t K=-1);
  void Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
      const VectorXu& z, int32_t K=-1);

  Matrix<T,Dynamic,Dynamic> Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
      int32_t K=-1)
  {
    Matrix<T,Dynamic,Dynamic> x(this->d_x_.rows(),this->d_x_.cols());
    Log_p_north(ps, *this->z_, x, K);
    return x;
  };

  virtual void update(uint32_t K);

  void relinearize(const Matrix<T,Dynamic,Dynamic>& ps);

  Matrix<T,Dynamic,Dynamic> karcherMeans(const Matrix<T,Dynamic,Dynamic>& p0,
      uint32_t maxIter = 50);

  virtual void computeLogLikelihoods(const Matrix<T,Dynamic,1>& pi,
    const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
    const Matrix<T,Dynamic,1>& logNormalizers);
  virtual void sampleGMMpdf(const Matrix<T,Dynamic,1>& pi,
      const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
      const Matrix<T,Dynamic,1>& logNormalizers, Sampler<T> *sampler);

  const Matrix<T,Dynamic,Dynamic>& ps() const {return ps_;};
  const Matrix<T,Dynamic,1>& p(uint32_t k) const {return ps_.col(k);};
  // TODO: broke compatibility! this used to be ps
//  Matrix<T,Dynamic,1> mean(uint32_t k) const {return ps_.col(k);};
//  const Matrix<T,Dynamic,Dynamic>& means() const {return ps_;};

  virtual const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& q() const
  {return this->x_;};
  virtual const Matrix<T,Dynamic,Dynamic>& qMat() const
  {return (*this->x_);};

  GpuMatrix<T>& d_q(){ return d_q_;};

protected:
  Matrix<T,Dynamic,Dynamic> meanInTpS2(const Matrix<T,Dynamic,Dynamic>& ps);
  Matrix<T,Dynamic,Dynamic> karcherMeans__(const Matrix<T,Dynamic,Dynamic>& p0,
      uint32_t maxIter = 50);
};

typedef ClTGMMDataGpu<double> ClTGMMDataGpud;
typedef ClTGMMDataGpu<float> ClTGMMDataGpuf;

// ------------------------------------ impl --------------------------------
template<typename T>
ClTGMMDataGpu<T>::ClTGMMDataGpu(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& q,
    const spVectorXu& z, uint32_t K)
  : ClGMMDataGpu<T>(q,z,K), d_q_(this->D_,this->N_),
    d_ps_(this->D_,this->K_), d_muKarch_(this->D_+1,this->K_),
    d_northRps_(this->D_-1,this->K_*this->D_),
    ps_(Matrix<T,Dynamic,Dynamic>::Zero(this->D_,this->K_)),
    sphere_(this->D_)
{};

template<typename T>
void ClTGMMDataGpu<T>::init(const Matrix<T,Dynamic,Dynamic>& ps)
{
  for(uint32_t i=0; i<this->N_; ++i)
    if(fabs(this->x_->col(i).norm()-1.0) > 1e-5 )
    {
        cout<<"ClTGMMDataGpu<T>::ClTGMMDataGpu: warning: renormalizing normal "
          <<this->x_->col(i).norm()<<endl;
      this->x_->col(i) /= this->x_->col(i).norm();
    }
  d_q_.set(*this->x_);
  this->d_x_.setZero();

  // resize the statistyics to be one less dim, since we are in
  // the tangent planes
  this->Ns_.setZero(this->K_);
  this->means_.setZero(this->D_-1,this->K_);
  for(uint32_t k=0; k<this->K_; ++k)
    this->Ss_[k].setZero(this->D_-1,this->D_-1);

  ps_ = ps;
//  for(uint32_t k=0; k<this->K_; ++k)
//    ps_.col(k) = sphere_.sampleUnif(this->pRndGen_);
};

template<typename T>
void ClTGMMDataGpu<T>::update(uint32_t K)
{
  this->K_ = K>0?K:this->z_->maxCoeff()+1;
  assert(this->z_->maxCoeff() < this->K_); // no indicators \geq K
  assert(this->z_->minCoeff() >= 0); // no indicators \le 0
  assert((this->z_->array() < this->K_).all());
  // update the labels on the GPU
  this->d_z_.set(this->z_);
  //TODO shouldnt need to do this everytime?
  //d_q_.set(*this->x_);
  // run karcher means to obtain ps
//  Matrix<T,Dynamic,Dynamic> q = d_q_.get();
//  cout<<q<<endl;
  Timer t;
  ps_ = karcherMeans__(ps_);
  t.toctic("karcherMeansFull");
  cout<<"linearize around: \n"<<ps_<<endl;
  // relinearize around the new centers
  t.tic();
  relinearize(ps_);
  t.toctic("relinearize");

//  // put points into tangent spaces - relinearize
//  Log_p_gpu(d_ps_.data(), d_q_.data(), d_northRps_.data(), this->d_z_.data(), this->K_,
//      d_q_.cols(),d_this->x_.data());
  // compute moments
  this->computeSufficientStatistics();
  t.toctic("computeSufficientStatistics");
}

template<typename T>
void ClTGMMDataGpu<T>::relinearize(const Matrix<T,Dynamic,Dynamic>& ps)
{
  // TODO: need to call udapte Labels before this function

  // compute rotations to north and push them to GPU
  Matrix<T,Dynamic,Dynamic> Rs(ps.rows()-1, this->K_*3);
  for(uint32_t k=0; k<this->K_; ++k)
    Rs.middleCols(k*3,3) = sphere_.north_R_TpS2(ps.col(k)).topRows(2);

  d_ps_.set(ps);
  d_northRps_.set(Rs);

  // q is already in GPU since construction
//  d_ps_.print(); d_q_.print(); d_northRps_.print(); this->d_z_.print(); d_q_.print();
//  this->d_x_.print();

  Log_p_gpu(d_ps_.data(), d_q_.data(), d_northRps_.data(), this->d_z_.data(),
      this->K_, d_q_.cols(), this->d_x_.data());

//  d_northRps_.get(Rs);
//  cout<<Rs<<endl;
//  Matrix<T,Dynamic,Dynamic> pps(ps.rows(),ps.cols());
//  d_ps_.get(pps);
//  cout<<pps<<endl;
};

template<typename T>
Matrix<T,Dynamic,Dynamic> ClTGMMDataGpu<T>::meanInTpS2(
    const Matrix<T,Dynamic,Dynamic>& ps)
{
  assert(ps.cols() == this->K_);
  assert(ps.rows() == this->D_);

  // one more dimension to hold the counts
  Matrix<T,Dynamic,Dynamic> muKarch =
    Matrix<T,Dynamic,Dynamic>::Zero(this->D_+1,this->K_);
  d_muKarch_.set(muKarch);
  d_ps_.set(ps);

//  cout<<"ClTGMMDataGpu<T>::meanInTpS2: mean starting from "<<endl<<ps<<endl;
//  cout<<d_ps_.rows()<<" "<<d_ps_.cols()<<endl;
//  cout<<d_muKarch_.rows()<<" "<<d_muKarch_.cols()<<endl;
//  cout<<d_q_.rows()<<" "<<d_q_.cols()<<endl;
//  cout<<this->d_z_.rows()<<" "<<this->d_z_.cols()<<endl;

//  cout<<d_ps_.get()<<endl;
//  cout<<d_muKarch_.get()<<endl;
//  cout<<d_q_.get()<<endl;
//  cout<<this->d_z_.get()<<endl;

  meanInTpS2_gpu(d_ps_.data(), d_muKarch_.data(), d_q_.data(), this->d_z_.data(),
      this->N_,this->K_);
  //meanInTpS2GPU(h_p, d_p_, h_mu_karch, d_mu_karch_, d_q_, d_z, w_, h_);
  d_muKarch_.get(muKarch);

  Matrix<T,Dynamic,Dynamic> mu = muKarch.topRows(this->D_);
  for(uint32_t k=0; k<this->K_; ++k)
    if(muKarch(this->D_,k) > 0)
    {
      mu.col(k) /= muKarch(this->D_,k);
    }

//  cout<<muKarch<<endl<<endl;
//  cout<<mu<<endl;
  return mu;
}

template<typename T>
Matrix<T,Dynamic,Dynamic> ClTGMMDataGpu<T>::karcherMeans(const Matrix<T,Dynamic,Dynamic>& p0, uint32_t maxIter)
{
  // stand alone karcher means
  assert(this->z_->maxCoeff() < this->K_); // no indicators \geq K
  assert(this->z_->minCoeff() >= 0); // no indicators \le 0
  assert((this->z_->array() < this->K_).all());
  // update the labels on the GPU
  this->d_z_.set(this->z_);

  Matrix<T,Dynamic,Dynamic> p(p0.rows(),p0.cols());
  Timer t;
  p = karcherMeans__(p0);
  t.toctic("karcherMeansFull");
  return p;
};

template<typename T>
Matrix<T,Dynamic,Dynamic> ClTGMMDataGpu<T>::karcherMeans__(const Matrix<T,Dynamic,Dynamic>& p0, uint32_t maxIter)
{

  assert(p0.rows() == d_ps_.rows()); // we dont want dimension change
  Matrix<T,Dynamic,Dynamic> p = p0;
//  cout<<"p0"<<endl<<p<<endl;

  Matrix<T,Dynamic,1> residual(this->K_);
  residual.setOnes(this->K_);
//  cout<<(this->z_->transpose())<<endl;
  for(uint32_t i=0; i< maxIter; ++i)
  {
//    Timer t0;
    Matrix<T,Dynamic,Dynamic> mu_karch = meanInTpS2(p);
//    t0.toctic("meanInTpS2_GPU");
//    cout<<"mu_karch"<<endl<<mu_karch<<endl;
//    cout<<"p"<<endl<<p<<endl;
    for (uint32_t k=0; k<this->K_; ++k)
    {
      p.col(k) = sphere_.Exp_p(p.col(k),mu_karch.col(k));
//      cout<<p.col(k).norm()<<endl;
      residual(k) = mu_karch.col(k).norm();
    }
//    cout<<"p"<<endl<<p<<endl;
    //cout<<"karcherMeans "<<i<<" residual="<<residual<<endl;
//    cout<<"@"<<i<<" residual = "<<residual.transpose()<<endl;
    if((residual.array() < 1e-5).all())
    {
      cout<<"ClTGMMDataGpu<T>::karcherMeans__: converged after "<<i
        <<" residual = "<<residual.transpose()<<endl;
      assert((residual.array() != 0.0).any());
//      assert(i>0); // first itaration convergence is rare
      break;
    }
  }
//  cout<<"p"<<endl<<p<<endl;
  return p;
}

template<typename T>
void ClTGMMDataGpu<T>::Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
    const VectorXu& z, Matrix<T,Dynamic,Dynamic>& x, int32_t K)
{
  Log_p_north(ps,z,K);
  this->d_x_.get(x);
};

template<typename T>
void ClTGMMDataGpu<T>::Log_p_north(const Matrix<T,Dynamic,Dynamic>& ps,
    const VectorXu& z,  int32_t K)
{
  if(K > 0){
    this->K_ = K>0?K:this->z_->maxCoeff()+1;
//    cout<<this->K_<<" "<< uint32_t(this->z_->maxCoeff()) <<endl;
    assert(this->z_->maxCoeff() < this->K_); // no indicators \geq K
    assert(this->z_->minCoeff() >= 0); // no indicators \le 0
    assert((this->z_->array() < this->K_).all());
  }
  // updates d_ps_, d_northRps_, this->d_z_, this->K_
  relinearize(ps);
  this->d_z_.set(z);
  //TODO shouldnt need to do this everytime?
  d_q_.set(*this->x_);
  // q is already in GPU since construction
//  d_ps_.print();
//  d_q_.print();
//  d_northRps_.print();
//  this->d_z_.print();
//  d_q_.print();
//  this->d_x_.print();

  Log_p_gpu(d_ps_.data(), d_q_.data(), d_northRps_.data(), this->d_z_.data(),
      this->K_, d_q_.cols(),this->d_x_.data());
};

template<typename T>
void ClTGMMDataGpu<T>::computeLogLikelihoods(const Matrix<T,Dynamic,1>& pi,
    const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
    const Matrix<T,Dynamic,1>& logNormalizers)
{
//  cout<<"ClGMMDataGpu<T>::sampleGMMpdf"<<endl;
  assert(pi.size() == this->K_);
  assert(logNormalizers.size() == this->K_);

  Matrix<T,Dynamic,Dynamic> invSigmas((this->D_-1)*(this->D_-1),this->K_);
//  Matrix<T,Dynamic,1> logNormalizer(this->K_);
  for(uint32_t k=0; k<this->K_; ++k)
  {
    Matrix<T,Dynamic,Dynamic> invS =Sigmas[k].inverse();
    for(uint32_t i=0; i<invS.cols(); ++i)
      for(uint32_t j=0; j<invS.rows(); ++j)
        invSigmas(i*invS.rows()+j,k) = invS(j,i);
//    logNormalizer(k) = -0.5*Sigmas[k]
  }
//  assert(fabs(pi.sum()-1.0) <1e-6);
  Matrix<T,Dynamic,1> logPi = pi.array().log().matrix();
  //copy parameters into memory
  this->d_logPi_.set(logPi);
  this->d_invSigmas_.set(invSigmas);
  this->d_logNormalizers_.set(logNormalizers);

//  cout<<"sphereGMMPdf"<<endl;
//  cout<<"logNormaloizers"<<endl<<logNormalizers.transpose()<<endl;
//  cout<<"logPi"<<endl<<logPi.transpose()<<endl;
//  cout<<invSigmas<<endl;
//
//  d_ps_.data();
//  d_northRps_.data();
//  d_q_.data();
//  d_invSigmas_.data();
//  d_logNormalizers_.data();
//  d_logPi_.data();

  Matrix<T,Dynamic,Dynamic> pdfs(this->N_,this->K_);
  if(!this->d_pdfs_->isInit())
  {
    this->d_pdfs_->setZero();
//    pdfs = Matrix<T,Dynamic,Dynamic>::Zero(this->N_,this->K_);
//    d_pdfs_->set(pdfs);
  }

  sphereGMMPdf(d_ps_.data(), d_northRps_.data(), d_q_.data(),
    this->d_invSigmas_.data(), this->d_logNormalizers_.data(),
    this->d_logPi_.data(), this->d_pdfs_->data(), this->N_, this->K_);
};

template<typename T>
void ClTGMMDataGpu<T>::sampleGMMpdf(const Matrix<T,Dynamic,1>& pi,
    const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
    const Matrix<T,Dynamic,1>& logNormalizers, Sampler<T> *sampler)
{
  computeLogLikelihoods(pi,Sigmas,logNormalizers);

  sampler->sampleDiscPdf(this->d_pdfs_->data(),this->z_);

//  cout<< this->z_->transpose()<<endl;
//  d_pdfs_->get(pdfs);
//  cout<<pdfs<<endl;

};