#ifndef ANTIAIMBOT_TRANSFORM_HPP
#define ANTIAIMBOT_TRANSFORM_HPP

#include <vector>
#include <cmath>
#include <cassert>

#include <dlib/geometry.h>
#include <sampml/feature_vector.hpp>

#include "samp.hpp"

/* sequence in which the information is provided by the data vector */
using input_vector = sampml::feature_vector<double, 56>;
enum {
	bHit,

	fShooterPosX,
	fShooterPosY,
	fShooterPosZ,
	fShooterVelocityX,
	fShooterVelocityY,
	fShooterVelocityZ,

	fShooterCameraPosX,
	fShooterCameraPosY,
	fShooterCameraPosZ,
	fShooterCameraFVX,
	fShooterCameraFVY,
	fShooterCameraFVZ,
	fShooterCameraZoom,
    fShooterCameraAspectRatio,
    iShooterCameraMode,

	fShooterFacingAngle,

	iShooterState,
	iShooterSpecialAction,
	bShooterInVehicle,
	bShooterSurfingVehicle,
	bShooterSurfingObject,
	iShooterWeaponID,
	iShooterSkinID,
	iShooterID,

	fVictimPosX,
	fVictimPosY,
	fVictimPosZ,
	fVictimVelocityX,
	fVictimVelocityY,
	fVictimVelocityZ,

	fVictimFacingAngle,

	iVictimState,
	iVictimSpecialAction,
	bVictimInVehicle,
	bVictimSurfingVehicle,
	bVictimSurfingObject,
	iVictimWeaponID,
	iVictimSkinID,
	iVictimID,

    iHitType,
	fOffsetX, // OPWS
	fOffsetY, // OPWS
	fOffsetZ, // OPWS
	fOriginX, // GPLSV
	fOriginY, // GPLSV
	fOriginZ, // GPLSV
	fHitPosX, // GPLSV
	fHitPosY, // GPLSV
	fHitPosZ, // GPLSV

	iShooterPing,
	fShooterPacketLoss,
	iVictimPing,
	fVictimPacketLoss,

	iSecond,
	iTick
};

using float_element_type = float;
using vector_3d = dlib::vector<float_element_type, 3>;
struct translated_input {
    bool hit; /* TRUE = hit, FALSE = miss */

    struct {
        bool in_vehicle;
        bool surfing_vehicle;
        bool surfing_object;

        int state;
	    int special_action;
        int weapon;
        int skin;
        int id;

        vector_3d position;
        vector_3d velocity;
        float_element_type facing_angle;

        struct {
            vector_3d position;
            vector_3d FV;
            float_element_type zoom;
            float_element_type aspect_ratio;
            int mode;
        } camera;

        struct {
            int ping;
            float_element_type packet_loss;
        } network;
    } shooter;

    struct {
        bool in_vehicle;
        bool surfing_vehicle;
        bool surfing_object;

        int state;
	    int special_action;
        int weapon;
        int skin;
        int id;

        vector_3d position;
        vector_3d velocity;
        float_element_type facing_angle;

        struct {
            int ping;
            float_element_type packet_loss;
        } network;
    } victim;

    struct {
        int hit_type;
        vector_3d offset;
        vector_3d origin;
        vector_3d hit_pos;
    } shot;

    int second;
    int tick;

    translated_input(const input_vector& input) {
        hit = input(bHit);

        shooter.in_vehicle = input(bShooterInVehicle);
        shooter.surfing_object =  input(bShooterSurfingObject);
        shooter.surfing_vehicle = input(bShooterSurfingVehicle);
        shooter.state = input(iShooterState);
        shooter.special_action = input(iShooterSpecialAction);
        shooter.weapon = input(iShooterWeaponID);
        shooter.skin = input(iShooterSkinID);
        shooter.id = input(iShooterID);

        shooter.position.x() = input(fShooterPosX);
        shooter.position.y() = input(fShooterPosY);
        shooter.position.z() = input(fShooterPosZ);

        shooter.velocity.x() = input(fShooterVelocityX);
        shooter.velocity.y() = input(fShooterVelocityY);
        shooter.velocity.z() = input(fShooterVelocityZ);

        shooter.facing_angle = input(fShooterFacingAngle);

        shooter.camera.position.x() = input(fShooterCameraPosX);
        shooter.camera.position.y() = input(fShooterCameraPosY);
        shooter.camera.position.z() = input(fShooterCameraPosZ);

        shooter.camera.FV.x() = input(fShooterCameraFVX);
        shooter.camera.FV.y() = input(fShooterCameraFVY);
        shooter.camera.FV.z() = input(fShooterCameraFVZ);

        shooter.camera.zoom = input(fShooterCameraZoom);
        shooter.camera.mode = input(iShooterCameraMode);
        shooter.camera.aspect_ratio = input(fShooterCameraAspectRatio);

        shooter.network.ping = input(iShooterPing);
        shooter.network.packet_loss = input(fShooterPacketLoss);

        /* Victim Information */
        victim.in_vehicle = input(bVictimInVehicle);
        victim.surfing_object = input(bVictimSurfingObject);
        victim.surfing_vehicle = input(bVictimSurfingVehicle);
        victim.state = input(iVictimState);
        victim.special_action = input(iVictimSpecialAction);
        victim.weapon = input(iVictimWeaponID);
        victim.skin = input(iVictimSkinID);
        victim.id = input(iVictimID);

        victim.position.x() = input(fVictimPosX);
        victim.position.y() = input(fVictimPosY);
        victim.position.z() = input(fVictimPosZ);

        victim.velocity.x() = input(fVictimVelocityX);
        victim.velocity.y() = input(fVictimVelocityY);
        victim.velocity.z() = input(fVictimVelocityZ);

        victim.facing_angle = input(fVictimFacingAngle);

        victim.network.ping = input(iVictimPing);
        victim.network.packet_loss = input(fVictimPacketLoss);

        /* Shot Information */
        shot.hit_type = input(iHitType);

        shot.offset.x() = input(fOffsetX);
        shot.offset.y() = input(fOffsetY);
        shot.offset.z() = input(fOffsetZ);

        shot.origin.x() = input(fOriginX);
        shot.origin.y() = input(fOriginY);
        shot.origin.z() = input(fOriginZ);

        shot.hit_pos.x() = input(fHitPosX);
        shot.hit_pos.y() = input(fHitPosY);
        shot.hit_pos.z() = input(fHitPosZ);

        /* Time information */
        second = input(iSecond);
        tick = input(iTick);
    }
};

using output_vector = sampml::feature_vector<double, 16>;
#if 0
/* the information which we wish to use for training */
struct features {
    /*
    ** LOS = Line Of Shot
    ** LOC = Line Of player Centers
    */

    /*
    ** Many aimbots automatically aim along the XY plane only. Once the Z of the aim is
    ** set, the player can hit the victim without changing the aim as the XY is set by the aimbot
    ** and Z generally does not change.
    **
    ** For those who do not use aimbot, they need to adjust the XY continously. As they adjust, the
    ** Z also alters slightly.
    **
    ** Z variations decrease as the distance between the players increases and hence becomes an
    ** ineffective gauge of aimbot usage unless compensated
    */

    /*
    ** For players not using aimbot, the HITS tend to come when the velocity of the enemy
    ** is not changing abruptly. This is not true in the case of aimbots.
    **
    ** Hence, high velocity changes and successful hits indicate the possiblity of aimbot being used.
    **
    ** In most cases, the Z component is negligible and does not indicate anything useful for most aimbots
    ** as they don't adjust for Z. There are two cases:
    ** 1. player changes direction:
    **    in this case, it is mostly a transfer of velocity from parallel component to the perpendicular
    **    component and vice versa
    ** 2. player changes magnitude along one specific direction
    **    if the player changes magnitude along the parallel direction, it doesn't affect the difficulty for
    **    the player significantly but changing the magnitude along the perpendicular direction might make
    **    a difference
    **
    ** From the above analysis, we conclude that knowing the statistics of the perpendicular component
    ** gives enough information.
    **
    ** Puzzle: what if the player gets the immediate shot after a change successfuly but misses everything
    ** that comes next
    */

    /*
    ** Aimbots do miss shots occasionally but they are usually not very off from the target but for
    ** non-aimbots, the miss offsets are generally larger.
    */

    /*
    ** Aimbots maintain very good aim at the player and hence the angle deviations are small
    ** even for random movement of players.
    */

    /*
    ** As in most cases, the shooter need not adjust their aim while using an aimbot, the Z angle
    ** does not change significantly (if at all) as the XY is automatically adjusted by the aimbot
    ** and Z tends to not change very much.
    **
    ** Without an aimbot, the player needs to continously adjust the XY which also causes the Z
    ** to change slightly.
    */

    double LOCLOSAngleDeltaMean;
    double LOCLOSAngleDeltaStdDev;

	double ShooterVerticalAngleStdDev,
		   ShooterVerticalAngleRateMean;
    int ConstantVerticalAngleStreak;

    double HitVerticalAngleStdDev;
    double HitLOCLOSAngleDeltaMean,
           HitLOCLOSAngleDeltaStdDev;
    double HitVictimVelocityPerpDeltasMean,
           HitVictimVelocityPerpDeltasStdDev;

    double ImmediateMissLOCLOSAngleDeltaMean,
           ImmediateMIssLOCLOSAngleDeltaStdDev;
    double ImmediateMissVictimVelocityPerpDeltasMean,
           ImmediateMissVictimVelocityPerpDeltasStdDev;

    double AllMissLOCLOSAngleDeltaMean,
           AllMissLOCLOSAngleDeltaStdDev;
};
#endif

class transformer {
    public:
        transformer() { }

        template<class Container>
        transformer(const Container& cont) {
            submit(cont);
        }

        template<class Container>
        void submit(const Container& cont) {
            for(const auto& v : cont)
                submit(v);
        }

        template<class Container = input_vector>
        void submit(const input_vector& raw_input) {
            translated_input input(raw_input);

            if(filter(input)) {
                filter_out++;
                return;
            }

            bool try_process = preprocess(input);

            if(try_process)
                process();
        }

        std::vector<output_vector> pool;

