// Standard libraries
#include <boost/shared_ptr.hpp>
#include <boost/lexical_cast.hpp>

// Required for the maths functions
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>

// Integration with the CRATES HAL
#include <hal_quadrotor/Quadrotor.h>

// Noise distributions
#include "../../noise/NoiseFactory.h"

// Dynamics
#include "dynamics/Propulsion.h"
#include "dynamics/Aerodynamics.h"
#include "dynamics/Energy.h"

// Sensors
#include "sensors/Altimeter.h"
#include "sensors/Compass.h"
#include "sensors/GNSS.h"
#include "sensors/IMU.h"
#include "sensors/Orientation.h"

// Custom messages
#include "noise.pb.h"

namespace gazebo
{
	typedef const boost::shared_ptr<const msgs::Noise> NoisePtr;

	class Quadrotor :
		public ModelPlugin, 				/* Needed for Gazebo integration */
		public hal::quadrotor::Quadrotor,	/* Exposes Quadrotor model       */
		public hal::sensor::Altimeter,		/* Exposes Altimeter sensor      */
		public hal::sensor::Compass,		/* Exposes Compass sensor      	 */
		public hal::sensor::GNSS,			/* Exposes GNSS sensor      	 */
		public hal::sensor::IMU,			/* Exposes IMU sensor      		 */
		public hal::sensor::Orientation 	/* Exposes Orientation sensor    */
 	{

  	private:

	    // Pointer to the update event connection
	    event::ConnectionPtr 		conPtr;

	    // Pointer to the model object
	    physics::ModelPtr 			modelPtr;
	    physics::LinkPtr 			linkPtr;

	    // Dynamics
	    gazebo::Aerodynamics    	dA;
	    gazebo::Propulsion			dP;
	    gazebo::Energy 				dE;

	    // Sensors
	    gazebo::Altimeter   		sA;
	    gazebo::Compass    			sC;
	    gazebo::GNSS    			sG;
	    gazebo::IMU    				sI;
	    gazebo::Orientation 		sO;

	    // Gazebo communication
		transport::NodePtr 			nodePtr;
		transport::SubscriberPtr 	subPtr;

	    // Last time tick
	    double 						tim;

    	// Callback for physics timer
		void PrePhysics(const common::UpdateInfo &_info)
		{
			// Time over which dynamics must be updated (needed for thrust update)
			double dt = _info.simTime.Double() - tim;

			// If simulation is paused, dont waste CPU calculating physics
			if (dt > 0)
			{
				// Propulsive forces
				dP.Update(dt);

				// Shear, turbulence and drag forces
				dA.Update(dt);

				// Energy consumption
				dE.Update(dt);
			}

			// Update timer
			tim = _info.simTime.Double();
		}

		// This will be called whenever a new meteorlogical topic is posted
		void ReceiveNoise(NoisePtr& inmsg)
		{
			NoiseFactory::SetEnabled(inmsg->enable());
		}

		// Animate motors
		void AnimateMotor(const char *name, const char *motor, double rpm)
		{
			std::map<std::string, common::NumericAnimationPtr> anim;
			anim[motor].reset(new common::NumericAnimation(name, 1.0, true));
			common::NumericKeyFrame *key;
			key = anim[motor]->CreateKeyFrame(0.0);
			key->SetValue(rpm*2*3.1415*0/4);
			key = anim[motor]->CreateKeyFrame(0.25);
			key->SetValue(rpm*2*3.1415*1/4);
			key = anim[motor]->CreateKeyFrame(0.50);
			key->SetValue(rpm*2*3.1415*2/4);
			key = anim[motor]->CreateKeyFrame(0.75);
			key->SetValue(rpm*2*3.1415*3/4);
			key = anim[motor]->CreateKeyFrame(1.0);
			key->SetValue(rpm*2*3.1415*4/4);
			modelPtr->SetJointAnimation(anim);
		}

  	public:

	    // Default constructor
	    Quadrotor() : hal::quadrotor::Quadrotor(), tim(0.0)
	    {
    		// Kill all random number streams
	    	NoiseFactory::Init();
	    }

	    /// Destructor
	    ~Quadrotor()
	    {
	    	// Make sure we clean up the connection
    		event::Events::DisconnectWorldUpdateBegin(conPtr);

    		// Kill all random number streams
	    	NoiseFactory::Destroy();
	    }

	    // REQUIRED METHODS

	    // All sensors must be configured using the current model information and the SDF
	    void Load(physics::ModelPtr model, sdf::ElementPtr root)
	    {
			// FIND LINK AGAINST WHICH ALL FORCES AND SENSING IS DONE /////////////

	    	std::string linkName;
			root->GetElement("link")->GetValue()->Get(linkName);
			modelPtr = model;
			linkPtr  = model->GetLink(linkName);

			// INITIALIZE THE NOISE DISTRIBUTION GENERATOR AND HAL ////////////////

	    	// Het
	    	std::string halName = (std::string)"/hal/quadrotor/hummingbird/" + model->GetName();
	    	hal::quadrotor::Quadrotor::Init(halName);
			ROS_INFO("Quadrotor plugin init %s",halName.c_str());
	    	hal::sensor::Altimeter::Init(halName);
	    	hal::sensor::Compass::Init(halName);
	    	hal::sensor::GNSS::Init(halName);
	    	hal::sensor::IMU::Init(halName);
	    	hal::sensor::Orientation::Init(halName);

			// DYNAMICS/SENSOR CONFIGURATION ///////////////////////////////////////

			// Configure the propulsion dynamics
			dP.Configure(linkPtr, root->GetElement("propulsion"));

			// Configure the aerodynamics
			dA.Configure(linkPtr, root->GetElement("aerodynamics"));

			// Configure the aerodynamics
			dE.Configure(linkPtr, root->GetElement("energy"));

			// Configure the altitude sensor
			sA.Configure(linkPtr, root->GetElement("altimeter"));

			// Configure the compass sensor
			sC.Configure(linkPtr, root->GetElement("compass"));

			// Configure the GNSS sensor
			sG.Configure(linkPtr, root->GetElement("gnss"));

			// Configure the IMU sensor
			sI.Configure(linkPtr, root->GetElement("imu"));

			// Configure the orientation sensor
			sO.Configure(linkPtr, root->GetElement("orientation"));

			// INITIALISE WGS84 <-> LTP CONVERSION CONSTANTS /////////////////////

			GetNavPtr()->SetOrigin(
				model->GetWorld()->GetSphericalCoordinates()->GetLatitudeReference().Degree(),
				model->GetWorld()->GetSphericalCoordinates()->GetLongitudeReference().Degree(),
				model->GetWorld()->GetSphericalCoordinates()->GetElevationReference()
			);

			// WORLD UPDATE CALLBACK CONFIGURATION ////////////////////////////////

			// Create and initialize a new Gazebo transport node
			nodePtr = transport::NodePtr(new transport::Node());
			nodePtr->Init(model->GetName());

			// Subscribe to meteorological updates
			subPtr = nodePtr->Subscribe("~/noise",
				&Quadrotor::ReceiveNoise, this);

			// WORLD UPDATE CALLBACK CONFIGURATION ////////////////////////////////

			// Create a pre-physics update call (before any physics)
			conPtr = event::Events::ConnectWorldUpdateBegin(
				boost::bind(&Quadrotor::PrePhysics, this, _1));

			// Create blade animation (10rpm)
			AnimateMotor("animation_motor0","motor0",5);
			AnimateMotor("animation_motor1","motor1",5);
			AnimateMotor("animation_motor2","motor2",5);
			AnimateMotor("animation_motor3","motor3",5);
	    }

	    // All sensors must be resettable
	    void Reset()
	    {
			// Reset the propulsion dynamics
			dP.Reset();

			// Reset the aerodynamics
			dA.Reset();

			// Reset the energy
			dE.Reset();

			// Reset the altitude sensor
			sA.Reset();

			// Reset the compass sensor
			sC.Reset();

			// Reset the GNSS sensor
			sG.Reset();

			// Reset the IMU sensor
			sI.Reset();

			// Reset the orientation sensor
			sO.Reset();
	    }

	    /////////////////////////////////////////////////////////////////////////////////////
	    // HAL CALLBACKS ////////////////////////////////////////////////////////////////////
		/////////////////////////////////////////////////////////////////////////////////////

	    // Called when the HAL wants an altimeter reading
	    bool GetMeasurement(hal_sensor_altimeter::Data& msg)
	    {
	    	if (sA.GetMeasurement(tim,msg))
	    	{
		    	GetNavPtr()->Process(msg);		// Also use for navigation
		    	return true;
		    }
	    	return false;
	    }

		// Called when the HAL wants a compass reading
	    bool GetMeasurement(hal_sensor_compass::Data& msg)
	    {
	    	if (sC.GetMeasurement(tim,msg))
	    	{
		    	GetNavPtr()->Process(msg);		// Also use for navigation
		    	return true;
		    }
	    	return false;
	    }

	    // Called when the HAL wants an imu reading
	    bool GetMeasurement(hal_sensor_imu::Data& msg)
	    {
	    	if (sI.GetMeasurement(tim,msg))
	    	{
		    	GetNavPtr()->Process(msg);		// Also use for navigation
		    	return true;
		    }
	    	return false;
	    }

	    // Called when the HAL wants a gnss reading
	    bool GetMeasurement(hal_sensor_gnss::Data& msg)
	    {
	    	if (sG.GetMeasurement(tim,msg))
	    	{
		    	GetNavPtr()->Process(msg);		// Also use for navigation
		    	return true;
		    }
	    	return false;
	    }

	    // Called when the HAL wants an orientation reading
	    bool GetMeasurement(hal_sensor_orientation::Data& msg)
	    {
	    	if (sO.GetMeasurement(tim,msg))
	    	{
		    	GetNavPtr()->Process(msg);		// Also use for navigation
		    	return true;
		    }
	    	return false;
	    }

    	// Called when the HAL wants the truthful state of the platform
		void GetTruth(hal_quadrotor::State& state)
		{
			// Get the state
			math::Vector3 pos = linkPtr->GetWorldPose().pos;
			math::Vector3 rot = linkPtr->GetWorldPose().rot.GetAsEuler();
			math::Vector3 vel = linkPtr->GetRelativeLinearVel();
			math::Vector3 ang = linkPtr->GetRelativeAngularVel();

			// Update the state
			state.t         = tim;
			state.x 		= pos.x;
			state.y 		= pos.y;
			state.z 		= pos.z;
			state.roll 		= rot.x;
			state.pitch 	= rot.y;
			state.yaw 		= rot.z;
			state.u 		= vel.x;
			state.v 		= vel.y;
			state.w 		= vel.z;
			state.p 		= ang.x;
			state.q 		= ang.y;
			state.r 		= ang.z;
			state.thrust 	= dP.GetThrustForce();
			state.remaining	= dE.GetRemaining();
		}

	    // Called when the HAL wants to set the truthful state of the platform
		void SetTruth(const hal_quadrotor::State& state)
		{
			// Get the state
			math::Pose pose(
				state.x,    state.y,     state.z,		// Position
				state.roll, state.pitch, state.yaw 		// Orientation
			);
			math::Vector3 vel(state.u, state.v, state.w);
			math::Vector3 ang(state.p, state.q, state.r);

			// Set the kinematic quantities
			linkPtr->SetWorldPose(pose);
			linkPtr->SetAngularVel(
				pose.rot.RotateVector(ang)
			);
			linkPtr->SetLinearVel(
				pose.rot.RotateVector(vel)
			);

			// Set the thrust force
			dP.SetThrustForce(state.thrust);

			// Set the remaining energy
			dE.SetRemaining(state.remaining);
		}

	    // Called when the HAL wants to pass down some control to the platform
		double SetControl(const hal_quadrotor::Control &control)
		{
			// Pass control to the propulsion module
			dP.SetControl(control);

			// Return the time at which control was applied
			return tim;
		}
	};