        int filter_out;
        int count_out;
        int ratio_out;
    private:
        bool filter(translated_input& input) {
            if((input.shooter.weapon != 31 && input.shooter.weapon != 30 && input.shooter.weapon != 29)
            || input.shooter.state != samp::PLAYER_STATE_ONFOOT
            || input.victim.state != samp::PLAYER_STATE_ONFOOT
            || input.shooter.special_action != samp::SPECIAL_ACTION_NONE
            || input.victim.special_action != samp::SPECIAL_ACTION_NONE
            || input.shooter.in_vehicle
            || input.shooter.surfing_object
            || input.shooter.surfing_vehicle
            || input.victim.in_vehicle
            || input.victim.surfing_object
            || input.victim.surfing_vehicle
            || std::abs((input.victim.velocity - input.shooter.velocity).length()) < 0.05
            || input.victim.velocity.length() < 0.05
            || input.shooter.camera.mode != samp::CAMERA_WEAPON_AIM
            || (input.shooter.position - input.victim.position).length() < 10.0
            || (input.shooter.position - input.victim.position).length() > 40.0
            || (input.hit == false && shot_status.size() == 0)
            ) {
                process(true);
                reset();
                return true;
            }

            if((input.hit && prev_victimid != samp::INVALID_PLAYER_ID && prev_victimid != input.victim.id)
            || (prev_weaponid != -1 && prev_weaponid != input.shooter.weapon)
            || (prev_tick != -1 && input.tick - prev_tick > 500)
            || (prev_second != -1 && input.second - prev_second >= 2)
            ) {
                process(true);
                reset();
                if (input.hit == false)
                    return true;
            }
            prev_victimid = input.victim.id;
            prev_weaponid = input.shooter.weapon;

            if(input.tick - prev_tick < 200) //TODO
                return true;

            prev_tick = input.tick;
            prev_second = input.second;
            return false;
        }

        bool preprocess(const translated_input& input) {
            const vector_3d xy_plane_normal(0, 0, 1);
            vector_3d line_of_centers = input.victim.position - input.shooter.position;

            vector_3d parallel_axis, perpendicular_axis, z_axis;
			parallel_axis 	    = dlib::normalize(line_of_centers);
            perpendicular_axis 	= dlib::normalize(parallel_axis.cross(xy_plane_normal));
            z_axis 				= dlib::normalize(parallel_axis.cross(perpendicular_axis));

            vector_3d transformed_shooter_velocity;
            transformed_shooter_velocity.x() = input.shooter.velocity.dot(perpendicular_axis);
            transformed_shooter_velocity.y() = input.shooter.velocity.dot(parallel_axis);
            transformed_shooter_velocity.z() = input.shooter.velocity.dot(z_axis);

            vector_3d transformed_victim_velocity;
            transformed_victim_velocity.x() = input.victim.velocity.dot(perpendicular_axis);
            transformed_victim_velocity.y() = input.victim.velocity.dot(parallel_axis);
            transformed_victim_velocity.z() = input.victim.velocity.dot(z_axis);

            const vector_3d camera_axis = input.shooter.camera.FV;
            vector_3d right_vector = dlib::normalize(camera_axis.cross(xy_plane_normal));
            vector_3d up_vector = dlib::normalize(camera_axis.cross(right_vector));

            vector_3d transformed_camera_axis;
            transformed_camera_axis.x() = camera_axis.dot(perpendicular_axis);
            transformed_camera_axis.y() = camera_axis.dot(parallel_axis);
            transformed_camera_axis.z() = camera_axis.dot(z_axis);

            const double scaleY = (input.shooter.camera.aspect_ratio > 1.375) ? 0.4 : 0.525;
            const double pX = ((335.0/640.0) - 0.5) * 2.0 * 0.7,
                         pZ = ((195.0/448.0) - 0.5) * 2.0 * scaleY;

            vector_3d reticle_axis;
            reticle_axis = dlib::normalize(camera_axis + right_vector * pX + up_vector * pZ);

            vector_3d transformed_reticle_axis;
            transformed_reticle_axis.x() = reticle_axis.dot(perpendicular_axis);
            transformed_reticle_axis.y() = reticle_axis.dot(parallel_axis);
            transformed_reticle_axis.z() = reticle_axis.dot(z_axis);

            shot_status.push_back(input.hit);
            shot_timestamp.push_back(input.tick);
            shooter_facing_angles.push_back(input.shooter.facing_angle);
            original_victim_velocities.push_back(input.victim.velocity);
            transformed_shooter_velocities.push_back(transformed_shooter_velocity);
            transformed_victim_velocities.push_back(transformed_victim_velocity);
            transformed_line_of_centers.push_back(vector_3d(0.0, line_of_centers.length(), 0.0));
            original_camera_axes.push_back(camera_axis);
            transformed_camera_axes.push_back(transformed_camera_axis);
            transformed_reticle_axes.push_back(transformed_reticle_axis);

            return (shot_status.size() >= 7);
        }

        void process(bool rejection = false) {
            if(shot_status.size() < 7)
                return;

            if(shot_status.size() >= 20) {
                reset();
                count_out++;
                return;
            }

            if(!rejection) {
                if(shot_status.back() == true)
                    return;
            }

            int shots = shot_status.size();
            for(int i = shot_status.size() - 1; shot_status[i] == false && shot_status[i - 1] == false; i--) {
                assert(i > 0);
                shots = i;
            }

            int hits = std::count(shot_status.begin(), shot_status.begin() + shots, true),
                misses = shots - hits,
                imisses = 0;
            for(int i = 1; i < shots; i++)
                if(shot_status[i] == false && shot_status[i-1] == true)
                    imisses++;

            if(misses < 2)
                return;

            if(hits < 5)
                return;

            float ratio = float(hits)/shots;
            if(ratio < 0.4) {
                ratio_out++;
                reset();
                return;
            } else if(ratio < 0.6) {
                return;
            }

            output_vector output;
            dlib::matrix<float_element_type, 0, 1> LOCLOSDifferences(shots);
            for(int i = 0; i < shots; i++) {
                const auto& line_of_center = transformed_line_of_centers[i];
                const auto& reticle_axis = transformed_reticle_axes[i];
                LOCLOSDifferences(i) = std::acos(reticle_axis.dot(line_of_center)/line_of_center.length());
            }
            output(0) = dlib::mean(LOCLOSDifferences); /* LOCLOSAngleDeltaMean */
            output(1) = dlib::stddev(LOCLOSDifferences); /* LOCLOSAngleDeltaStdDev */

            dlib::matrix<float_element_type, 0, 1> VerticalAngles(shots);
            for(int i = 0; i < shots; i++) {
                const auto& camera_axis = original_camera_axes[i];
                VerticalAngles(i) = std::asin(camera_axis(2));
            }
            output(2) = dlib::stddev(VerticalAngles); /* ShooterVerticalAngleStdDev */

            dlib::matrix<float_element_type, 0, 1> VerticalAngleRates(shots - 1);
            for(int i = 1; i < shots; i++) {
                auto prev = VerticalAngles(i - 1);
                auto cur = VerticalAngles(i);
                VerticalAngleRates(i - 1) = std::abs(cur - prev);
            }
            output(3) = dlib::mean(VerticalAngleRates); /* ShooterVerticalAngleRateMean */

            int max_vangle_streak = 0;
            for(int i = 1, current_streak = 0; i < shots; i++) {
                auto prev = VerticalAngles(i - 1);
                auto cur = VerticalAngles(i);
                if(cur == prev && (shooter_facing_angles[i] != shooter_facing_angles[i - 1]))
                    current_streak++;
                else {
                    max_vangle_streak = std::max(max_vangle_streak, current_streak);
                    current_streak = 0;
                }
            }
            output(4) = max_vangle_streak; /* ConstantVerticalAngleStreak */

            dlib::matrix<float_element_type, 0, 1> HitVerticalAngles(hits);
            dlib::matrix<float_element_type, 0, 1> HitLOCLOSDifferences(hits);
            for(int i = 0, hit_idx = 0; i < shots; i++) {
                if(shot_status[i] == true) {
                    HitVerticalAngles(hit_idx) = VerticalAngles(i);
                    HitLOCLOSDifferences(hit_idx) = LOCLOSDifferences(i);
                    hit_idx++;
                }
            }
            output(5) = dlib::stddev(HitVerticalAngles); /* HitVerticalAngleStdDev */
            output(6) = dlib::mean(HitLOCLOSDifferences); /* HitLOCLOSAngleDeltaMean */
            output(7) = dlib::stddev(HitLOCLOSDifferences); /* HitLOCLOSAngleDeltaStdDev */

            dlib::matrix<float_element_type, 0, 1> HitVictimPerpVelocityDeltas(hits - 1);
            for(int i = 1, hit_idx = 0; i < shots; i++) {
                if(shot_status[i] == true) {
                    const auto& v1 = transformed_victim_velocities[i-1];
                    const auto& v2 = transformed_victim_velocities[i];
                    HitVictimPerpVelocityDeltas(hit_idx) = std::abs(v2.x() - v1.x());
                    hit_idx++;
                }
            }
            output(8) = dlib::mean(HitVictimPerpVelocityDeltas); /* HitVictimVelocityPerpDeltasMean */
            output(9) = dlib::stddev(HitVictimPerpVelocityDeltas); /* HitVictimVelocityPerpDeltasStdDev */

            dlib::matrix<float_element_type, 0, 1> IMissLOCLOSDifferences(imisses);
            for(int i = 1, imiss_idx = 0; i < shots; i++) {
                if(shot_status[i] == false && shot_status[i-1] == true) {
                    IMissLOCLOSDifferences(imiss_idx) = LOCLOSDifferences(i);
                    imiss_idx++;
                }
            }
            output(10) = dlib::mean(IMissLOCLOSDifferences); /* ImmediateMissLOCLOSAngleDeltaMean */
            output(11) = dlib::stddev(IMissLOCLOSDifferences); /* ImmediateMIssLOCLOSAngleDeltaStdDev */

            dlib::matrix<float_element_type, 0, 1> IMissVictimPerpVelocityDeltas(imisses);
            for(int i = 1, imiss_idx = 0; i < shots; i++) {
                if(shot_status[i] == false && shot_status[i-1] == true) {
                    const auto& v1 = transformed_victim_velocities[i-1];
                    const auto& v2 = transformed_victim_velocities[i];
                    IMissVictimPerpVelocityDeltas(imiss_idx) = std::abs(v2.x() - v1.x());
                    imiss_idx++;
                }
            }
            output(12) = dlib::mean(IMissVictimPerpVelocityDeltas); /* ImmediateMissVictimVelocityPerpDeltasMean */
            output(13) = dlib::stddev(IMissVictimPerpVelocityDeltas); /* ImmediateMissVictimVelocityPerpDeltasStdDev */

            dlib::matrix<float_element_type, 0, 1> AllMissLOCLOSDifferences(misses);
            for(int i = 1, miss_idx = 0; i < shots; i++) {
                if(shot_status[i] == false) {
                    AllMissLOCLOSDifferences(miss_idx) = LOCLOSDifferences(i);
                    miss_idx++;
                }
            }
            output(14) = dlib::mean(AllMissLOCLOSDifferences); /* AllMissLOCLOSAngleDeltaMean */
            output(15) = dlib::stddev(AllMissLOCLOSDifferences); /* AllMissLOCLOSAngleDeltaStdDev */

            pool.push_back(output);
            reset();
        }

        /* processing data */
        std::vector<unsigned int> shot_timestamp;
        std::vector<bool> shot_status;
        std::vector<float_element_type> shooter_facing_angles;
        std::vector<vector_3d> original_victim_velocities;
        std::vector<vector_3d> transformed_shooter_velocities;
        std::vector<vector_3d> transformed_victim_velocities;
        std::vector<vector_3d> transformed_line_of_centers;
        std::vector<vector_3d> original_camera_axes;
        std::vector<vector_3d> transformed_camera_axes;
        std::vector<vector_3d> transformed_reticle_axes;

        /* filtering data */
        int prev_victimid, prev_weaponid;
        int64_t prev_tick, prev_second;

        void reset () {
            prev_victimid = samp::INVALID_PLAYER_ID;
            prev_weaponid = -1;
            prev_tick = -1;
            prev_second = -1;

            shot_timestamp.clear();
            shot_status.clear();
            shooter_facing_angles.clear();
            original_victim_velocities.clear();
            transformed_shooter_velocities.clear();
            transformed_victim_velocities.clear();
            transformed_line_of_centers.clear();
            original_camera_axes.clear();
            transformed_camera_axes.clear();
            transformed_reticle_axes.clear();
        }
};

#endif /* ANTIAIMBOT_TRANSFORM_HPP */


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_STATIC_NUM_ROWS_INCLUDE
#define MTL_STATIC_NUM_ROWS_INCLUDE

#include <boost/mpl/bool.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/matrix/dimension.hpp>
#include <boost/numeric/mtl/vector/dimension.hpp>

namespace mtl {

/// Number of rows given at compile time
/** General declaration, used to disable unsupported types **/
template <typename Collection>
struct static_num_rows {
    // typedef xxx type;
    // static const type value= yyy;
};

/// static_num_rows implementation for (1D) arrays interpreted as vectors
template <typename Value, unsigned Size>
struct static_num_rows<Value[Size]>
{
    typedef std::size_t   type;
    static const type value= Size;
};

/// static_num_rows implementation for (2D and higher) arrays interpreted as matrices
template <typename Value, unsigned Rows, unsigned Cols>
struct static_num_rows<Value[Rows][Cols]>
{
    typedef std::size_t   type;
    static const type value= Rows;
};

template <std::size_t Size>
struct static_num_rows< vec::fixed::dimension<Size> >
{
    typedef std::size_t   type;
    static const type value= Size;
};

template <typename V, typename P>
struct static_num_rows<mtl::vec::dense_vector<V, P> >
{
    typedef std::size_t   type;
    static const type value= traits::is_row_major<P>::value ? 1 : static_num_rows<typename P::dimension>::value;
};

template <std::size_t Rows, std::size_t Cols>
struct static_num_rows< fixed::dimensions<Rows, Cols> >
{
    typedef std::size_t   type;
    static const type value= Rows;
};

template <typename V, typename P>
struct static_num_rows<mtl::mat::dense2D<V, P> >
  : static_num_rows<typename P::dimensions>
{};

template <typename V, std::size_t M, typename P>
struct static_num_rows<mtl::mat::morton_dense<V, M, P> >
  : static_num_rows<typename P::dimensions>
{};

template <typename V, typename P>
struct static_num_rows<mtl::mat::compressed2D<V, P> >
  : static_num_rows<typename P::dimensions>
{};

} // namespace mtl

#endif // MTL_STATIC_NUM_ROWS_INCLUDE


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/monad/bind_mcd.hpp>
#include <boost/hana/monad/flatten_mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/comparable.hpp>
#include <boost/hana/detail/minimal/monad.hpp>

#include <tuple>
using namespace boost::hana;

template <template <typename ...> class mcd>
void test() {
    constexpr auto monad = detail::minimal::monad<mcd>;
    constexpr auto comparable = detail::minimal::comparable<>;
    BOOST_HANA_CONSTEXPR_LAMBDA auto f = [=](auto x) {
        return monad(std::make_tuple(x));
    };

    BOOST_HANA_CONSTEXPR_ASSERT(bind(monad(comparable(1)), f) == f(comparable(1)));
    BOOST_HANA_CONSTEXPR_ASSERT((monad(comparable(1)) | f) == f(comparable(1)));

    // check associativity
    BOOST_HANA_CONSTEXPR_ASSERT((monad(comparable(1)) | f | f) == f(std::make_tuple(comparable(1))));
}

int main() {
    test<Monad::flatten_mcd>();
    test<Monad::bind_mcd>();
}


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

#include <localization_system_tests/localization_evaluation_node.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_eigen/tf2_eigen.h>
#include <Eigen/Geometry>
#include <memory>
#include <string>

namespace localization_system_tests
{

LocalizationEvaluationNode::LocalizationEvaluationNode(
  const rclcpp::NodeOptions & options)
: Node("localization_evaluator", options), m_pose_subscribers{
    std::make_unique<message_filters::Subscriber<Pose>>(this, "localization/ndt_pose"),
    std::make_unique<message_filters::Subscriber<Pose>>(this, "/vehicle/odom_pose")
}
{
  EigTransform estimate_offset;
  estimate_offset.setIdentity();
  estimate_offset.translate(
    Eigen::Vector3d{
      declare_parameter("estimate_offset.translation.x").get<double>(),
      declare_parameter("estimate_offset.translation.y").get<double>(),
      declare_parameter("estimate_offset.translation.z").get<double>()
    });
  estimate_offset.rotate(
    Eigen::Quaterniond {
      declare_parameter("estimate_offset.rotation.w").get<double>(),
      declare_parameter("estimate_offset.rotation.x").get<double>(),
      declare_parameter("estimate_offset.rotation.y").get<double>(),
      declare_parameter("estimate_offset.rotation.z").get<double>()
    });

  EigTransform gt_offset;
  gt_offset.setIdentity();
  gt_offset.translate(
    Eigen::Vector3d{
      declare_parameter("ground_truth_offset.translation.x").get<double>(),
      declare_parameter("ground_truth_offset.translation.y").get<double>(),
      declare_parameter("ground_truth_offset.translation.z").get<double>()
    });

  estimate_offset.rotate(
    Eigen::Quaterniond {
      declare_parameter("ground_truth_offset.rotation.w").get<double>(),
      declare_parameter("ground_truth_offset.rotation.x").get<double>(),
      declare_parameter("ground_truth_offset.rotation.y").get<double>(),
      declare_parameter("ground_truth_offset.rotation.z").get<double>()
    });

  m_ground_truth_to_estimate_tf = estimate_offset * gt_offset.inverse();

  m_pose_synchronizer = std::make_unique<message_filters::Synchronizer<SyncPolicyT>>(
    SyncPolicyT(50), *m_pose_subscribers[0], *m_pose_subscribers[1]);

  m_pose_synchronizer->registerCallback(
    std::bind(
      &LocalizationEvaluationNode::evaluation_callback, this,
      std::placeholders::_1, std::placeholders::_2));
}

void LocalizationEvaluationNode::evaluation_callback(
  const Pose::ConstSharedPtr & estimate, const Pose::ConstSharedPtr & ground_truth)
{
  EigTransform estimate_eig;
  EigTransform ground_truth_eig;
  EigTransform ground_truth_in_map_frame_eig;

  tf2::fromMsg(estimate->pose.pose, estimate_eig);
  tf2::fromMsg(ground_truth->pose.pose, ground_truth_eig);

  ground_truth_in_map_frame_eig = m_ground_truth_to_estimate_tf * ground_truth_eig;

  compute_and_update_metrics(ground_truth_in_map_frame_eig, estimate_eig);
}

LocalizationEvaluationNode::~LocalizationEvaluationNode() noexcept
{
  std::stringstream report;

  report <<
    "Processed " << m_num_computed << " samples." << std::endl <<
    "Average translation error: " << m_average_translation_err << " meters." << std::endl <<
    "Average rotation error: " << m_average_rotation_err << " radians." << std::endl;
  RCLCPP_INFO(get_logger(), report.str());
}

LocalizationEvaluationNode::float64_t
LocalizationEvaluationNode::get_new_average(float64_t current_avg, float64_t addition)
{
  const auto current_num_samples = static_cast<float64_t>(m_num_computed);
  const auto total_err = (current_avg * current_num_samples) + addition;
  return total_err / (current_num_samples + 1.0);
}

LocalizationEvaluationNode::float64_t LocalizationEvaluationNode::translation_error(
  const LocalizationEvaluationNode::EigTranslationPart & tf1,
  const LocalizationEvaluationNode::EigTranslationPart & tf2)
{
  return (tf1 - tf2).norm();
}

LocalizationEvaluationNode::float64_t LocalizationEvaluationNode::rotation_error(
  const LocalizationEvaluationNode::EigRotationPart & tf1,
  const LocalizationEvaluationNode::EigRotationPart & tf2)
{
  const Eigen::Quaterniond rot1{tf1};
  const Eigen::Quaterniond rot2{tf2};
  return std::fabs(rot1.angularDistance(rot2));
}

void LocalizationEvaluationNode::compute_and_update_metrics(
  const EigTransform & ground_truth, const EigTransform & estimate)
{
  const auto translation_err =
    translation_error(ground_truth.translation(), estimate.translation());
  const auto rotation_err = rotation_error(ground_truth.rotation(), estimate.rotation());

  m_average_translation_err = get_new_average(m_average_translation_err, translation_err);
  m_average_rotation_err = get_new_average(m_average_rotation_err, rotation_err);

  ++m_num_computed;
}
}  // namespace localization_system_tests