	// Register the plugin
	GZ_REGISTER_MODEL_PLUGIN(Quadrotor);
}


// File generated on Thu Aug 21, 2014 05:54:13 PM by xcpp.
#define XC_MATLAB
#include <armadillo>
#include <xc>

namespace excentury {
XC_DUMP_TEMPLATED_TENSOR(class elementType, arma::Mat<elementType>, m, m.mem[0]) {
    size_t ndims = 2;
    unsigned char rm = 0;
    XC_BYTE(rm);
    XC_SIZE(ndims);
    size_t size = m.n_elem;
    size_t dim[2] = {m.n_rows, m.n_cols};
    XC_ARRAY(dim, ndims);
    XC_ARRAY(m.mem, size);
}
XC_LOAD_TEMPLATED_TENSOR(class elementType, arma::Mat<elementType>, m) {
    size_t ndims;
    unsigned char rm;
    XC_BYTE(rm);
    if (rm != 0) {
        char msg[500];
        sprintf(msg, "Armadillo Mat::load:\n"
                "    RM mismatch, got %d, needs %d.", rm, 0);
        excentury::error(msg);
    }
    XC_SIZE(ndims);
    if (ndims != 2) {
        char msg[500];
        sprintf(msg, "Armadillo Mat::load('%s'):\n" \
                "    dimension mismatch, needs dim = 2", varname);
        excentury::error(msg);
    }
    size_t* dim = new size_t[ndims];
    XC_ARRAY(dim, ndims);
    m.resize(dim[0], dim[1]);
    elementType* mem = const_cast<elementType*>(m.mem);
    XC_ARRAY(mem, m.n_elem);
    delete [] dim;
}
}
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
    size_t ncin_ = mxGetScalar(prhs[0]);
    char* pcin_ = mxArrayToString(prhs[1]);
    excentury::STextInterface<excentury::load_mode> XC_LI_(pcin_, ncin_);
    arma::mat A(2, 2); XC_LI_.load(A, A(0,0));
    XC_LI_.close();

    double d = det(A);
    arma::mat Ainv;
    try {
        Ainv = inv(A);
    } catch (std::runtime_error& run_error) {
        excentury::error(run_error.what());
    }

    excentury::STextInterface<excentury::dump_mode> XC_DI_;
    XC_DI_.dump(d, "detA");
    XC_DI_.dump(Ainv, "Ainv", Ainv(0, 0));
    XC_DI_.close();
    std::string xc_mex_str_ = XC_DI_.str();
    plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
    double* ncout_ = mxGetPr(plhs[0]);
    ncout_[0] = xc_mex_str_.size();
    plhs[1] = mxCreateString(xc_mex_str_.data());
    mxFree(pcin_);
}


/*
 * drle.cc -- Delta Run-Length Encoding Manager implementation
 *
 * Copyright (C) 2009-2012, Computing Systems Laboratory (CSLab), NTUA.
 * Copyright (C) 2009-2011, Kornilios Kourtis
 * Copyright (C) 2009-2012, Vasileios Karakasis
 * Copyright (C) 2010-2012, Theodoros Gkountouvas
 * All rights reserved.
 *
 * This file is distributed under the BSD License. See LICENSE.txt for details.
 */
#include <algorithm>
#include <vector>
#include <iostream>
#include <fstream>
#include <ios>
#include <exception>
#include <stdexcept>

#include <boost/foreach.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/function.hpp>
#define FOREACH BOOST_FOREACH

#include "spm.h"
#include "drle.h"

namespace bll = boost::lambda;

using namespace csx;

bool debug = false;

template<typename T>
struct RLE {
    long freq;
    T val;
};

template<typename T>
T DeltaEncode(T input)
{
    T output;
    typename T::iterator in, out;
    typename T::iterator::value_type prev, curr;

    output.resize(input.size());
    in = input.begin();
    out = output.begin();
    prev = *out++ = *in++;
    while (in < input.end()) {
        curr = *in++;
        *out++ = curr - prev;
        prev = curr;
    }

    return output;
}

template<typename T>
std::vector<RLE<typename T::iterator::value_type> >
RLEncode(T input)
{
    typename T::iterator in;
    typename T::iterator::value_type curr;
    std::vector<RLE<typename T::iterator::value_type> > output;
    RLE<typename T::iterator::value_type> rle;

    in = input.begin();
    rle.freq = 1;
    rle.val = *in++;
    while (in < input.end()) {
        curr = *in;
        if (rle.val != curr) {
            output.push_back(rle);
            rle.freq = 1;
            rle.val = curr;
        } else {
            ++rle.freq;
        }

        ++in;
    }

    output.push_back(rle);
    return output;
}

DRLE_Manager::DRLE_Manager(SPM *spm, long min_limit, long max_limit,
                           double min_perc, uint64_t sort_window_size,
                           split_alg_t split_type,
                           double sampling_portion,
                           uint64_t samples_max,
                           bool split_blocks,
                           bool onedim_blocks)
    : spm_(spm), min_limit_(min_limit),
      max_limit_(max_limit), min_perc_(min_perc),
      sort_window_size_(sort_window_size),
      split_type_(split_type),
      sampling_portion_(sampling_portion),
      samples_max_(samples_max),
      split_blocks_(split_blocks),
      onedim_blocks_(onedim_blocks)
{
    // These are delimiters, ignore them by default.
    AddIgnore(BLOCK_TYPE_START);
    AddIgnore(BLOCK_COL_START);
    AddIgnore(BLOCK_TYPE_END);

    if (!onedim_blocks_) {
        AddIgnore(BLOCK_ROW_TYPE_NAME(1));
        AddIgnore(BLOCK_COL_TYPE_NAME(1));
    }

    assert(sampling_portion_ >= 0 && sampling_portion_ <= 1 &&
           "invalid sampling portion");
    CheckAndSetSorting();
    if (sort_windows_) {
        if (sampling_portion > 0) {
            sort_window_size_ =
                sampling_portion_*spm_->GetNrNonzeros() / samples_max_;
        }

        ComputeSortSplits();
#if 0
        std::cout << "Window size (computed): "
                  << sort_window_size_ << std::endl;
        for (size_t i = 0; i < sort_splits_.size() - 1; ++i) {
            printf("(%ld, %ld)\n", sort_splits_[i+1] - sort_splits_[i],
                   sort_splits_nzeros_[i]);
        }
#endif

        if (samples_max_ > sort_splits_.size()) {
            samples_max_ = sort_splits_.size();
        }

        SelectSplits();
    } else {
        selected_splits_ = 0;
    }

    for (int i = 0; i < PRE_TIMER_END; i++)
        timer_init(&timers_[i]);
}