#include "rclcpp_components/register_node_macro.hpp"  // NOLINT
RCLCPP_COMPONENTS_REGISTER_NODE(localization_system_tests::LocalizationEvaluationNode)


#include <signal.h>
#include <fstream>
#include <sstream>
#include <string>
#include <iostream>
#include <valarray>
#include <limits>
#include <cstdio>

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/make_shared.hpp>
#include <boost/graph/copy.hpp>

#include "betweenness_centrality.hpp"
#include "BaseTypes.h"
#include "Tree.h"
#include "StrategicSolver.h"
#include "GraphUtil.h"

using namespace boost;

StrategicSolver::StrategicSolver(int betweenness_cutoff, int brandes_pivots)
    : betweenness_cutoff(betweenness_cutoff),
      brandes_pivots(brandes_pivots)
{
}

std::vector<int> solve_naively(UndirectedGraph& graph){
    std::vector<int> output;

    int nNodes = num_vertices(graph); // nNodes is same as depth
    output.push_back(nNodes);

    // get an index map
    ud_IndexMap index = get(vertex_index, graph);

    // look up each vertex by index
    std::pair<ud_vertex_iterator, ud_vertex_iterator> vi = vertices(graph);
    for(ud_vertex_iterator vertex_iter = vi.first; vertex_iter != vi.second; ++vertex_iter){
        output.push_back(index[*vertex_iter]);
    }
    return output;
}

void StrategicSolver::Decompose(UndirectedGraph graph, Tree& bucket)
{
    int nNodes = num_vertices(graph);
    VIndex root_id = choose_node(graph);
    Vertex root = graph[root_id];
    clear_vertex(root_id, graph);
    remove_vertex(root_id, graph);
    bucket.ParentLookup[root.num] = -1;

    std::vector<ComponentGraph> cc = ConnectedComponents(graph);
    int depth = std::numeric_limits<int>::min();
    if (cc.size() > 1){
        for(ComponentGraph& c: cc){
            UndirectedGraph* new_subgraph = new UndirectedGraph();
            copy_graph(c, *new_subgraph);
            depth = std::max(decomp(new_subgraph, bucket, root), depth);
        }
    }
    else
    {
        UndirectedGraph* new_subgraph = new UndirectedGraph();
        copy_graph(graph, *new_subgraph);
        depth = decomp(new_subgraph, bucket, root);
    }
    bucket.Depth = depth + 1;
}

int StrategicSolver::decomp(UndirectedGraph* graph, Tree& bucket, Vertex parent)
{
    std::vector<ComponentGraph> cc;
    int depth = 0;
    do
    {
        VIndex child_id = choose_node(*graph);
        Vertex child = (*graph)[child_id];
        bucket.ParentLookup[child.num] = parent.num;

        clear_vertex(child_id, *graph);
        remove_vertex(child_id, *graph);
        depth++;

        if(num_vertices(*graph) == 0)
            return depth;

        cc = ConnectedComponents(*graph);
        parent = child;
    } while(cc.size() == 1);

    std::vector<UndirectedGraph*> subgraphs;
    for(ComponentGraph& c: cc)
    {
        UndirectedGraph* new_subgraph = new UndirectedGraph();
        copy_graph(c, *new_subgraph);
        subgraphs.push_back(new_subgraph);
    }
    delete graph;

    int largestDepth = std::numeric_limits<int>::min();
    for(UndirectedGraph* subgraph : subgraphs)
    {
        int branchDepth = decomp(subgraph, bucket, parent);
        largestDepth = std::max(branchDepth, largestDepth);
    }

    return largestDepth + depth;
}

VIndex StrategicSolver::choose_node(UndirectedGraph& graph)
{
    int child_id;
    if(true)
    {
        child_id = ChooseNodeBrandes(graph, brandes_pivots);
    }
    else
    {
        // For large graphs use degree centrality
        int maxDegree = std::numeric_limits<int>::min();
        for(auto v : make_iterator_range(vertices(graph)))
        {
            int deg = degree(v, graph);
            if(deg > maxDegree)
            {
                maxDegree = deg;
                child_id = v;
            }
        }
    }

    auto VIter = vertices(graph);
    return *(VIter.first + child_id);
}


/************************************************************************
 *  Copyright (C) 2012 Eindhoven University of Technology (TU/e).       *
 *  All rights reserved.                                                *
 ************************************************************************
 *  Redistribution and use in source and binary forms, with or without  *
 *  modification, are permitted provided that the following conditions  *
 *  are met:                                                            *
 *                                                                      *
 *      1.  Redistributions of source code must retain the above        *
 *          copyright notice, this list of conditions and the following *
 *          disclaimer.                                                 *
 *                                                                      *
 *      2.  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.                             *
 *                                                                      *
 *  THIS SOFTWARE IS PROVIDED BY TU/e "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 TU/e 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.                                                             *
 *                                                                      *
 *  The views and conclusions contained in the software and             *
 *  documentation are those of the authors and should not be            *
 *  interpreted as representing official policies, either expressed or  *
 *  implied, of TU/e.                                                   *
 ************************************************************************/

#include "ros/ros.h"
#include "problib/conversions.h"
#include "problib/datatypes.h"

#include <iostream>
#include <armadillo>

#include <time.h>

using namespace std;
using namespace arma;

list<timespec> TIMERS;

stringstream TIMER_LOG;
stringstream OUTPUT_LOG;

inline void startTimer(int ID = 0) {
	timespec t_start;
	clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &t_start);
	TIMERS.push_back(t_start);
}

inline void stopTimer(string msg, int ID = 0, double factor = 1) {
	timespec t_end;
	clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &t_end);

	timespec& t_start = TIMERS.back();
	TIMER_LOG << msg << ": " << factor * ((t_end.tv_sec - t_start.tv_sec) + double(t_end.tv_nsec - t_start.tv_nsec) / 1e9) << " secs." << endl;
	TIMERS.pop_back();
}

void showTimerLog() {
	cout << endl;
	cout << "------------- TIME -----------" << endl;
	cout << TIMER_LOG.str();
	cout << "------------------------------" << endl;
}

void testOutput(string msg, double out, double actual, double epsilon = 1e-9) {
	double diff = fabs(out - actual);
	if (diff < epsilon) {
		OUTPUT_LOG << "OK   - " << msg << ": " << out << endl;
	} else {
		OUTPUT_LOG << "DIFF - " << msg << ": " << out << " - " << actual << " = " << diff << endl;
	}
}

void showOutputLog() {
	cout << endl;
	cout << "----------- OUTPUT -----------" << endl;
	cout << OUTPUT_LOG.str();
	cout << "------------------------------" << endl;
}

void test() {

	using namespace pbl;

    Uniform U1(pbl::Vector3(1, 2, 0.8), pbl::Vector3(2, 1, 0.1));
    Uniform U2(pbl::Vector3(1.2, 2.3, 0.85), pbl::Vector3(2, 1, 0.1));

    cout << U1.toString() << endl;
    cout << U2.toString() << endl;
    cout << U1.getLikelihood(U2) << endl;
    cout << U2.getLikelihood(U1) << endl;

    Hybrid H;

    PMF pmf1;
    pmf1.setProbability("test", 0.8);
    H.addPDF(pmf1, 1);

    problib::PDF H_msg;
    pbl::PDFtoMsg(H, H_msg);

    cout << H_msg << endl;

    PDF* H_received = pbl::msgToPDF(H_msg);

    cout << H_received->toString() << endl;

	Vector3 mean1(0, 0, 0);
	Matrix3 var1(1, 1, 1);
	Gaussian pdf1(mean1, var1);

	Vector3 mean2(1, 1, 1);
	Matrix3 var2(0.1, 0.1, 0.1);
	Gaussian pdf2(mean2, var2);

	Vector3 mean3(0.3, 2, -0.7);
	Matrix3 var3(0.2, 0.1, 0.4);
	Gaussian pdf3(mean3, var3);

	Mixture mix;
	mix.addComponent(pdf1, 0.7);
	mix.addComponent(pdf2, 0.3);

	Mixture mix2;
	mix2.addComponent(mix, 0.1);
	mix2.addComponent(pdf3, 0.9);

	Gaussian exact2(3);
	cout << "Before initialization: " << exact2.toString() << endl;
	exact2.setMean(mean2);

	cout << "Mixture: " << endl << mix2.toString() << endl;

	double d;
	for (int i = 0; i < 1000; ++i) {
		d = mix2.getLikelihood(exact2);
	}
	cout << "Density of mixture at " << mean2 << " = " << d << endl << endl;

	cout << "Converting to msg ..." << endl << endl;
	problib::PDF pdf_msg;
	pbl::PDFtoMsg(mix2, pdf_msg);
	cout << "Result:" << endl << pdf_msg << endl;

	cout << "Converting back to pdf ..." << endl << endl;
	PDF* received_pdf = pbl::msgToPDF(pdf_msg);
	cout << "Result:" << endl << received_pdf->toString() << endl << endl;

	cout << "Density of mixture at " << mean2 << " = " << received_pdf->getLikelihood(exact2) << endl << endl;

	delete received_pdf;

	cout << "Creating pmf ..." << endl;
	PMF pmf;
	pmf.setDomainSize(3);

	PMF pmf2;
	pmf2.setDomainSize(3);
	pmf2.setProbability("A", 0.25);
	pmf2.setProbability("B", 0.5);
	//pmf2.setProbability("C", 0.25);

	cout << pmf.toString() << endl << endl;

	cout << "Updating pmf ..." << endl;
	pmf.update(pmf2);
	cout << pmf.toString() << endl << endl;

	PMF pmf3;
	pmf3.setDomainSize(3);
	pmf3.setProbability("C", 0.999);

	cout << "Updating pmf ..." << endl;
	pmf.update(pmf3);
	cout << pmf.toString() << endl;

	PMF pmf_copy;
	pmf_copy = pmf;

	for(int i = 0; i < 1000; ++i) {
		pmf_copy = pmf;
	}

	pmf_copy.update(pmf3);
	cout << pmf_copy.toString() << endl;
	cout << pmf.toString() << endl;

	cout << "Converting to msg ..." << endl << endl;
	problib::PDF pmf_msg;
	pbl::PDFtoMsg(pmf, pmf_msg);
	cout << "Result:" << endl << pmf_msg << endl;

	cout << "Converting back to pdf ..." << endl << endl;
	PDF* received_pdf2 = pbl::msgToPDF(pmf_msg);
	cout << "Result:" << endl << received_pdf2->toString() << endl << endl;

	delete received_pdf2;

	cout << "Testing simple population of msg for exact (string) value ..." << endl;
	problib::PDF exact_str;
	exact_str.exact_value_str = "test";
	PDF* pdf_exact_str = pbl::msgToPDF(exact_str);
	cout << "exact_str:" << endl << pdf_exact_str->toString("    ") << endl << endl;
	delete pdf_exact_str;

	cout << "Testing simple population of msg for exact (real) value ..." << endl;
	problib::PDF exact_real;
	exact_real.exact_value_vec.push_back(1);
	exact_real.exact_value_vec.push_back(1);
	exact_real.exact_value_vec.push_back(1);
	PDF* pdf_exact_real = pbl::msgToPDF(exact_real);
	cout << "exact_real:" << endl << pdf_exact_real->toString("    ") << endl << endl;

	startTimer();

	double d2;
	for(int i = 0; i < 1000000; ++i) {
		d2 = mix2.getLikelihood(*pdf_exact_real);
	}

	stopTimer("Likelihood on mixture", (double)1 / 1000000);

	cout << "Likelihood with mixture = " << mix2.getLikelihood(*pdf_exact_real) << endl << endl;

	testOutput("Likelihood with mixture", d2, 0.0612611897752479);

	printf("%.16f\n", d2);

	//cout << "Likelihood with itself = " << pdf_exact_real->getLikelihood(*pdf_exact_real) << endl << endl;

	Mixture mix_copy = mix2;
	mix2.clear();

	cout << mix_copy.toString() << endl;

	double d3 = mix_copy.getLikelihood(*pdf_exact_real);
	testOutput("Likelihood with mixture (copy)", d3, d2);

	delete pdf_exact_real;
}

int main(int argc, char **argv) {

	startTimer();
	test();
	stopTimer("TOTAL");

	showTimerLog();
	showOutputLog();
}


//  Copyright (c) 2006-2009, Bernhard Reiter
//
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#include <boost/tree/binary_tree.hpp>
//[ for_each_include_algorithm
#include <boost/tree/algorithm.hpp>
//]

#include <iostream>

#include "../test/binary_tree_test_data.hpp"

using namespace boost::tree;

//[ for_each_example
void to_cout(int i) {
    std::cout << ' ' << i;
    return;
}

int main() {
    binary_tree<int> bt;

    // Fill it with data...
    test_binary_tree_fixture<int>::create_test_dataset1_tree(bt);

    std::cout << "Preorder:";
    for_each(preorder(), bt.root(), to_cout);

    std::cout << std::endl << "Inorder:";
    for_each(inorder(), bt.root(), to_cout);

    std::cout << std::endl << "Postorder:";
    for_each(postorder(), bt.root(), to_cout);

    return 0;
}
//]


//
//  data_util.cpp
//  lpm
//
//  Created by Seong-Hwan Jun on 2019-02-01.
//

#include "data_util.hpp"

#include <algorithm>
#include <fstream>
#include <iostream>
#include <unordered_map>
#include <vector>

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>

#include <gsl/gsl_randist.h>

gsl_matrix *get_first_n_lines(gsl_matrix *obs_matrix, unsigned int n)
{
    gsl_matrix *ret = gsl_matrix_alloc(n, obs_matrix->size2);
    for (unsigned int i = 0; i < n; i++) {
        for (unsigned int j = 0; j < obs_matrix->size2; j++) {
            gsl_matrix_set(ret, i, j, gsl_matrix_get(obs_matrix, i, j));
        }
    }
    return ret;
}

bool path_exists(string path)
{
    return boost::filesystem::exists(path);
}

vector<vector<unsigned int> > read_states(string file_name, string sep)
{
    unsigned int n_patients = 0;
    unsigned int n_genes = 0;

    string line;
    ifstream dat_file (file_name);
    if (!dat_file.is_open())
    {
        cerr << "Could not open the file: " << file_name << endl;
        exit(-1);
    }

    vector<string> results;
    vector<vector<unsigned int> > dat;

    while ( getline (dat_file, line) )
    {
        boost::split(results, line, boost::is_any_of(sep));
        if (n_genes == 0) {
            n_genes = results.size();
        }
        else if (results.size() != n_genes) {
            cerr << "Error in the input file: " << file_name << ". Uneven length of genes." << endl;
            exit(-1);
        }
        vector<unsigned int> row;
        for (unsigned int i = 0; i < n_genes; i++) {
            row.push_back(stoi(results[i]));
        }
        n_patients++;
        dat.push_back(row);
    }
    dat_file.close();

    return dat;
}

gsl_matrix *read_data(string file_name, string sep, bool header)
{
    unsigned int n_patients = 0;
    unsigned int n_genes = 0;

    string line;
    ifstream dat_file (file_name);
    if (!dat_file.is_open())
    {
        cerr << "Could not open the file: " << file_name << endl;
        exit(-1);
    }

    vector<string> results;
    vector<double> dat;

    if (header) {
        // skip the first line
        getline(dat_file, line);
    }

    while ( getline (dat_file, line) )
    {
        boost::split(results, line, boost::is_any_of(sep));
        if (n_genes == 0) {
            n_genes = results.size();
        }
        else if (results.size() != n_genes) {
            cerr << "Error in the input file: " << file_name << endl;
            exit(-1);
        }
        for (unsigned int i = 0; i < n_genes; i++) {
            dat.push_back(stod(results[i]));
        }
        n_patients++;
    }
    dat_file.close();

    gsl_matrix *matrix = gsl_matrix_alloc(n_patients, n_genes);
    unsigned int idx = 0;
    for (unsigned int m = 0; m < n_patients; m++) {
        for (unsigned int n = 0; n < n_genes; n++) {
            gsl_matrix_set(matrix, m, n, dat[idx]);
            idx++;
        }
    }

    return matrix;
}

gsl_matrix *read_csv(string file_name, bool header)
{
    string line;
    ifstream dat_file (file_name);
    if (!dat_file.is_open())
    {
        cerr << "Could not open the file: " << file_name << endl;
        exit(-1);
    }

    vector<string> results;
    vector<double> dat;

    if (header) {
        // skip the first line
        getline(dat_file, line);
    }

    unsigned int n_cols = 0;
    unsigned int n_rows = 0;
    while ( getline (dat_file, line) )
    {
        boost::split(results, line, boost::is_any_of(","));
        if (n_cols == 0) {
            n_cols = results.size();
            if (n_cols == 0) {
                cerr << "Error: " << file_name << " contains 0 columns." << endl;
                exit(-1);
            }
        }
        else if (results.size() != n_cols) {
            cerr << "Error: Column numbers do not match in the input file: " << file_name << endl;
            exit(-1);
        }
        for (unsigned int i = 0; i < n_cols; i++) {
            dat.push_back(stod(results[i]));
        }
        n_rows++;
    }
    dat_file.close();

    gsl_matrix *matrix = gsl_matrix_alloc(n_rows, n_cols);
    unsigned int idx = 0;
    for (unsigned int m = 0; m < n_rows; m++) {
        for (unsigned int n = 0; n < n_cols; n++) {
            gsl_matrix_set(matrix, m, n, dat[idx]);
            idx++;
        }
    }

    return matrix;
}

vector<unsigned int> *read_stages(string file_name)
{
    string line;
    ifstream dat_file (file_name);
    if (!dat_file.is_open())
    {
        cerr << "Could not open the file: " << file_name << endl;
        return 0;
    }

    auto *stages = new vector<unsigned int>();
    while ( getline (dat_file, line) )
    {
        unsigned int stage = stoi(line);
        stages->push_back(stage);
    }
    dat_file.close();

    return stages;
}

unsigned int read_ground_truth_pathway_from_matrix(string file_name, unsigned int *ret)
{
    ifstream dat_file (file_name);
    string line;
    if (!dat_file.is_open())
    {
        cerr << "Could not open the file: " << file_name << endl;
        exit(-1);
    }

    unsigned int gene_idx;
    unsigned int pathway_idx;

    gsl_matrix *pathway_matrix = read_csv(file_name, false);
    for (gene_idx = 0; gene_idx < pathway_matrix->size1; gene_idx++) {
        for (pathway_idx = 0; pathway_idx < pathway_matrix->size1; pathway_idx++) {
            if (gsl_matrix_get(pathway_matrix, gene_idx, pathway_idx) == 1) {
                ret[gene_idx] = pathway_idx;
                break;
            }
        }
    }
    return pathway_matrix->size2;
}

void read_error_params(string path_name, double &fbp, double &bgp)
{
    string line;
    ifstream dat_file (path_name);
    if (!dat_file.is_open())
    {
        cerr << "Could not open the file: " << path_name << endl;
    }

    getline (dat_file, line); // first line is FBP
    fbp = stod(line);

    getline (dat_file, line); // second line is BGP
    bgp = stod(line);

    dat_file.close();
}

void read_error_params(string path_name, vector<double> &vec)
{
    string line;
    ifstream dat_file (path_name);
    if (!dat_file.is_open())
    {
        cerr << "Could not open the file: " << path_name << endl;
    }

    double error_param;
    while ( getline (dat_file, line) )
    {
        error_param = stod(line);
        vec.push_back(error_param);
    }
}

double *convert_to_array(vector<unsigned int> &pathway_membership, unsigned int n_pathways)
{
    unsigned int n_genes = pathway_membership.size();
    double *x = new double[n_genes * n_pathways];
    for (unsigned int n = 0; n < n_genes; n++) {
        unsigned int idx = pathway_membership[n];
        for (unsigned int k = 0; k < n_pathways; k++) {
            x[n * n_pathways + k] = idx == k ? 1 : 0;
        }
    }
    return x;
}