DeltaRLE::Stats DRLE_Manager::GenerateStats(uint64_t rs, uint64_t re)
{
    return GenerateStats(spm_, rs, re);
}

DeltaRLE::Stats DRLE_Manager::GenerateStats(SPM *spm, uint64_t rs, uint64_t re)
{
    std::vector<uint64_t> xs;
    DeltaRLE::Stats stats;

    for (uint64_t i = rs; i < re; ++i) {
        for (const SpmRowElem *elem = spm->RowBegin(i);
             elem != spm->RowEnd(i); ++elem) {
            if (elem->pattern == NULL) {
                xs.push_back(elem->x);
                continue;
            }

            UpdateStats(spm, xs, stats);
        }

        UpdateStats(spm, xs, stats);
    }
    return stats;
}

void DRLE_Manager::GenAllStats()
{
    DeltaRLE::Stats::iterator iter, tmp;
    DeltaRLE::Stats *sp;

    stats_.clear();
    deltas_to_encode_.clear();
    for (int t = HORIZONTAL; t != XFORM_MAX; ++t) {
        if (xforms_ignore_[t])
            continue;

        SpmIterOrder type = SpmTypes[t];

        // std::cout << "Checking for " << SpmTypesNames[t] << std::endl;
        if (sort_windows_ && spm_->GetNrRows() > samples_max_) {
            uint64_t samples_nnz = 0;
            for (size_t i = 0; i < samples_max_; ++i) {
                DeltaRLE::Stats w_stats;
                uint64_t window_start = sort_splits_[selected_splits_[i]];
                uint64_t window_size =
                    sort_splits_[selected_splits_[i]+1] - window_start;
                SPM *window = spm_->GetWindow(window_start, window_size);

                // std::cout << "wstart: " << window_start << std::endl;
                // std::cout << "wsize (rows): " << window_size << std::endl;
                // std::cout << "wsize (nnz): " <<
                //               sort_splits_nzeros_[selected_splits_[i]] <<
                //               std::endl;
                // Check for empty windows, since nonzeros might be captured
                // from previous patterns.
                if  (!window->nr_nzeros_)
                    goto exit_loop;

                samples_nnz += sort_splits_nzeros_[selected_splits_[i]];
                window->Transform(type);
                w_stats = GenerateStats(window, 0, window->GetNrRows());
                UpdateStats(type, w_stats);
                window->Transform(HORIZONTAL);
                spm_->PutWindow(window);
            exit_loop:
                delete window;
            }

            CorrectStats(type, spm_->nr_nzeros_ / (double) samples_nnz);
        } else {
            spm_->Transform(type);
            stats_[type] = GenerateStats(0, spm_->GetNrRows());
            spm_->Transform(HORIZONTAL);
        }

        sp = &stats_[type];

        uint64_t block_align = IsBlockType(type);

        if (block_align && split_blocks_)
            CorrectBlockStats(sp, block_align);

        for (iter = sp->begin(); iter != sp->end(); ) {
            tmp = iter++;
            double p = (double) tmp->second.nnz / (double) spm_->nr_nzeros_;
            if (block_align == 1 && (unsigned int) tmp->first < min_limit_)
                // the dimension of one-dimensional blocks must exceed
                // min_limit_
                sp->erase(tmp);
            else if (p < min_perc_ || tmp->first >= CSX_PID_OFFSET)
                sp->erase(tmp);
            else
                deltas_to_encode_[type].insert(tmp->first);
        }
    }
}

namespace csx {

void DRLE_OutStats(DeltaRLE::Stats &stats, SPM &spm, std::ostream &os)
{
    DeltaRLE::Stats::iterator iter;

    for (iter = stats.begin(); iter != stats.end(); ++iter) {
        os << "    " << iter->first << "-> " << "np:"
           << iter->second.npatterns
           << " nnz: " << iter->second.nnz
           << " ("
           <<  100 * ((double) iter->second.nnz / (double) spm.GetNrNonzeros())
           << "%" << ")";
    }
}

} // end of csx namespace

void DRLE_Manager::AddIgnore(SpmIterOrder type)
{
    xforms_ignore_.set(type);
}

void DRLE_Manager::IgnoreAll()
{
    xforms_ignore_.set();
}

void DRLE_Manager::RemoveIgnore(SpmIterOrder type)
{
    // the following types are always ignored
    if (type <= NONE || type == BLOCK_TYPE_START ||
        (type == BLOCK_ROW_TYPE_NAME(1) && !onedim_blocks_) ||
        type == BLOCK_COL_START ||
        (type == BLOCK_COL_TYPE_NAME(1) && !onedim_blocks_) ||
        type == BLOCK_TYPE_END ||
        type >= XFORM_MAX)
        return;
    xforms_ignore_.reset(type);
}

void DRLE_Manager::RemoveAll()
{
    for (int t = NONE; t < XFORM_MAX; ++t)
        xforms_ignore_.reset(t);
}

SpmIterOrder DRLE_Manager::ChooseType()
{
    SpmIterOrder ret;
    uint64_t max_out;
    DRLE_Manager::StatsMap::iterator iter;

    ret = NONE;
    max_out = 0;
    for (iter = stats_.begin(); iter != stats_.end(); ++iter){
        uint64_t out = GetTypeScore(iter->first);

        if (out == 0) {
            AddIgnore(iter->first);
        } else if (out > max_out) {
            max_out = out;
            ret = iter->first;
        }
    }

    return ret;
}

uint64_t DRLE_Manager::GetTypeScore(SpmIterOrder type)
{
    DeltaRLE::Stats *sp;
    DeltaRLE::Stats::iterator iter;
    uint64_t ret;

    ret = 0;
    if (stats_.find(type) == stats_.end())
        return ret;

    sp = &stats_[type];

    uint64_t nr_nzeros_encoded = 0;
    uint64_t nr_patterns = 0;

    for (iter = sp->begin(); iter != sp->end(); ++iter) {
        nr_nzeros_encoded += iter->second.nnz;
        nr_patterns += iter->second.npatterns;
    }

    ret = nr_nzeros_encoded - nr_patterns;
    return ret;
}

void DRLE_Manager::Encode(SpmIterOrder type)
{
    SPM::Builder *SpmBld;
    SpmIterOrder oldtype;
    std::vector<SpmRowElem> new_row;
    uint64_t nr_size;
    SpmRowElem *elems;

    if (type == NONE && ((type = ChooseType()) == NONE))
        return;

    SpmBld = new SPM::Builder(spm_, spm_->elems_size_, spm_->max_rowptr_size_);

    // Transform matrix to the desired iteration order
    oldtype = spm_->type_;
    spm_->Transform(type);

    for (uint64_t i = 0; i < spm_->GetNrRows(); ++i) {
        EncodeRow(spm_->RowBegin(i), spm_->RowEnd(i), new_row);
        nr_size = new_row.size();
        if (nr_size > 0) {
            timer_start(&timers_[PRE_TIMER_ALLOC]);
            elems = SpmBld->AllocElems(nr_size);
            timer_pause(&timers_[PRE_TIMER_ALLOC]);
            for (uint64_t i = 0; i < nr_size; ++i)
                MakeRowElem(new_row[i], elems + i);
        }

        new_row.clear();
        SpmBld->NewRow();
    }

    SpmBld->Finalize();
    delete SpmBld;

    // Transform matrix to the original iteration order
    spm_->Transform(oldtype);
    AddIgnore(type);
}

void DRLE_Manager::Decode(SpmIterOrder type)
{
    SPM::Builder *SpmBld;
    SpmIterOrder oldtype;
    std::vector<SpmRowElem> new_row;
    uint64_t nr_size;
    SpmRowElem *elems;

    if (type == NONE)
        return;

    // Transform matrix to the desired iteration order
    oldtype = spm_->type_;
    spm_->Transform(type);

    // Do the decoding
    SpmBld = new SPM::Builder(spm_, spm_->elems_size_, spm_->max_rowptr_size_);
    for (uint64_t i = 0; i < spm_->GetNrRows(); ++i) {
        DecodeRow(spm_->RowBegin(i), spm_->RowEnd(i), new_row);
        nr_size = new_row.size();
        if (nr_size > 0) {
            elems = SpmBld->AllocElems(nr_size);
            for (uint64_t i = 0; i < nr_size; ++i)
                MakeRowElem(new_row[i], elems + i);
	}

	new_row.clear();
        SpmBld->NewRow();
    }

    SpmBld->Finalize();
    delete SpmBld;

    // Transform matrix to the original iteration order
    spm_->Transform(oldtype);
    RemoveIgnore(type);
}