void write_model_selection_output_to_file(string path, unsigned int n_patients,
                                          const vector<double> &log_evidences,
                                          const vector<double> &fbps, const vector<double> &bgps,
                                          const vector<vector<double>> &log_marginals)
{
    unsigned int num_samples = fbps.size();
    unsigned int L = log_evidences.size();

    ofstream myfile;
    string evidence_file = path + "/model_evidences_" + to_string(n_patients) + ".csv";
    myfile.open(evidence_file, ios::out);
    if (myfile.is_open()) {
        // write log_evidences to a file
        for (unsigned int l = 0; l < L; l++) {
            myfile << (l+2) << ", " << log_evidences[l] << std::endl;
        }
        myfile.close();
    } else {
        cerr << "Error: cannot open " << evidence_file << endl;
        exit(-1);
    }

    string log_marginals_file = path + "/log_marginals_" + to_string(n_patients) + ".csv";
    myfile.open(log_marginals_file, ios::out);
    if (myfile.is_open()) {
        // write log_evidences to a file
        for (unsigned int i = 0; i < num_samples; i++) {
            for (unsigned int l = 0; l < L; l++) {
                myfile << (l+2) << ", " << fbps[i] << ", " << bgps[i] << ", " << log_marginals[l][i] << endl;
            }
        }
        myfile.close();
    } else {
        cerr << "Error: cannot open " << log_marginals_file << endl;
        exit(-1);
    }

}

//void write_pg_output(string path,
//                     ParticleGibbs<LinearProgressionState, LinearProgressionParameters> &pg,
//                     LPMParamProposal &pg_proposal)
//{
//    vector<shared_ptr<LinearProgressionParameters> > &params = pg.get_parameters();
//    vector<double > log_marginals = pg.get_log_marginal_likelihoods();
//    vector<shared_ptr<ParticleGenealogy<LinearProgressionState> > > states = pg.get_states();
//
//    if (states.size() == 0) {
//        cout << "Warning: no posterior samples to write!" << endl;
//        return;
//    }
//    if (pg_proposal.get_bgps().size() != pg_proposal.get_fbps().size()) {
//        cerr << "Error: number of BGPs sampled does not match the number of FBPs sampled." << endl;
//        exit(-1);
//    }
//
//    string state_file_path = path + "/states.csv";
//    string params_file_path = path + "/params.csv";
//    string log_marginal_file_path = path + "/log_marginal.csv";
//
//    ofstream state_file;
//    ofstream params_file;
//    ofstream log_marginal_file;
//    state_file.open(state_file_path, ios::out);
//    params_file.open(params_file_path, ios::out);
//    log_marginal_file.open(log_marginal_file_path, ios::out);
//    if (!state_file.is_open()) {
//        cerr << "Error: cannot open " << state_file_path << endl;
//        exit(-1);
//    }
//    if (!params_file.is_open()) {
//        cerr << "Error: cannot open " << params_file_path << endl;
//        exit(-1);
//    }
//    if (!log_marginal_file.is_open()) {
//        cerr << "Error: cannot open " << log_marginal_file_path << endl;
//        exit(-1);
//    }
//
//    // write states
//    unsigned int len = states[0]->size();
//    for (unsigned int i = 0; i < states.size(); i++) {
//        state_file << states[i]->get_state_at(len-1).to_string() << endl;
//    }
//    state_file.close();
//
//    // write all parameters
//    const vector<double> &bgps = pg_proposal.get_bgps();
//    const vector<double> &fbps = pg_proposal.get_fbps();
//    params_file << "FBP, BGP" << endl;
//    for (unsigned int i = 0; i < pg_proposal.get_bgps().size(); i++) {
//        params_file << bgps[i] << ", " << fbps[i] << endl;
//    }
//    params_file.close();
//
//    // write log marginals
//    log_marginal_file << "FBP, BGP, LogMarginal" << endl;
//    for (unsigned int i = 0; i < log_marginals.size(); i++) {
//        log_marginal_file << params[i].get()->get_fbp() << ", " << params[i].get()->get_bgp() << ", " << to_string(log_marginals[i]) << endl;
//    }
//    log_marginal_file.close();
//    cout << "Finished writing data!" << endl;
//
//}

void write_vector(string path, const vector<unsigned int> &data)
{
    ofstream f;
    f.open(path, ios::out);
    for (unsigned int i = 0; i < data.size(); i++) {
        f << data[i] << endl;
    }
    f.close();
}

void write_vector(string path, const vector<double> &data)
{
    ofstream f;
    f.open(path, ios::out);
    for (unsigned int i = 0; i < data.size(); i++) {
        f << data[i] << endl;
    }
    f.close();
}

void write_matrix_as_csv(string path, const gsl_matrix &data)
{
    ofstream f;
    f.open(path, ios::out);
    if (f.is_open()) {
        for (unsigned int i = 0; i < data.size1; i++) {
            for (unsigned int j = 0; j < data.size2; j++) {
                f << gsl_matrix_get(&data, i, j);
                if (j < data.size2 - 1) {
                    f << ", ";
                }
            }
            f << "\n";
        }
        f.close();
    } else {
        cerr << "Error: cannot open file: " << path << endl;
        exit(-1);
    }
}

void compute_row_sum(const gsl_matrix &obs, vector<unsigned int> &row_sum)
{
    // compute the row sum for obs matrix
    unsigned int M = obs.size1;
    unsigned int N = obs.size2;
    if (row_sum.size() != M) {
        cerr << "Error: dimension for row_sum: " << row_sum.size() << " does not num rows of obs: " << M << endl;
        exit(-1);
    }
    for (unsigned int m = 0; m < M; m++) {
        unsigned int sum_m = 0;
        for (unsigned int n = 0; n < N; n++) {
            sum_m += (unsigned int)gsl_matrix_get(&obs, m, n);
        }
        row_sum[m] = sum_m;
    }
}

void propose_pathway(gsl_rng *random, unsigned int num_pathways, vector<unsigned int> &ret_pathway)
{
    unsigned int n_genes = ret_pathway.size();
    unsigned int num_divider_locations = n_genes - 1;
    unsigned int num_dividers = num_pathways;

    unsigned int *possible_divider_positions = new unsigned int[num_divider_locations];
    unsigned int *dividers = new unsigned int[num_pathways];
    unsigned int *genes = new unsigned int[n_genes];

    // populate divider locations
    for (unsigned int n = 0; n < num_divider_locations; n++) {
        possible_divider_positions[n] = n + 1;
    }

    // populate genes
    for (unsigned int n = 0; n < n_genes; n++) {
        genes[n] = n;
    }

    // select divider locations
    gsl_ran_choose(random, dividers, num_dividers - 1, possible_divider_positions, num_divider_locations, sizeof(unsigned int));
    // last divider location is the last index
    dividers[num_dividers - 1] = num_divider_locations + 1;
    sort(dividers, dividers + num_dividers);

    gsl_ran_shuffle(random, genes, n_genes, sizeof(unsigned int));
    unsigned int n = 0;
    for (unsigned int k = 0; k < num_pathways; k++) {
        for (; n < n_genes; n++) {
            unsigned int g = genes[n];
            if (n < dividers[k]) {
                ret_pathway[g] = k;
            } else {
                break;
            }
        }
    }

    delete [] genes;
    delete [] possible_divider_positions;
    delete [] dividers;
}

void sample_stages_uniform(gsl_rng *random, unsigned int n_pathways, vector<unsigned int> &ret_stages)
{
    unsigned int n_patients = ret_stages.size();
    unsigned int i;
    unsigned int stage;
    for (i = 0; i < n_patients; i++) {
        stage = gsl_rng_uniform_int(random, n_pathways);
        ret_stages[i] = stage;
    }
}

gsl_matrix *simulate_data(gsl_rng *random,
                          unsigned int n_pathways,
                          const vector<unsigned int> &pathway,
                          const vector<unsigned int> &patient_stages)
{
    unsigned int n_patients = patient_stages.size();
    unsigned int n_genes = pathway.size();
    unsigned int k, g, n;

    gsl_matrix *data_matrix = gsl_matrix_alloc(n_patients, n_genes);

    // convert pathway to map : k -> vector
    unordered_map<unsigned int, vector<unsigned int>> pathway2genes;
    for (g = 0; g < n_genes; g++) {
        k = pathway[g];
        pathway2genes[k].push_back(g);
    }

    for (n = 0; n < n_patients; n++) {
        for (g = 0; g < n_genes; g++) {
            gsl_matrix_set(data_matrix, n, g, 0);
        }
    }

    for (n = 0; n < n_patients; n++) {
        for (k = 0; k <= patient_stages[n]; k++) {
            // sample gene from pathway2genes[k]
            g = pathway2genes[k][gsl_rng_uniform_int(random, pathway2genes[k].size())];
            gsl_matrix_set(data_matrix, n, g, 1.0);
        }
    }
    return data_matrix;
}

void add_noise(gsl_rng *random,
               double fbp,
               double bgp,
               gsl_matrix *matrix)
{
    unsigned int i, j;
    double val, unif;
    for (i = 0; i < matrix->size1; i++) {
        for (j = 0; j < matrix->size2; j++) {
            val = gsl_matrix_get(matrix, i, j);
            unif = gsl_ran_flat(random, 0.0, 1.0);
            if (val == 0) {
                // set val to 1 (from 0) with prob bgp
                if (unif < bgp) {
                    //cout << "Background mutation at (" << i << ", " << j << ")" << endl;
                    val = 1;
                }
            } else {
                // set val to 0 (from 1) with prob fbp
                if (unif < fbp) {
                    //cout << "Flip back at (" << i << ", " << j << ")" << endl;
                    val = 0;
                }
            }
            gsl_matrix_set(matrix, i, j, val);
        }
    }
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    NonlinearFactorGraph.cpp
 * @brief   Factor Graph Consisting of non-linear factors
 * @author  Frank Dellaert
 * @author  Carlos Nieto
 * @author  Christian Potthast
 */

#include <cmath>
#include <boost/foreach.hpp>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/inference.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
double NonlinearFactorGraph::probPrime(const Values& c) const {
	return exp(-0.5 * error(c));
}

/* ************************************************************************* */
void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
	cout << str << "size: " << size() << endl;
	for (size_t i = 0; i < factors_.size(); i++) {
		stringstream ss;
		ss << "factor " << i << ": ";
		if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
	}
}