void DRLE_Manager::EncodeAll(std::ostream &os)
{
    SpmIterOrder type = NONE;
    StatsMap::iterator iter;
    SpmIterOrder enc_seq[22];
    int counter = 0;

    timer_start(&timers_[PRE_TIMER_TOTAL]);
    for (;;) {
        timer_start(&timers_[PRE_TIMER_STATS]);
        GenAllStats();
        timer_pause(&timers_[PRE_TIMER_STATS]);
        OutStats(os);
        type = ChooseType();
        if (type == NONE)
            break;
        timer_start(&timers_[PRE_TIMER_ENCODE]);
        os << "Encode to " << SpmTypesNames[type] << std::endl;

        Encode(type);

        enc_seq[counter++] = type;
        timer_pause(&timers_[PRE_TIMER_ENCODE]);
    }

    timer_pause(&timers_[PRE_TIMER_TOTAL]);

    os << "Encoding sequence: ";
    if (counter == 0)
        os << "NONE";
    else
        os << SpmTypesNames[enc_seq[0]];

    for (int i = 1; i < counter; i++)
        os << ", " << SpmTypesNames[enc_seq[i]];
    os << std::endl;

    /*
    double t;

    for (int i = 0; i < PRE_TIMER_END; i++) {
        t = timer_secs(&timers_[i]);
        os << timers_desc_[i] << t << std::endl;
    }
    */
}

void DRLE_Manager::MakeEncodeTree()
{
    uint32_t i,j;
    uint32_t count = 0;
    StatsMap::iterator iter;
    std::vector<Node> nodes;
    std::vector<Node>::iterator it,it2;

    nodes.push_back(Node(0));
    while (nodes.size() != 0) {
        for (i = 0; nodes[0].type_ignore_[i] != NONE; ++i)
            AddIgnore(nodes[0].type_ignore_[i]);

        GenAllStats();
        for (i = 0; nodes[0].type_ignore_[i] != NONE; ++i)
            RemoveIgnore(nodes[0].type_ignore_[i]);

        for (iter = stats_.begin(); iter != stats_.end(); ++iter)
            if (GetTypeScore(iter->first) == 0)
                nodes[0].Ignore(iter->first);

        it = nodes.begin();
        for (iter = stats_.begin(); iter != stats_.end(); ++iter) {
            if (GetTypeScore(iter->first) > 0) {
                ++it;
                it = nodes.insert(it, nodes[0].MakeChild(iter->first,
                                  deltas_to_encode_[iter->first]));
            }
        }

        if (it != nodes.begin()) {
            Encode(nodes[1].type_path_[nodes[1].depth_-1]);
        } else {
            nodes[0].PrintNode();
            ++count;
            if (nodes.size() != 1) {
                i = 0;
                while (nodes[0].type_path_[i] == nodes[1].type_path_[i])
                    ++i;
                for (j = 1; j <= nodes[0].depth_ - i; ++j)
                     Decode(nodes[0].type_path_[nodes[0].depth_-j]);

                while (i < nodes[1].depth_) {
                    Encode(nodes[1].type_path_[i]);
                    ++i;
                }
            } else {
                for (j = 1; j <= nodes[0].depth_; ++j)
                    Decode(nodes[0].type_path_[nodes[0].depth_-j]);
            }
        }

        delete nodes[0].type_path_;
        delete nodes[0].type_ignore_;
        nodes.erase(nodes.begin());
    }

    std::cout << "Tree has " << count << " possible paths" << std::endl;
    exit(0);
}

void DRLE_Manager::EncodeSerial(int *xform_buf, int **deltas)
{
    for (uint32_t i = 0; i < XFORM_MAX; ++i)
        AddIgnore((SpmIterOrder) i);

    for (int i = 0; xform_buf[i] != -1; ++i) {
        SpmIterOrder t = static_cast<SpmIterOrder>(xform_buf[i]);

        RemoveIgnore(t);
        for (int j = 0; deltas[i][j] != -1; ++j)
            deltas_to_encode_[t].insert(deltas[i][j]);

        Encode(t);
        AddIgnore(t);
    }
}

void DRLE_Manager::OutputSortSplits(std::ostream& out)
{
    sort_split_iterator iter;
    for (iter = sort_splits_.begin(); iter != sort_splits_.end() - 1; ++iter) {
        uint64_t rs = *iter;
        uint64_t re = *(iter + 1);
        uint64_t nnz = spm_->rowptr_[re] - spm_->rowptr_[rs];

        out << "(rs, re, nnz) = (" << rs << ", " << re << ", " << nnz << ")"
            << std::endl;
    }
}

void DRLE_Manager::DoEncode(std::vector<uint64_t> &xs, std::vector<double> &vs,
                            std::vector<SpmRowElem> &encoded)
{
    uint64_t col;
    std::vector< RLE<uint64_t> > rles;
    std::set<uint64_t> *deltas_set;
    std::vector<double>::iterator vi = vs.begin();
    SpmRowElem elem;

    if (IsBlockType(spm_->type_)) {
        if (!split_blocks_)
            DoEncodeBlock(xs, vs, encoded);
        else
            DoEncodeBlockAlt(xs, vs, encoded);
        return;
    }

    // do a delta run-length encoding of the x values
    rles = RLEncode(DeltaEncode(xs));

    // Not all delta rles are to be encoded, only those
    // that are in the ->deltas_to_encode set
    deltas_set = &deltas_to_encode_[spm_->type_];

    col = 0;
    elem.pattern = NULL;
    FOREACH (RLE<uint64_t> rle, rles) {
        long rle_freq, rle_start;
        rle_freq = rle.freq;
        if (rle_freq != 1 && deltas_set->find(rle.val) != deltas_set->end()) {

            col += rle.val;
            if (col != rle.val) {
                SpmRowElem *last_elem = &encoded.back();
                rle_start = col;
                rle_freq = rle.freq;
#ifndef SPM_NUMA
                if (last_elem->pattern == NULL) {
                    // include the previous element, too
                    rle_start -= rle.val;
                    rle_freq++;
                    encoded.pop_back();
                    --vi;
                }
#endif
            } else {
                // we are the first unit in the row
                rle_start = col;
                rle_freq = rle.freq;
            }

            int nr_parts = 0;
            while (rle_freq >= min_limit_) {
                SpmRowElem *last_elem;
                long curr_freq = std::min(max_limit_, rle_freq);

                elem.x = rle_start;
                encoded.push_back(elem);
                last_elem = &encoded.back();
                last_elem->pattern = new DeltaRLE(curr_freq, rle.val,
                                                  spm_->type_);
                last_elem->vals = new double[curr_freq];
                std::copy(vi, vi + curr_freq, last_elem->vals);
                vi += curr_freq;
                rle_start += rle.val * curr_freq;
                rle_freq -= curr_freq;
                ++nr_parts;
            }

            // leave col at the last element of the pattern
            col = rle_start - rle.val;
        }

        // add individual elements
        for (int i = 0; i < rle_freq; ++i) {
            col += rle.val;
            elem.x = col;
            elem.val = *vi++;
            encoded.push_back(elem);
        }
    }

    assert(vi == vs.end() && "not all elements processed or out of bounds");
    xs.clear();
    vs.clear();
}

//static uint64_t nr_lines = 0;
void DRLE_Manager::DoEncodeBlock(std::vector<uint64_t> &xs,
                                 std::vector<double> &vs,
                                 std::vector<SpmRowElem> &encoded)
{
    uint64_t col;
    std::vector< RLE<uint64_t> > rles;
    std::set<uint64_t> *deltas_set;
    std::vector<double>::iterator vi = vs.begin();
    SpmRowElem elem;
    SpmRowElem *last_elem;

    // do a delta run-length encoding of the x values
    rles = RLEncode(DeltaEncode(xs));

    // Not all delta rles are to be encoded, only those
    // that are in the ->deltas_to_encode set
    deltas_set = &deltas_to_encode_[spm_->type_];

    int block_align = IsBlockType(spm_->type_);
    assert(block_align && "not a block type");

    col = 0;
    elem.pattern = NULL;
    FOREACH (RLE<uint64_t> rle, rles) {
        uint64_t skip_front, skip_back, nr_elem;

        col += rle.val;
        if (col == 1) {
            skip_front = 0;
            nr_elem = rle.freq;
        } else {
            skip_front = (col - 2) % block_align;
            if (skip_front != 0)
                skip_front  = block_align - skip_front;

            nr_elem = rle.freq + 1;
        }

        if (nr_elem > skip_front)
            nr_elem -= skip_front;
        else
            nr_elem = 0;

        skip_back = nr_elem % block_align;
        if (nr_elem > skip_back)
            nr_elem -= skip_back;
        else
            nr_elem = 0;

        if (rle.val == 1 &&
            deltas_set->find(nr_elem / block_align) != deltas_set->end() &&
            nr_elem >= (uint64_t) 2 * block_align) {

            uint64_t rle_start;

            if (col != 1) {
                rle_start = col - 1;
                encoded.pop_back();
                --vi;
            } else {
                rle_start = col;
            }

            // Add elements skipped from start
            for (uint64_t i = 0; i < skip_front; ++i) {
                elem.x = rle_start + i;
                elem.val = *vi++;
                encoded.push_back(elem);
            }

            // Align max_limit
            uint64_t max_limit = max_limit_ / block_align * block_align;
            uint64_t nr_blocks = nr_elem / max_limit;
            uint64_t nr_elem_block = std::min(max_limit, nr_elem);

            if (nr_blocks == 0)
                nr_blocks = 1;
            else
                // Adjust skip_back with leftover items from max_limit alignment
                skip_back += nr_elem - nr_elem_block * nr_blocks;

            for (uint64_t i = 0; i < nr_blocks; ++i) {
                // Add the blocks
                elem.x = rle_start + skip_front + i * nr_elem_block;
                encoded.push_back(elem);
                last_elem = &encoded.back();
                last_elem->pattern = new BlockRLE(nr_elem_block,
                                                  nr_elem_block / block_align,
                                                  spm_->type_);
                last_elem->vals = new double[nr_elem_block];
                std::copy(vi, vi + nr_elem_block, last_elem->vals);
                vi += nr_elem_block;
            }

            // Add the remaining elements
            for (uint64_t i = 0; i < skip_back; ++i) {
                elem.x = rle_start + skip_front + nr_elem_block * nr_blocks + i;
                elem.val = *vi++;
                encoded.push_back(elem);
            }
        } else {
            // add individual elements
            for (int i = 0; i < rle.freq; ++i) {
                elem.x = col + i * rle.val;
                elem.val = *vi++;
                encoded.push_back(elem);
            }
        }

        col += rle.val * (rle.freq - 1);
    }

    assert(vi == vs.end() && "out of bounds");
    xs.clear();
    vs.clear();
}

void DRLE_Manager::DoEncodeBlockAlt(std::vector<uint64_t> &xs,
                                    std::vector<double> &vs,
                                    std::vector<SpmRowElem> &encoded)
{
    uint64_t col;
    std::vector< RLE<uint64_t> > rles;
    std::set<uint64_t> *deltas_set;
    std::vector<double>::iterator vi = vs.begin();
    SpmRowElem elem;
    SpmRowElem *last_elem;

    // do a delta run-length encoding of the x values
    rles = RLEncode(DeltaEncode(xs));

    // Not all delta rles are to be encoded, only those
    // that are in the ->deltas_to_encode set
    deltas_set = &deltas_to_encode_[spm_->type_];
    int block_align = IsBlockType(spm_->type_);
    assert(block_align && "not a block type");

    col = 0;
    elem.pattern = NULL;
    FOREACH (RLE<uint64_t> rle, rles) {
        uint64_t skip_front, skip_back, nr_elem;

        col += rle.val;
        if (col == 1) {
            skip_front = 0;
            nr_elem = rle.freq;
        } else {
            skip_front = (col - 2) % block_align;
            if (skip_front != 0)
                skip_front  = block_align - skip_front;
            nr_elem = rle.freq + 1;
        }

        if (nr_elem > skip_front)
            nr_elem -= skip_front;
        else
            nr_elem = 0;
        skip_back = nr_elem % block_align;
        nr_elem -= skip_back;
        if (rle.val == 1 && nr_elem >= (uint64_t) 2 * block_align) {
            uint64_t rle_start;

            if (col != 1) {
                rle_start = col - 1;
                encoded.pop_back();
                --vi;
            } else {
                rle_start = col;
            }

            // Add elements skipped from start
            for (uint64_t i = 0; i < skip_front; ++i) {
                elem.x = rle_start++;
                elem.val = *vi++;
                encoded.push_back(elem);
            }

            // Split Blocks to Match the Appropriate Sizes for Encode
            uint64_t other_dim = nr_elem / block_align;

            for (std::set<uint64_t>::reverse_iterator i = deltas_set->rbegin();
                                              i != deltas_set->rend(); ++i) {
                while (other_dim >= *i) {
                    //We have a new block RLE
                    uint64_t nr_elem_block = block_align * (*i);

                    elem.x = rle_start;
                    rle_start += nr_elem_block;
                    encoded.push_back(elem);
                    last_elem = &encoded.back();
                    last_elem->pattern = new BlockRLE(nr_elem_block, *i,
                                                      spm_->type_);
                    last_elem->vals = new double[nr_elem_block];
                    std::copy(vi, vi + nr_elem_block, last_elem->vals);
                    vi += nr_elem_block;
                    nr_elem -= nr_elem_block;
                    other_dim -= (*i);
                }
            }

            // Add the remaining elements
            skip_back += nr_elem;
            for (uint64_t i = 0; i < skip_back; ++i) {
                elem.x = rle_start++;
                elem.val = *vi++;
                encoded.push_back(elem);
            }
        } else {
            // add individual elements
            for (int i = 0; i < rle.freq; ++i) {
                elem.x = col + i * rle.val;
                elem.val = *vi++;
                encoded.push_back(elem);
            }
        }

        col += rle.val * (rle.freq - 1);
    }

    assert(vi == vs.end() && "out of bounds");
    xs.clear();
    vs.clear();
}

void DRLE_Manager::DoDecode(const SpmRowElem *elem,
                            std::vector<SpmRowElem> &newrow)
{
    long int i;
    uint64_t cur_x;
    SpmRowElem new_elem;

    new_elem.pattern = NULL;
    cur_x = elem->x;
    for (i = 0; i < elem->pattern->GetSize(); ++i) {
        new_elem.x = cur_x;
        new_elem.val = elem->vals[i];
        newrow.push_back(new_elem);
        cur_x = elem->pattern->GetNextCol(cur_x);
    }

    delete elem->pattern;
    delete elem->vals;
}

void DRLE_Manager::EncodeRow(const SpmRowElem *rstart, const SpmRowElem *rend,
                             std::vector<SpmRowElem> &newrow)
{
    std::vector<uint64_t> xs;
    std::vector<double> vs;

    // gather x values into xs vector until a pattern is found
    // and encode them using DoEncode()
    for (const SpmRowElem *e = rstart; e < rend; ++e) {
        if (e->pattern == NULL) {
            xs.push_back(e->x);
            vs.push_back(e->val);
            continue;
        }

        if (xs.size() != 0)
            DoEncode(xs, vs, newrow);

        newrow.push_back(*e);

        if (e->pattern)
            delete e->pattern;
    }

    // Encode any remaining elements
    if (xs.size() != 0)
        DoEncode(xs, vs, newrow);
}

void DRLE_Manager::DecodeRow(const SpmRowElem *rstart,
                             const SpmRowElem *rend,
                             std::vector<SpmRowElem> &newrow)
{
    for (const SpmRowElem *e = rstart; e < rend; ++e) {
        if (e->pattern != NULL && e->pattern->GetType() == spm_->type_) {
            DoDecode(e, newrow);
        } else {
            newrow.push_back(*e);
            if (e->pattern)
                delete e->pattern;
        }
    }
}