/* ************************************************************************* */
double NonlinearFactorGraph::error(const Values& c) const {
	double total_error = 0.;
	// iterate over all the factors_ to accumulate the log probabilities
	BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
		if(factor)
			total_error += factor->error(c);
	}
	return total_error;
}

/* ************************************************************************* */
FastSet<Key> NonlinearFactorGraph::keys() const {
	FastSet<Key> keys;
	BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
		if(factor)
			keys.insert(factor->begin(), factor->end());
	}
	return keys;
}

/* ************************************************************************* */
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
		const Values& config) const {

	// Create symbolic graph and initial (iterator) ordering
	SymbolicFactorGraph::shared_ptr symbolic;
	Ordering::shared_ptr ordering;
	boost::tie(symbolic, ordering) = this->symbolic(config);

	// Compute the VariableIndex (column-wise index)
	VariableIndex variableIndex(*symbolic, ordering->size());
	if (config.size() != variableIndex.size()) throw std::runtime_error(
			"orderingCOLAMD: some variables in the graph are not constrained!");

	// Compute a fill-reducing ordering with COLAMD
	Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD(
			variableIndex));

	// Permute the Ordering with the COLAMD ordering
	ordering->permuteWithInverse(*colamdPerm->inverse());
	return ordering;
}

/* ************************************************************************* */
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Values& config,
		const std::map<Key, int>& constraints) const {
	// Create symbolic graph and initial (iterator) ordering
	SymbolicFactorGraph::shared_ptr symbolic;
	Ordering::shared_ptr ordering;
	boost::tie(symbolic, ordering) = this->symbolic(config);

	// Compute the VariableIndex (column-wise index)
	VariableIndex variableIndex(*symbolic, ordering->size());
	if (config.size() != variableIndex.size()) throw std::runtime_error(
			"orderingCOLAMD: some variables in the graph are not constrained!");

	// Create a constraint list with correct indices
	typedef std::map<Key, int>::value_type constraint_type;
	std::map<Index, int> index_constraints;
	BOOST_FOREACH(const constraint_type& p, constraints)
		index_constraints[ordering->at(p.first)] = p.second;

	// Compute a constrained fill-reducing ordering with COLAMD
	Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMDGrouped(
			variableIndex, index_constraints));

	// Permute the Ordering with the COLAMD ordering
	ordering->permuteWithInverse(*colamdPerm->inverse());
	return ordering;
}

/* ************************************************************************* */
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
	// Generate the symbolic factor graph
	SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
	symbolicfg->reserve(this->size());

	BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
		if(factor)
			symbolicfg->push_back(factor->symbolic(ordering));
		else
			symbolicfg->push_back(SymbolicFactorGraph::sharedFactor());
	}

	return symbolicfg;
}

/* ************************************************************************* */
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
		const Values& config) const {
	// Generate an initial key ordering in iterator order
	Ordering::shared_ptr ordering(config.orderingArbitrary());
	return make_pair(symbolic(*ordering), ordering);
}

/* ************************************************************************* */
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
		const Values& config, const Ordering& ordering) const {

	// create an empty linear FG
	GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
	linearFG->reserve(this->size());

	// linearize all factors
	BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
		if(factor) {
			GaussianFactor::shared_ptr lf = factor->linearize(config, ordering);
			if (lf) linearFG->push_back(lf);
		} else
			linearFG->push_back(GaussianFactor::shared_ptr());
	}

	return linearFG;
}

/* ************************************************************************* */
NonlinearFactorGraph NonlinearFactorGraph::clone() const {
	NonlinearFactorGraph result;
	BOOST_FOREACH(const sharedFactor& f, *this) {
		if (f)
			result.push_back(f->clone());
		else
			result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
	}
	return result;
}

/* ************************************************************************* */
NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map<Key,Key>& rekey_mapping) const {
	NonlinearFactorGraph result;
	BOOST_FOREACH(const sharedFactor& f, *this) {
		if (f)
			result.push_back(f->rekey(rekey_mapping));
		else
			result.push_back(sharedFactor());
	}
	return result;
}

/* ************************************************************************* */

} // namespace gtsam


#include <src/algorithms/semihdp_sampler.h>
#include <src/collectors/memory_collector.h>
#include <src/includes.h>

#include <Eigen/Dense>
#include <stan/math/prim.hpp>
#include <vector>

#include "utils.hpp"
#include "src/utils/io_utils.h"

template<typename M>
M load_csv (const std::string & path) {
    std::ifstream indata;
    indata.open(path);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line)) {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ',')) {
            values.push_back(std::stod(cell));
        }
        ++rows;
    }
    return Map<const Matrix<typename M::Scalar, M::RowsAtCompileTime, M::ColsAtCompileTime, RowMajor>>
        (values.data(), rows, values.size()/rows);
}

std::vector<MatrixXd> read_data(std::string filename) {
   Eigen::MatrixXd data = load_csv<Eigen::MatrixXd>(filename);
   int ngroups = static_cast<int>(data.col(0).maxCoeff()) + 1;
   std::vector<std::vector<double>> tmp(ngroups);
   for (int i=0; i < data.rows(); i++) {
       int group = static_cast<int>(data(i, 0));
       double x = data(i, 1);
       tmp[group].push_back(x);
    }
    std::vector<MatrixXd> out(ngroups);
    for (int i=0; i < tmp.size(); i++) {
        Eigen::MatrixXd groupdata = Eigen::Map<Eigen::MatrixXd>(
            tmp[i].data(), tmp[i].size(), 1);
        out[i] = groupdata;
    }

   return out;
}

int main(int argc, char *argv[]) {
    std::string data_file = argv[1];
    std::string params_file = argv[2];
    std::string output_chains_file = argv[3];
    std::string output_rest_file = argv[4];
    std::string output_latent_vars_file = argv[5];
    std::string dens_grid_file = argv[6];
    std::string output_dens_path = argv[7];
    int niter = std::stoi(argv[8]);
    int nburn = std::stoi(argv[9]);
    int thin = std::stoi(argv[10]);
    std::string update_c = argv[11];
    int seed = std::stoi(argv[12]);

    auto &rng = bayesmix::Rng::Instance().get();
    rng.seed(seed);

    std::vector<MatrixXd> data = read_data(data_file);
    Eigen::MatrixXd dens_grid = load_csv<Eigen::MatrixXd>(dens_grid_file);
    MemoryCollector chains;
    chains = run_semihdp(data, output_chains_file, params_file,
                         niter, nburn, thin, update_c);

    Eigen::MatrixXi rest_allocs = get_rest_allocs(chains, data.size());
    const static Eigen::IOFormat CSVFormat(
        Eigen::StreamPrecision, Eigen::DontAlignCols, ",", "\n");
    std::ofstream file(output_rest_file.c_str());
    file << rest_allocs.format(CSVFormat);

    std::vector<MatrixXd> densities = eval_uni_dens(chains, dens_grid, data.size());
    for (int i = 0; i < densities.size(); i++) {
        std::string dens_file = output_dens_path + "/group_" +
                                std::to_string(i) + ".csv";
        bayesmix::write_matrix_to_file(densities[i], dens_file);
    }

    Eigen::MatrixXd latent_vars = get_latent_vars(chains, data.size());
    bayesmix::write_matrix_to_file(latent_vars, output_latent_vars_file);
    return -1;
}


/*
 * Copyright (c) 2012, Frederic Dubouchet
 * 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 CERN 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 Frederic Dubouchet ``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 Frederic DUBOUCHET 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.
 */

#include <stdio.h>
#include <iostream>
#include <vector>
#define __CL_ENABLE_EXCEPTIONS
#include <cl.hpp>
#ifdef __linux__
#include <GL/glx.h>
#endif
#include <boost/date_time/posix_time/posix_time.hpp>

#include "cl_floyd_warshall.h"

using namespace boost::posix_time;

cl_floyd_warshall::cl_floyd_warshall(bool gpu) {
	//setup devices_ and context_
	std::vector<cl::Platform> platforms;
	err_ = cl::Platform::get(&platforms);
	device_used_ = 0;
	err_ = platforms[0].getDevices(
		(gpu) ?	CL_DEVICE_TYPE_GPU : CL_DEVICE_TYPE_CPU,
		&devices_);
	cl_context_properties properties[] = {
		CL_CONTEXT_PLATFORM,
		(cl_context_properties)(platforms[0])(),
		0
	};
	context_ = cl::Context(
		(gpu) ? CL_DEVICE_TYPE_GPU : CL_DEVICE_TYPE_CPU ,
		properties);
	devices_ = context_.getInfo<CL_CONTEXT_DEVICES>();
	queue_ = cl::CommandQueue(context_, devices_[device_used_], 0, &err_);
}

void cl_floyd_warshall::init(const std::string& cl_file) {
	FILE* file = fopen(cl_file.c_str(), "rt");
	if (!file) throw std::runtime_error("could not open file " + cl_file);
	const unsigned int BUFFER_SIZE = 1024 * 1024;
	size_t bytes_read = 0;
	std::string kernel__source = "";
	do {
		char temp[BUFFER_SIZE];
		memset(temp, 0, BUFFER_SIZE);
		bytes_read = fread(temp, sizeof(char), BUFFER_SIZE, file);
		kernel__source += temp;
	} while (bytes_read != 0);
	fclose(file);
	cl::Program::Sources source(
		1,
		std::make_pair(
			kernel__source.c_str(),
			kernel__source.size()));
	program_ = cl::Program(context_, source);
	try {
		err_ = program_.build(devices_);
	} catch (cl::Error er) {
		std::cerr << "build status    : "
		<< program_.getBuildInfo<CL_PROGRAM_BUILD_STATUS>(devices_[0]) << std::endl;
		std::cerr << "build options   : "
		<< program_.getBuildInfo<CL_PROGRAM_BUILD_OPTIONS>(devices_[0]) << std::endl;
		std::cerr << "build log       : "  << std::endl
		<< program_.getBuildInfo<CL_PROGRAM_BUILD_LOG>(devices_[0]) << std::endl;
		throw er;
	}
}

void cl_floyd_warshall::setup(const std::pair<unsigned int, unsigned int>& s) {
	mdx_ = s.first;
	mdy_ = s.second;
}

void cl_floyd_warshall::prepare(const std::vector<float>& in) {
	total_size_ = mdx_ * mdy_;
	if (in.size() != (total_size_))
		throw std::runtime_error("input buffer size != image size!");
	kernel_ = cl::Kernel(program_, "floyd_warshall_buffer", &err_);
	//initialize our CPU memory arrays, send them to the device and set the kernel_ arguments
	cl_buffer_mat_ = cl::Buffer(
		context_,
		CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
		sizeof(float) * total_size_,
		(void*)&in[0],
		&err_);
	cl_buffer_in_x_ = cl::Buffer(
        context_,
        CL_MEM_READ_WRITE,
        sizeof(float) * mdx_,
        NULL,
        &err_);
	cl_buffer_in_y_ = cl::Buffer(
        context_,
        CL_MEM_READ_WRITE,
        sizeof(float) * mdy_,
        NULL,
        &err_);
	queue_.finish();
	//set the arguements of our kernel_
	err_ = kernel_.setArg(0, cl_buffer_mat_);
	err_ = kernel_.setArg(1, cl_buffer_in_x_);
	err_ = kernel_.setArg(2, cl_buffer_in_y_);
	//Wait for the command queue_ to finish these commands before proceeding
	queue_.finish();
}

time_duration cl_floyd_warshall::run(std::vector<float>& mat) {
	ptime before;
	ptime after;
	assert(mdx_ == mdy_);
	time_duration total = seconds(0);
	if (mat.size() != total_size_)
		mat.resize(total_size_);
	cl::size_t<3> dst_origin;
	dst_origin[0] = 0;
	dst_origin[1] = 0;
	dst_origin[2] = 0;
	for (int i = 0; i < mdx_; ++i) {
		std::cout << "Compute generation [" << i + 1 << "/" << mdx_ << "]\r";
		std::cout.flush();
		before = microsec_clock::universal_time();
		// create the cl_buffer_in_x_ and cl_buffer_in_y_
		{ // cl_buffer_x_
			cl::size_t<3> src_origin;
			cl::size_t<3> region;
			src_origin[0] = i * sizeof(float);
			src_origin[1] = 0;
			src_origin[2] = 0;
			region[0] = sizeof(float);
			region[1] = mdx_;
			region[2] = 1;
			queue_.enqueueCopyBufferRect(
				cl_buffer_mat_,
				cl_buffer_in_x_,
				src_origin,
				dst_origin,
				region,
				mdx_ * sizeof(float),
				0,
				sizeof(float),
				0,
				nullptr,
				&event_);
		}
		{ // cl_buffer_y_
			queue_.enqueueCopyBuffer(
				cl_buffer_mat_,
				cl_buffer_in_y_,
				mdx_ * i * sizeof(float),
				0,
				mdx_ * sizeof(float),
				nullptr,
				&event_);
		}
		queue_.finish();
		// make the computation
		err_ = queue_.enqueueNDRangeKernel(
			kernel_,
			cl::NullRange,
			cl::NDRange(mdx_, mdy_),
			cl::NullRange,
			NULL,
			&event_);
		queue_.finish();
		after = microsec_clock::universal_time();
		total += (after - before);
	}
	std::cout << std::endl;
	err_ = queue_.enqueueReadBuffer(
         cl_buffer_mat_,
         CL_TRUE,
         0,
         sizeof(float) * total_size_,
         &mat[0],
         NULL,
         &event_);
	queue_.finish();
	return total;
}


#include <PushRelabel.h>

#include <cassert>
#include <iostream>

#include <utils.h>

#include <boost/graph/push_relabel_max_flow.hpp>

// some stuff needed for boost
struct IntIterator {
    size_t val = 0;
    IntIterator() = default;
    IntIterator(size_t start) : val(start) {}
    auto operator!=(const IntIterator& other) { return val != other.val; }
    auto operator==(const IntIterator& other) { return val == other.val; }
    auto operator*() { return val; }
    auto operator++() { ++val; }
    auto operator--() { --val; }
};

template<>
struct boost::graph_traits<GoldbergTarjan::LinearizedNetwork> {
    using vertex_descriptor = size_t;
    using edge_descriptor = size_t;
    using vertex_iterator = IntIterator;
    using out_edge_iterator = IntIterator;
    using vertices_size_type = size_t;
    using edges_size_type = size_t;
};

namespace GoldbergTarjan {
    struct EdgeCapMap { std::vector<Edge>& data; };
    struct EdgeResCapMap { std::vector<Edge>& data; };
    struct EdgeRevMap { std::vector<Edge>& data; };
    auto get(EdgeCapMap& m, size_t e) { return m.data[e].capacity; }
    //auto get(EdgeResCapMap& m, size_t e) { return m.data[e].residual_capacity < 1e-6 ? 0.0 : m.data[e].residual_capacity; } // very small == nothing
    double get(EdgeResCapMap& m, size_t e) { return m.data[e].residual_capacity; } // very small == nothing
    auto get(EdgeRevMap& m, size_t e) { return m.data[e].reverse_edge; }
    // auto put(EdgeCapMap& m, size_t e, double val) { m.data[e].capacity = val; }
    auto put(EdgeResCapMap& m, size_t e, double val) { m.data[e].residual_capacity = val; }
    // auto put(EdgeRevMap& m, size_t e, size_t val) { m.data[e].reverse_edge = val; }

    auto num_vertices(LinearizedNetwork& g) { return g.n; }
    auto num_edges(LinearizedNetwork& g) { return g.data.size(); }
    auto vertices(LinearizedNetwork& g) { return std::pair(IntIterator(0),IntIterator(g.n)); }
    auto out_edges(size_t v, LinearizedNetwork& g) { return std::pair(IntIterator(g.adj[v].edge_begin), IntIterator(g.adj[v].edge_end)); }
    auto source(size_t e, LinearizedNetwork& g) { return g.data[e].from; }
    auto target(size_t e, LinearizedNetwork& g) { return g.data[e].to; }
}

// pimpl pattern hidden data
namespace GoldbergTarjan {
    class InternalBoostAlg {
    public:
        boost::detail::push_relabel<LinearizedNetwork, EdgeCapMap, EdgeResCapMap, EdgeRevMap, boost::identity_property_map, double> m_alg;
        InternalBoostAlg(LinearizedNetwork& graph, size_t s, size_t t)
            : m_alg(graph, {graph.data}, {graph.data}, {graph.data}, static_cast<size_t>(s), static_cast<size_t>(t), boost::identity_property_map()) {
        }
    };
}
// end boost stuff

using namespace std;

GoldbergTarjan::LinearizedNetwork::LinearizedNetwork(const EdgeList &graph) : n(graph.n) {
    if(!graph.isDirected) printf("lin net supports only directed for now"), exit(0);

    adj.resize(n);
    data.resize(2*std::size(graph.edges));

    vector<size_t> deg(n,0); // first stores degree, then index for next edge
    for(auto [u,v] : graph.edges) deg[u]++, deg[v]++;
    adj[0].edge_begin = 0;
    for(int i=0; i<n; ++i) {
        adj[i].edge_end = adj[i].edge_begin + deg[i];
        deg[i] = adj[i].edge_begin;
        if(i+1<n) adj[i+1].edge_begin = adj[i].edge_end;
    }
    assert(adj[n-1].edge_end == 2*size(graph.edges));

    for(int i=0; i<size(graph.edges); ++i) {
        auto [u,v] = graph.edges[i];
        auto w = graph.hasWeights ? graph.weights[i] : 1.0;
        auto e1 = deg[u];
        data[e1] = Edge{w,w,(size_t)u,(size_t)v,0};
        deg[u]++;
        auto e2 = deg[v];
        data[e2] = Edge{0,0,(size_t)v,(size_t)u,0};
        deg[v]++;
        data[e1].reverse_edge = e2;
        data[e2].reverse_edge = e1;
    }

#ifndef NDEBUG
    assert(adj.front().edge_begin == size_t(0));
    assert(adj.back().edge_end == size(data));
    for(int i=0; i<n; ++i) {
        for(auto [be,en] = adj[i]; be != en; ++be) {
            assert(be<=en && en<=size(data));
            assert(data[be].from==i);
            assert(data[be].reverse_edge < size(data));
            assert(data[data[be].reverse_edge].from==data[be].to);
            assert(data[data[be].reverse_edge].to==i);
        }
    }
#endif
}

GoldbergTarjan::PushRelabel::PushRelabel(const EdgeList &graph)
: m_graph(graph.isDirected ? graph : undirectedToDirected(graph))
{ }

GoldbergTarjan::PushRelabel::~PushRelabel() {};

void GoldbergTarjan::PushRelabel::init(int s, int t) {
    if(m_state != EMPTY) cout << "WARNING: PR state was not empty before init\n";
    m_internal_alg = make_unique<InternalBoostAlg>(m_graph, static_cast<size_t>(s), static_cast<size_t>(t));
    m_state = INIT;
}

double GoldbergTarjan::PushRelabel::maxFlow(int s, int t) {

    if(m_state != INIT || !m_internal_alg || m_internal_alg->m_alg.src != s || m_internal_alg->m_alg.sink != t)
        cout << "ERROR: PR was not or incorrect initialized\n", exit(0);

    auto flow = m_internal_alg->m_alg.maximum_preflow();
    assert(m_internal_alg->m_alg.is_optimal());
    m_state = PREFLOW;

    return flow;
}

void GoldbergTarjan::PushRelabel::convertPreflow() {
    if(m_state != PREFLOW)
        cout << "ERROR: must compute preflow before flow\n", exit(0);
    m_internal_alg->m_alg.convert_preflow_to_flow();
    assert(m_internal_alg->m_alg.is_flow()); // may break when using non-integer weights
    m_state = FLOW;
}

void GoldbergTarjan::PushRelabel::deinit() {
    if(m_state==EMPTY) cout << "WARNING: PR deinit  called while empty\n";
    m_internal_alg.reset();
    m_state = EMPTY;
}