void DRLE_Manager::CorrectBlockStats(DeltaRLE::Stats *sp, uint64_t block_align)
{
    DeltaRLE::Stats temp, temp2;
    uint64_t max_block_dim;
    DeltaRLE::Stats::iterator tmp;
    DeltaRLE::Stats::reverse_iterator iter, rtmp;

    max_block_dim = max_limit_ / block_align;
    temp[max_block_dim].nnz = 0;
    temp[max_block_dim].npatterns = 0;
    for (iter = sp->rbegin(); iter != sp->rend() &&
         iter->first * block_align > (unsigned) max_limit_; ++iter) {

        uint64_t div_factor = iter->first / max_block_dim;
        uint64_t mod_factor = iter->first % max_block_dim;
        uint64_t max_block_size = max_block_dim * block_align;
        uint64_t block_patterns = div_factor * iter->second.npatterns;
        uint64_t remaining_nnz;
        temp[max_block_dim].nnz += block_patterns * max_block_size;
        temp[max_block_dim].npatterns += block_patterns;
        remaining_nnz = iter->second.nnz - block_patterns * max_block_size;
        if (mod_factor >= 2) {
            tmp = temp.find(mod_factor);
            if (tmp == temp.end()) {
                temp[mod_factor].nnz = remaining_nnz;
                temp[mod_factor].npatterns = iter->second.npatterns;
            } else {
                temp[mod_factor].nnz += remaining_nnz;
                temp[mod_factor].npatterns += iter->second.npatterns;
            }
        }
    }

    if (temp[max_block_dim].npatterns == 0) {
        assert(temp[max_block_dim].nnz == 0);
        temp.erase(max_block_dim);
    }

    for ( ; iter != sp->rend(); ++iter) {
        tmp = temp.find(iter->first);
        if (tmp == temp.end()) {
            temp[iter->first].nnz = iter->second.nnz;
            temp[iter->first].npatterns = iter->second.npatterns;
        } else {
            temp[iter->first].nnz += iter->second.nnz;
            temp[iter->first].npatterns += iter->second.npatterns;
        }
    }

    uint64_t div_factor, mod_factor;

    rtmp = sp->rbegin();
    for (iter = temp.rbegin(); iter != temp.rend(); ++iter) {
        if (((double) iter->second.nnz) / ((double) spm_->nr_nzeros_) >=
            min_perc_) {
            for ( ; rtmp != sp->rend() && rtmp->first >= iter->first; ++rtmp) {
                div_factor = rtmp->first / iter->first;
                mod_factor = rtmp->first % iter->first;

                tmp = temp2.find(iter->first);
                if (tmp == temp2.end()) {
                    temp2[iter->first].npatterns =
                        div_factor * rtmp->second.npatterns;
                    temp2[iter->first].nnz =
                        div_factor * (rtmp->second.nnz / rtmp->first) *
                        iter->first;
                } else {
                    tmp->second.npatterns +=
                        div_factor * rtmp->second.npatterns;
                    tmp->second.nnz +=
                        div_factor * (rtmp->second.nnz / rtmp->first) *
                        iter->first;
                }

                if (mod_factor >= 2) {
                    tmp = temp2.find(mod_factor);
                    if (tmp == temp2.end()) {
                        temp2[mod_factor].npatterns = rtmp->second.npatterns;
                        temp2[mod_factor].nnz =
                            (rtmp->second.nnz / rtmp->first) * mod_factor;
                    } else {
                        tmp->second.npatterns += rtmp->second.npatterns;
                        tmp->second.nnz +=
                            (rtmp->second.nnz / rtmp->first) * mod_factor;
                    }
                }
            }
        }
    }
    sp->clear();
    *sp = temp2;
}

void DRLE_Manager::HandleStats(DeltaRLE::Stats *sp, uint64_t size,
                               uint64_t block_align)
{
    DeltaRLE::Stats temp;
    DeltaRLE::Stats::iterator iter, not_passed, tmp, tmp2;

    not_passed = sp->end();
    for (iter = sp->end(); iter != sp->begin(); ) {
        double p = 0;

        iter--;
        if (iter->first * block_align <= (unsigned) max_limit_) {
            for (tmp = iter; tmp != not_passed; ++tmp)
                p += ((double) tmp->second.npatterns * block_align *
                     iter->first * (tmp->first / iter->first) / (double) size);

            if (p > min_perc_) {
                uint64_t block_size = iter->first * block_align;

                temp[iter->first].nnz = iter->second.nnz;
                temp[iter->first].npatterns = iter->second.npatterns;
                tmp = iter;
                ++tmp;
                for (; tmp != not_passed; ++tmp) {
                    uint64_t div_factor = tmp->first / iter->first;
                    uint64_t mod_factor = tmp->first % iter->first;
                    temp[iter->first].nnz +=
                        tmp->second.npatterns * div_factor * block_size;
                    temp[iter->first].npatterns +=
                        tmp->second.npatterns * div_factor;
                    tmp->second.nnz -=
                        tmp->second.npatterns * div_factor * block_size;
                    if (mod_factor >= 2) {
                        tmp2 = temp.find(mod_factor);
                        if (tmp2 == temp.end()) {
                            temp[mod_factor].nnz = tmp->second.nnz;
                            temp[mod_factor].npatterns = tmp->second.npatterns;
                        } else {
                            temp[mod_factor].nnz = tmp->second.nnz;
                            temp[mod_factor].npatterns = tmp->second.npatterns;
                        }
                    }
                }

                not_passed = iter;
            }
        }
    }

    for (iter = sp->begin(); iter != sp->end(); ++iter)
        sp->erase(iter);

    for (iter = temp.begin(); iter != temp.end(); ++iter)
        sp->insert(std::pair<const long unsigned int,
                   csx::DeltaRLE::StatsVal>(iter->first,iter->second));
}

void DRLE_Manager::UpdateStats(SpmIterOrder type, DeltaRLE::Stats stats)
{
    if (stats_.find(type) == stats_.end()) {
        stats_[type] = stats;
    } else {
        for (DeltaRLE::Stats::const_iterator it = stats.begin();
            it != stats.end(); ++it) {
            stats_[type][it->first].Update(it->second);
        }
    }
}

void DRLE_Manager::UpdateStats(std::vector<uint64_t> &xs,
                               DeltaRLE::Stats &stats)
{
    UpdateStats(spm_, xs, stats);
}

void DRLE_Manager::UpdateStats(SPM *spm, std::vector<uint64_t> &xs,
                               DeltaRLE::Stats &stats)
{
    std::vector< RLE<uint64_t> > rles;
    uint64_t block_align = IsBlockType(spm->type_);

    if (block_align) {
        DRLE_Manager::UpdateStatsBlock(xs, stats, block_align);
        return;
    }

    if (xs.size() == 0)
        return;

    rles = RLEncode(DeltaEncode(xs));
    uint64_t col = 0;
    bool last_rle_patt = false; // turn on for encoded units
    FOREACH(RLE<uint64_t> &rle, rles) {
#ifndef SPM_NUMA
        long real_limit = (col && !last_rle_patt) ? min_limit_ - 1 : min_limit_;
#else
        long real_limit = min_limit_;
#endif
        if (rle.freq > 1 && rle.freq >= real_limit) {
#ifndef SPM_NUMA
            long real_nnz = (col && !last_rle_patt) ?
                rle.freq + 1 : rle.freq;
#else
            long real_nnz = rle.freq;
#endif
            stats[rle.val].nnz += real_nnz;
            stats[rle.val].npatterns += (real_nnz) / max_limit_ +
                                        (real_nnz % max_limit_ != 0);
            last_rle_patt = true;
        } else {
            last_rle_patt = false;
        }
        col += rle.val;
    }
    xs.clear();
}

void DRLE_Manager::UpdateStatsBlock(std::vector<uint64_t> &xs,
                                    DeltaRLE::Stats &stats,
                                    uint64_t block_align)
{
    std::vector<RLE<uint64_t> > rles;

    assert(block_align && "not a block type");
    if (xs.size() == 0)
        return;

    rles = RLEncode(DeltaEncode(xs));

    uint64_t unit_start = 0;

    FOREACH (RLE<uint64_t> &rle, rles) {
        unit_start += rle.val;
        if (rle.val == 1) {
            // Start of the real block is at `unit_start - 1' with
            // one-based indexing. When computing the `%' we need
            // zero-based indexing.
            uint64_t nr_elem;
            uint64_t skip_front;

            if (unit_start == 1) {
                skip_front = 0;
                nr_elem = rle.freq;
            } else {
                skip_front = (unit_start-2) % block_align;
                if (skip_front != 0)
                    skip_front = block_align - skip_front;
                nr_elem = rle.freq + 1;
            }

            if (nr_elem > skip_front)
                nr_elem -= skip_front;
            else
                nr_elem = 0;

            uint64_t other_dim = nr_elem / block_align;

            if (other_dim >= 2) {
                stats[other_dim].nnz += other_dim * block_align;
                stats[other_dim].npatterns++;
            }
        }

        unit_start += rle.val * (rle.freq - 1);
    }

    xs.clear();
}

void DRLE_Manager::CorrectStats(SpmIterOrder type, double factor)
{
    if (stats_.find(type) == stats_.end())
        return;

    DeltaRLE::Stats *l_stats = &stats_[type];
    DeltaRLE::Stats::iterator iter;

    for (iter = l_stats->begin(); iter != l_stats->end(); ++iter) {
        iter->second.nnz = (uint64_t) (iter->second.nnz * factor);
        iter->second.npatterns = (long) (iter->second.npatterns * factor);
        assert(iter->second.nnz <= spm_->GetNrNonzeros() &&
               "nonzeros of pattern exceed nonzeros of matrix");
    }
}

void DRLE_Manager::SelectSplits()
{
    size_t nr_splits = sort_splits_.size();
    size_t nr_samples = samples_max_;

    selected_splits_ = new size_t[nr_samples];
    if (nr_samples == nr_splits) {
        for (size_t i = 0; i < nr_splits; ++i)
            selected_splits_[i] = i;
        return;
    }

    if (nr_samples > nr_splits / 2) {
        for (size_t i = 0; i < nr_splits/2; ++i)
            selected_splits_[i] = i;
        nr_samples -= nr_splits / 2;
        nr_splits -= nr_splits / 2;
    }

    size_t skip = nr_splits / (nr_samples+1);
    for (size_t i = 0; i < nr_samples; ++i) {
        selected_splits_[i] = (i+1)*skip;
    }

    return;
}

void DRLE_Manager::ComputeSortSplits()
{
    switch (split_type_) {
    case DRLE_Manager::SPLIT_BY_ROWS:
        DoComputeSortSplitsByRows();
        break;
    case DRLE_Manager::SPLIT_BY_NNZ:
        DoComputeSortSplitsByNNZ();
        break;
    default:
        assert(false && "unknown split algorithm");
    }
}

void DRLE_Manager::CheckAndSetSorting()
{
    switch (split_type_) {
    case DRLE_Manager::SPLIT_BY_ROWS:
        DoCheckSortByRows();
        break;
    case DRLE_Manager::SPLIT_BY_NNZ:
        DoCheckSortByNNZ();
        break;
    default:
        assert(false && "unknown split algorithm");
    }
}

void DRLE_Manager::DoComputeSortSplitsByRows()
{
    uint64_t nr_rows = spm_->GetNrRows();
    uint64_t i;

    for (i = 0; i <= nr_rows; i += sort_window_size_)
        sort_splits_.push_back(i);

    if (i > nr_rows && i - nr_rows < sort_window_size_ / 2) {
        sort_splits_.push_back(nr_rows);
    } else {
        sort_splits_.pop_back();
        sort_splits_.push_back(nr_rows);
    }
}

void DRLE_Manager::DoComputeSortSplitsByNNZ()
{
    uint64_t nzeros_cnt;
    uint64_t nr_rows = spm_->GetNrRows();

    nzeros_cnt = 0;
    sort_splits_.push_back(0);
    for (uint64_t i = 0; i < nr_rows; ++i) {
        uint64_t new_nzeros_cnt =
            nzeros_cnt + spm_->rowptr_[i+1] - spm_->rowptr_[i];
        if (new_nzeros_cnt < sort_window_size_) {
            nzeros_cnt = new_nzeros_cnt;
        } else {
            sort_splits_.push_back(i + 1);
            sort_splits_nzeros_.push_back(new_nzeros_cnt);
            nzeros_cnt = 0;
        }
    }

    if (nzeros_cnt) {
        uint64_t &last_split_nz = sort_splits_nzeros_.back();
        last_split_nz += nzeros_cnt;
        if (nzeros_cnt > sort_window_size_ / 2) {
            sort_splits_.push_back(nr_rows);
        } else {
            sort_splits_.pop_back();
            sort_splits_.push_back(nr_rows);

        }
    }
}

void DRLE_Manager::DoCheckSortByRows()
{
    assert(sort_window_size_ <= spm_->GetNrRows() && "invalid sort window");
    if (sampling_portion_ == 0.0 || sort_window_size_ == spm_->GetNrRows())
        sort_windows_ = false;
    else
        sort_windows_ = true;
}

void DRLE_Manager::DoCheckSortByNNZ()
{
    assert(sort_window_size_ <= spm_->elems_size_ && "invalid sort window");
    if (sampling_portion_ == 0 || sort_window_size_ == spm_->elems_size_)
        sort_windows_ = false;
    else
        sort_windows_ = true;
}

void DRLE_Manager::OutStats(std::ostream &os)
{
    DRLE_Manager::StatsMap::iterator iter;
    for (iter = stats_.begin(); iter != stats_.end(); ++iter){
         if (!iter->second.empty()) {
             os << SpmTypesNames[iter->first] << "\ts:"
                << GetTypeScore(iter->first);
             DRLE_OutStats(iter->second, *(spm_), os);
             os << std::endl;
         }
    }
}

Node::Node(uint32_t depth) : depth_(depth)
{
    uint32_t i;

    type_path_ = new SpmIterOrder[depth_];
    type_ignore_ = new SpmIterOrder[XFORM_MAX];
    for (i = 0; i < ((uint32_t) XFORM_MAX); i++)
        type_ignore_[i] = NONE;
}

void Node::PrintNode()
{
    for (uint32_t i = 0; i < depth_; ++i) {
        if (i != 0)
            std::cout << ",";

        std::cout << type_path_[i];
    }

    std::cout << std::endl;
    for (uint32_t i = 0; i < depth_; ++i) {
        SpmIterOrder temp_type = type_path_[i];
        std::set<uint64_t>::iterator it = deltas_path_[temp_type].begin();

        if (i != 0)
            std::cout << ",";

        std::cout << "{";
        for (uint32_t i = 1;
             i < static_cast<uint32_t>(deltas_path_[temp_type].size());
             ++i) {
            std::cout << *it << ",";
            ++it;
        }

        std::cout << *it << "}";
    }

    std::cout << std::endl;
}

void Node::Ignore(SpmIterOrder type)
{
    uint32_t i = 0;

    while (type_ignore_[i] != NONE) {
        assert(type_ignore_[i] != type && "type already ignored");
        ++i;
    }

    type_ignore_[i] = type;
}

Node Node::MakeChild(SpmIterOrder type, std::set<uint64_t> deltas)
{
    Node new_node = Node(depth_ + 1);

    for (uint32_t i = 0; i < static_cast<uint32_t>(XFORM_MAX); ++i)
        new_node.type_ignore_[i] = type_ignore_[i];

    for (uint32_t i = 0; i < depth_; ++i) {
        SpmIterOrder temp_type = type_path_[i];
        new_node.type_path_[i] = temp_type;
        new_node.deltas_path_[temp_type] = deltas_path_[temp_type];
    }

    new_node.type_path_[depth_] = type;
    new_node.deltas_path_[type] = deltas;
    return new_node;
}

// vim:expandtab:tabstop=8:shiftwidth=4:softtabstop=4


//==================================================================================================
/*!
  @file

  @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_CONSTANT_VALMIN_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_VALMIN_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Valmin Valmin (function template)

  Generates the minimum signed finite representable value of a given type.

  @headerref{<boost/simd/constant/valmin.hpp>}

  @par Description

  1.  @code
      template<typename T> T Valmin();
      @endcode

  2.  @code
      template<typename T> T Valmin( boost::simd::as_<T> const& target );
      @endcode

  Generates the minimum signed finite representable value of a given type.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to:

  - `T(std::numeric_limits<scalar_of_t<T>>::min())` if @c T models IntegerValue
  - `T(-std::numeric_limits<scalar_of_t<T>>::max())` if @c T models IEEEValue

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/valmin.hpp>
#include <boost/simd/constant/simd/valmin.hpp>

#endif


#include <cstdlib>
#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>

#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "filelib.h"
#include "weights.h"
#include "sparse_vector.h"
#include "optimize.h"
#include "liblbfgs/lbfgs++.h"

using namespace std;
namespace po = boost::program_options;

// since this is a ranking model, there should be equal numbers of
// positive and negative examples, so the bias should be 0

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("weights,w", po::value<string>(), "Weights from previous iteration (used as initialization and interpolation")
        ("regularization_strength,C",po::value<double>()->default_value(500.0), "l2 regularization strength")
        ("l1",po::value<double>()->default_value(0.0), "l1 regularization strength")
        ("regularize_to_weights,y",po::value<double>()->default_value(5000.0), "Differences in learned weights to previous weights are penalized with an l2 penalty with this strength; 0.0 = no effect")
        ("memory_buffers,m",po::value<unsigned>()->default_value(100), "Number of memory buffers (LBFGS)")
        ("min_reg,r",po::value<double>()->default_value(0.01), "When tuning (-T) regularization strength, minimum regularization strenght")
        ("max_reg,R",po::value<double>()->default_value(1e6), "When tuning (-T) regularization strength, maximum regularization strenght")
        ("testset,t",po::value<string>(), "Optional held-out test set")
        ("tune_regularizer,T", "Use the held out test set (-t) to tune the regularization strength")
        ("interpolate_with_weights,p",po::value<double>()->default_value(1.0), "[deprecated] Output weights are p*w + (1-p)*w_prev; 1.0 = no effect")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

void ParseSparseVector(string& line, size_t cur, SparseVector<weight_t>* out) {
  SparseVector<weight_t>& x = *out;
  size_t last_start = cur;
  size_t last_comma = string::npos;
  while(cur <= line.size()) {
    if (line[cur] == ' ' || cur == line.size()) {
      if (!(cur > last_start && last_comma != string::npos && cur > last_comma)) {
        cerr << "[ERROR] " << line << endl << "  position = " << cur << endl;
        exit(1);
      }
      const int fid = FD::Convert(line.substr(last_start, last_comma - last_start));
      if (cur < line.size()) line[cur] = 0;
      const weight_t val = strtod(&line[last_comma + 1], NULL);
      x.set_value(fid, val);

      last_comma = string::npos;
      last_start = cur+1;
    } else {
      if (line[cur] == '=')
        last_comma = cur;
    }
    ++cur;
  }
}

void ReadCorpus(istream* pin, vector<pair<bool, SparseVector<weight_t> > >* corpus) {
  istream& in = *pin;
  corpus->clear();
  bool flag = false;
  int lc = 0;
  string line;
  SparseVector<weight_t> x;
  while(getline(in, line)) {
    ++lc;
    if (lc % 1000 == 0) { cerr << '.'; flag = true; }
    if (lc % 40000 == 0) { cerr << " [" << lc << "]\n"; flag = false; }
    if (line.empty()) continue;
    const size_t ks = line.find("\t");
    assert(string::npos != ks);
    assert(ks == 1);
    const bool y = line[0] == '1';
    x.clear();
    ParseSparseVector(line, ks + 1, &x);
    corpus->push_back(make_pair(y, x));
  }
  if (flag) cerr << endl;
}

void GradAdd(const SparseVector<weight_t>& v, const double scale, weight_t* acc) {
  for (SparseVector<weight_t>::const_iterator it = v.begin();
       it != v.end(); ++it) {
    acc[it->first] += it->second * scale;
  }
}

double ApplyRegularizationTerms(const double C,
                                const double T,
                                const vector<weight_t>& weights,
                                const vector<weight_t>& prev_weights,
                                weight_t* g) {
  double reg = 0;
  for (size_t i = 0; i < weights.size(); ++i) {
    const double prev_w_i = (i < prev_weights.size() ? prev_weights[i] : 0.0);
    const double& w_i = weights[i];
    reg += C * w_i * w_i;
    g[i] += 2 * C * w_i;

    const double diff_i = w_i - prev_w_i;
    reg += T * diff_i * diff_i;
    g[i] += 2 * T * diff_i;
  }
  return reg;
}

double TrainingInference(const vector<weight_t>& x,
                         const vector<pair<bool, SparseVector<weight_t> > >& corpus,
                         weight_t* g = NULL) {
  double cll = 0;
  for (int i = 0; i < corpus.size(); ++i) {
    const double dotprod = corpus[i].second.dot(x) + (x.size() ? x[0] : weight_t()); // x[0] is bias
    double lp_false = dotprod;
    double lp_true = -dotprod;
    if (0 < lp_true) {
      lp_true += log1p(exp(-lp_true));
      lp_false = log1p(exp(lp_false));
    } else {
      lp_true = log1p(exp(lp_true));
      lp_false += log1p(exp(-lp_false));
    }
    lp_true*=-1;
    lp_false*=-1;
    if (corpus[i].first) {  // true label
      cll -= lp_true;
      if (g) {
        // g -= corpus[i].second * exp(lp_false);
        GradAdd(corpus[i].second, -exp(lp_false), g);
        g[0] -= exp(lp_false); // bias
      }
    } else {                  // false label
      cll -= lp_false;
      if (g) {
        // g += corpus[i].second * exp(lp_true);
        GradAdd(corpus[i].second, exp(lp_true), g);
        g[0] += exp(lp_true); // bias
      }
    }
  }
  return cll;
}

struct ProLoss {
  ProLoss(const vector<pair<bool, SparseVector<weight_t> > >& tr,
          const vector<pair<bool, SparseVector<weight_t> > >& te,
          const double c,
          const double t,
          const vector<weight_t>& px) : training(tr), testing(te), C(c), T(t), prev_x(px){}
  double operator()(const vector<double>& x, double* g) const {
    fill(g, g + x.size(), 0.0);
    double cll = TrainingInference(x, training, g);
    tppl = 0;
    if (testing.size())
      tppl = pow(2.0, TrainingInference(x, testing, g) / (log(2) * testing.size()));
    double ppl = cll / log(2);
    ppl /= training.size();
    ppl = pow(2.0, ppl);
    double reg = ApplyRegularizationTerms(C, T, x, prev_x, g);
    return cll + reg;
  }
  const vector<pair<bool, SparseVector<weight_t> > >& training, testing;
  const double C, T;
  const vector<double>& prev_x;
  mutable double tppl;
};

// return held-out log likelihood
double LearnParameters(const vector<pair<bool, SparseVector<weight_t> > >& training,
                       const vector<pair<bool, SparseVector<weight_t> > >& testing,
                       const double C,
                       const double C1,
                       const double T,
                       const unsigned memory_buffers,
                       const vector<weight_t>& prev_x,
                       vector<weight_t>* px) {
  assert(px->size() == prev_x.size());
  ProLoss loss(training, testing, C, T, prev_x);
  LBFGS<ProLoss> lbfgs(px, loss, memory_buffers, C1);
  lbfgs.MinimizeFunction();
  return loss.tppl;
}

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  string line;
  vector<pair<bool, SparseVector<weight_t> > > training, testing;
  const bool tune_regularizer = conf.count("tune_regularizer");
  if (tune_regularizer && !conf.count("testset")) {
    cerr << "--tune_regularizer requires --testset to be set\n";
    return 1;
  }
  const double min_reg = conf["min_reg"].as<double>();
  const double max_reg = conf["max_reg"].as<double>();
  double C = conf["regularization_strength"].as<double>(); // will be overridden if parameter is tuned
  double C1 = conf["l1"].as<double>(); // will be overridden if parameter is tuned
  const double T = conf["regularize_to_weights"].as<double>();
  assert(C >= 0.0);
  assert(min_reg >= 0.0);
  assert(max_reg >= 0.0);
  assert(max_reg > min_reg);
  const double psi = conf["interpolate_with_weights"].as<double>();
  if (psi < 0.0 || psi > 1.0) { cerr << "Invalid interpolation weight: " << psi << endl; return 1; }
  ReadCorpus(&cin, &training);
  if (conf.count("testset")) {
    ReadFile rf(conf["testset"].as<string>());
    ReadCorpus(rf.stream(), &testing);
  }
  cerr << "Number of features: " << FD::NumFeats() << endl;

  vector<weight_t> x, prev_x;  // x[0] is bias
  if (conf.count("weights")) {
    Weights::InitFromFile(conf["weights"].as<string>(), &x);
    x.resize(FD::NumFeats());
    prev_x = x;
  } else {
    x.resize(FD::NumFeats());
    prev_x = x;
  }
  cerr << "         Number of features: " << x.size() << endl;
  cerr << "Number of training examples: " << training.size() << endl;
  cerr << "Number of  testing examples: " << testing.size() << endl;
  double tppl = 0.0;
  vector<pair<double,double> > sp;
  vector<double> smoothed;
  if (tune_regularizer) {
    C = min_reg;
    const double steps = 18;
    double sweep_factor = exp((log(max_reg) - log(min_reg)) / steps);
    cerr << "SWEEP FACTOR: " << sweep_factor << endl;
    while(C < max_reg) {
      cerr << "C=" << C << "\tT=" <<T << endl;
      tppl = LearnParameters(training, testing, C, C1, T, conf["memory_buffers"].as<unsigned>(), prev_x, &x);
      sp.push_back(make_pair(C, tppl));
      C *= sweep_factor;
    }
    smoothed.resize(sp.size(), 0);
    smoothed[0] = sp[0].second;
    smoothed.back() = sp.back().second;
    for (int i = 1; i < sp.size()-1; ++i) {
      double prev = sp[i-1].second;
      double next = sp[i+1].second;
      double cur = sp[i].second;
      smoothed[i] = (prev*0.2) + cur * 0.6 + (0.2*next);
    }
    double best_ppl = 9999999;
    unsigned best_i = 0;
    for (unsigned i = 0; i < sp.size(); ++i) {
      if (smoothed[i] < best_ppl) {
        best_ppl = smoothed[i];
        best_i = i;
      }
    }
    C = sp[best_i].first;
  }  // tune regularizer
  tppl = LearnParameters(training, testing, C, C1, T, conf["memory_buffers"].as<unsigned>(), prev_x, &x);
  if (conf.count("weights")) {
    for (int i = 1; i < x.size(); ++i) {
      x[i] = (x[i] * psi) + prev_x[i] * (1.0 - psi);
    }
  }
  cout.precision(15);
  cout << "# C=" << C << "\theld out perplexity=";
  if (tppl) { cout << tppl << endl; } else { cout << "N/A\n"; }
  if (sp.size()) {
    cout << "# Parameter sweep:\n";
    for (int i = 0; i < sp.size(); ++i) {
      cout << "# " << sp[i].first << "\t" << sp[i].second << "\t" << smoothed[i] << endl;
    }
  }
  Weights::WriteToFile("-", x);
  return 0;
}


//==================================================================================================
/*!
  @file

  @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_CONSTANT_MINF_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_MINF_HPP_INCLUDED

/*!
  @ingroup group-constant
  @defgroup constant-Minf Minf (function template)

  Generates a constant less than all other numeric values of a given type.

  @headerref{<boost/simd/constant/minf.hpp>}

  @par Description

  1.  @code
      template<typename T> T Minf();
      @endcode

  2.  @code
      template<typename T> T Minf( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T which value is less than all other numeric values of type @c T.
  Compared to the [C++ standard infinity](http://en.cppreference.com/w/cpp/types/numeric_limits/infinity),
  `Minf<T>()` is defined for both IEEEValue and IntegerValue.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to:
  - `T(-std::numeric_limits<scalar_of_t<T>>::inf()` if @c T models IEEEValue.
  - [the minimum representable value](@ref constant-Valmin) if @c T models IntegerValue.

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/minf.hpp>
#include <boost/simd/constant/simd/minf.hpp>

#endif