#include "partition/bisection_graph_view.hpp"
#include "partition/dinic_max_flow.hpp"
#include "partition/graph_generator.hpp"
#include "partition/recursive_bisection_state.hpp"

#include <algorithm>
#include <vector>

#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>

using namespace osrm::partition;
using namespace osrm::util;

BOOST_AUTO_TEST_SUITE(dinic_algorithm)

BOOST_AUTO_TEST_CASE(horizontal_cut_between_two_grids)
{
    // 40 entries of left/right edges
    const double step_size = 0.01;
    const int rows = 10;
    const int cols = 10;

    // build a small grid (10*10) and a (100 * 10) below (to make the different steps unique)
    auto graph = [&]() {
        std::vector<Coordinate> grid_coordinates;
        std::vector<EdgeWithSomeAdditionalData> grid_edges;

        const auto connect = [&grid_edges](const NodeID from, const NodeID to) {
            grid_edges.push_back({from, to, 1});
            grid_edges.push_back({to, from, 1});
        };

        // 10 rows of large components, interrupted by small disconnected components
        const auto small_coordinates = makeGridCoordinates(rows, cols, step_size, 0, 0);
        grid_coordinates.insert(
            grid_coordinates.end(), small_coordinates.begin(), small_coordinates.end());

        // connect the grid edges, starting with i * (rows * cols + 1) as first id (0,11,22...)
        const auto small_edges = makeGridEdges(rows, cols, 0);
        grid_edges.insert(grid_edges.end(), small_edges.begin(), small_edges.end());

        const auto large_coordinates =
            makeGridCoordinates(10 * rows, cols, step_size, 0, rows * step_size);
        grid_coordinates.insert(
            grid_coordinates.end(), large_coordinates.begin(), large_coordinates.end());
        const auto large_edges = makeGridEdges(10 * rows, cols, (rows * cols));
        grid_edges.insert(grid_edges.end(), large_edges.begin(), large_edges.end());

        connect(45, 1001);
        connect(55, 800);
        connect(65, 600);
        connect(75, 200);

        groupEdgesBySource(grid_edges.begin(), grid_edges.end());
        return makeBisectionGraph(grid_coordinates, adaptToBisectionEdge(std::move(grid_edges)));
    }();

    RecursiveBisectionState bisection_state(graph);
    BisectionGraphView view(graph);

    DinicMaxFlow::SourceSinkNodes sources, sinks;

    for (int i = 0; i < 10; ++i)
    {
        sources.insert(static_cast<NodeID>(i));
        sinks.insert(static_cast<NodeID>(1000 + i));
    }

    DinicMaxFlow flow;
    const auto cut = flow(view, sources, sinks);
    BOOST_CHECK(cut.num_edges == 4);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * Copyright (c) 2018, University Osnabrück
 * 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 University Osnabrück nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL University Osnabrück 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.
 */

/*
 * Matrix.hpp
 *
 *  @date 26.08.2008
 *  @author Thomas Wiemann (twiemann@uos.de)
 */

#ifndef LVR2_MATRIX_H_
#define LVR2_MATRIX_H_

#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>

#include <lvr2/geometry/Normal.hpp>
#include <lvr2/io/DataStruct.hpp>

#define _USE_MATH_DEFINES
#include <cmath>

#include <Eigen/Dense>

#ifndef M_PI
#define M_PI 3.141592654
#endif
using namespace std;

namespace lvr2
{

/**
 * @brief	A 4x4 matrix class implementation for use with the provided
 * 			vertex types.
 */
template<typename BaseVecT>
class Matrix4 {
public:

    using ValueType = typename BaseVecT::CoordType;

	/**
	 * @brief 	Default constructor. Initializes a identity matrix.
	 */
	Matrix4()
	{
		for(int i = 0; i < 16; i++) m[i] = 0;
		m[0] = m[5] = m[10] = m[15] = 1;
	}

	/**
	 * @brief	Initializes a matrix wit the given data array. Ensure
	 * 			that the array has exactly 16 fields.
	 */
	template<typename T>
	Matrix4(T* matrix)
	{
		for(int i = 0; i < 16; i++) m[i] = matrix[i];
	}

	/**
	 * @brief 	Copy constructor.
	 */
	template<typename T>
	Matrix4(const Matrix4<T>& other)
	{
		for(int i = 0; i < 16; i++) m[i] = other[i];
	}

	/**
	 * @brief	Constructs a matrix from given axis and angle. Trys to
	 * 			avoid a gimbal lock.
	 */
	template<typename T>
	Matrix4(T axis, ValueType angle)
	{
		// Check for gimbal lock
		if(fabs(angle) < 0.0001){

			bool invert_z = axis.z < 0;

			//Angle to yz-plane
			float pitch = atan2(axis.z, axis.x) - M_PI_2;
			if(pitch < 0.0f) pitch += 2.0f * M_PI;

			if(axis.x == 0.0f && axis.z == 0.0) pitch = 0.0f;

			//Transform axis into yz-plane
			axis.x =  axis.x * cos(pitch) + axis.z * sin(pitch);
			axis.z = -axis.x * sin(pitch) + axis.z * cos(pitch);

			//Angle to y-Axis
			float yaw = atan2(axis.y, axis.z);
			if(yaw < 0) yaw += 2 * M_PI;

			Matrix4<BaseVecT> m1, m2, m3;

			if(invert_z) yaw = -yaw;

			cout << "YAW: " << yaw << " PITCH: " << pitch << endl;

			if(fabs(yaw)   > 0.0001){
				m2 = Matrix4(T(1.0, 0.0, 0.0), yaw);
				m3 = m3 * m2;
			}

			if(fabs(pitch) > 0.0001){
				m1 = Matrix4(T(0.0, 1.0, 0.0), pitch);
				m3 = m3 * m1;
			}

			for(int i = 0; i < 16; i++) m[i] = m3[i];

		} else {
			float c = cos(angle);
			float s = sin(angle);
			float t = 1.0f - c;
			float tmp1, tmp2;

			// Normalize axis
			Normal<ValueType> a(axis);

			m[ 0] = c + a.x * a.x * t;
			m[ 5] = c + a.y * a.y * t;
			m[10] = c + a.z * a.z * t;

			tmp1 = a.x * a.y * t;
			tmp2 = a.z * s;
			m[ 4] = tmp1 + tmp2;
			m[ 1] = tmp1 - tmp2;

			tmp1 = a.x * a.z * t;
			tmp2 = a.y * s;
			m[ 8] = tmp1 - tmp2;
			m[ 2] = tmp1 + tmp2;

			tmp1 = a.y * a.z * t;
			tmp2 = a.x * s;
			m[ 9] = tmp1 + tmp2;
			m[ 6] = tmp1 - tmp2;

			m[ 3] = m[ 7] = m[11] = 0.0;
			m[12] = m[13] = m[14] = 0.0;
			m[15] = 1.0;
		}
	}

	template<typename T>
	Matrix4(const T &position, const T &angles)
	{
		float sx = sin(angles[0]);
		float cx = cos(angles[0]);
		float sy = sin(angles[1]);
		float cy = cos(angles[1]);
		float sz = sin(angles[2]);
		float cz = cos(angles[2]);

		m[0]  = cy*cz;
		m[1]  = sx*sy*cz + cx*sz;
		m[2]  = -cx*sy*cz + sx*sz;
		m[3]  = 0.0;
		m[4]  = -cy*sz;
		m[5]  = -sx*sy*sz + cx*cz;
		m[6]  = cx*sy*sz + sx*cz;
		m[7]  = 0.0;
		m[8]  = sy;
		m[9]  = -sx*cy;
		m[10] = cx*cy;

		m[11] = 0.0;

		m[12] = position[0];
		m[13] = position[1];
		m[14] = position[2];
		m[15] = 1;
	}

	Matrix4(string filename);

	~Matrix4()
	{

	}

    Matrix4& operator=(const Eigen::Matrix4d& mat)
    {
        m[0]  = mat(0, 0);
        m[1]  = mat(0, 1);
        m[2]  = mat(0, 2);
        m[3]  = mat(0, 3);

        m[4]  = mat(1, 0);
        m[5]  = mat(1, 1);
        m[6]  = mat(1, 2);
        m[7]  = mat(1, 3);

        m[8]  = mat(2, 0);
        m[9]  = mat(2, 1);
        m[10] = mat(2, 2);
        m[11] = mat(2, 3);

        m[12] = mat(3, 0);
        m[13] = mat(3, 1);
        m[14] = mat(3, 2);
        m[15] = mat(3, 3);

        return *this;
    }

	/**
	 * @brief	Scales the matrix elemnts by the given factor
	 */
	Matrix4<BaseVecT> operator*(const ValueType &scale) const
	{
		ValueType new_matrix[16];
		for(int i = 0; i < 16; i++){
			new_matrix[i] = m[i] * scale;
		}
		return Matrix4<BaseVecT>(new_matrix);
	}

	/**
	 * @brief	Matrix-Matrix multiplication. Returns the new
	 * 			matrix
	 */
	template<typename T>
	Matrix4<BaseVecT> operator*(const Matrix4<T> &other) const
	{
		ValueType new_matrix[16];
		new_matrix[ 0] = m[ 0] * other[ 0] + m[ 4] * other[ 1] + m[ 8] * other[ 2] + m[12] * other[ 3];
		new_matrix[ 1] = m[ 1] * other[ 0] + m[ 5] * other[ 1] + m[ 9] * other[ 2] + m[13] * other[ 3];
		new_matrix[ 2] = m[ 2] * other[ 0] + m[ 6] * other[ 1] + m[10] * other[ 2] + m[14] * other[ 3];
		new_matrix[ 3] = m[ 3] * other[ 0] + m[ 7] * other[ 1] + m[11] * other[ 2] + m[15] * other[ 3];
		new_matrix[ 4] = m[ 0] * other[ 4] + m[ 4] * other[ 5] + m[ 8] * other[ 6] + m[12] * other[ 7];
		new_matrix[ 5] = m[ 1] * other[ 4] + m[ 5] * other[ 5] + m[ 9] * other[ 6] + m[13] * other[ 7];
		new_matrix[ 6] = m[ 2] * other[ 4] + m[ 6] * other[ 5] + m[10] * other[ 6] + m[14] * other[ 7];
		new_matrix[ 7] = m[ 3] * other[ 4] + m[ 7] * other[ 5] + m[11] * other[ 6] + m[15] * other[ 7];
		new_matrix[ 8] = m[ 0] * other[ 8] + m[ 4] * other[ 9] + m[ 8] * other[10] + m[12] * other[11];
		new_matrix[ 9] = m[ 1] * other[ 8] + m[ 5] * other[ 9] + m[ 9] * other[10] + m[13] * other[11];
		new_matrix[10] = m[ 2] * other[ 8] + m[ 6] * other[ 9] + m[10] * other[10] + m[14] * other[11];
		new_matrix[11] = m[ 3] * other[ 8] + m[ 7] * other[ 9] + m[11] * other[10] + m[15] * other[11];
		new_matrix[12] = m[ 0] * other[12] + m[ 4] * other[13] + m[ 8] * other[14] + m[12] * other[15];
		new_matrix[13] = m[ 1] * other[12] + m[ 5] * other[13] + m[ 9] * other[14] + m[13] * other[15];
		new_matrix[14] = m[ 2] * other[12] + m[ 6] * other[13] + m[10] * other[14] + m[14] * other[15];
		new_matrix[15] = m[ 3] * other[12] + m[ 7] * other[13] + m[11] * other[14] + m[15] * other[15];
		return Matrix4<BaseVecT>(new_matrix);
	}

	/**
	 * @brief 	Matrix addition operator. Returns a new matrix
	 *
	 */
	template<typename T>
	Matrix4<BaseVecT> operator+(const Matrix4<T> &other) const
	{
		ValueType new_matrix[16];
		for(int i = 0; i < 16; i++)
		{
			new_matrix[i] = m[i] + other[i];
		}
		return Matrix4<BaseVecT>(new_matrix);
	}

	/**
	 * @brief 	Matrix addition operator
	 */
	template<typename T>
	Matrix4<BaseVecT> operator+=(const Matrix4<T> &other)
	{
		//if(other != *this)
		//{
			return *this + other;
		//}
		//else
		//{
			//return *this;
		//}
	}

	/**
	 * @brief	Matrix-Matrix multiplication (array based). Mainly
	 * 			implemented for compatibility with other math libs.
	 * 			ensure that the used array has at least 16 elements
	 * 			to avoid memory access violations.
	 */
	template<typename T>
	Matrix4<BaseVecT> operator*(const T* &other) const
	{
		ValueType new_matrix[16];
		new_matrix[ 0] = m[ 0] * other[ 0] + m[ 4] * other[ 1] + m[ 8] * other[ 2] + m[12] * other[ 3];
		new_matrix[ 1] = m[ 1] * other[ 0] + m[ 5] * other[ 1] + m[ 9] * other[ 2] + m[13] * other[ 3];
		new_matrix[ 2] = m[ 2] * other[ 0] + m[ 6] * other[ 1] + m[10] * other[ 2] + m[14] * other[ 3];
		new_matrix[ 3] = m[ 3] * other[ 0] + m[ 7] * other[ 1] + m[11] * other[ 2] + m[15] * other[ 3];
		new_matrix[ 4] = m[ 0] * other[ 4] + m[ 4] * other[ 5] + m[ 8] * other[ 6] + m[12] * other[ 7];
		new_matrix[ 5] = m[ 1] * other[ 4] + m[ 5] * other[ 5] + m[ 9] * other[ 6] + m[13] * other[ 7];
		new_matrix[ 6] = m[ 2] * other[ 4] + m[ 6] * other[ 5] + m[10] * other[ 6] + m[14] * other[ 7];
		new_matrix[ 7] = m[ 3] * other[ 4] + m[ 7] * other[ 5] + m[11] * other[ 6] + m[15] * other[ 7];
		new_matrix[ 8] = m[ 0] * other[ 8] + m[ 4] * other[ 9] + m[ 8] * other[10] + m[12] * other[11];
		new_matrix[ 9] = m[ 1] * other[ 8] + m[ 5] * other[ 9] + m[ 9] * other[10] + m[13] * other[11];
		new_matrix[10] = m[ 2] * other[ 8] + m[ 6] * other[ 9] + m[10] * other[10] + m[14] * other[11];
		new_matrix[11] = m[ 3] * other[ 8] + m[ 7] * other[ 9] + m[11] * other[10] + m[15] * other[11];
		new_matrix[12] = m[ 0] * other[12] + m[ 4] * other[13] + m[ 8] * other[14] + m[12] * other[15];
		new_matrix[13] = m[ 1] * other[12] + m[ 5] * other[13] + m[ 9] * other[14] + m[13] * other[15];
		new_matrix[14] = m[ 2] * other[12] + m[ 6] * other[13] + m[10] * other[14] + m[14] * other[15];
		new_matrix[15] = m[ 3] * other[12] + m[ 7] * other[13] + m[11] * other[14] + m[15] * other[15];
		return Matrix4<BaseVecT>(new_matrix);
	}

	/**
	 * @brief	Multiplication of Matrix and Vertex types
	 */
	template<typename T>
	T operator*(const T &v) const
	{
        using ValType = typename T::CoordType;
		ValType x = m[ 0] * v.x + m[ 4] * v.y + m[8 ] * v.z;
		ValType y = m[ 1] * v.x + m[ 5] * v.y + m[9 ] * v.z;
		ValType z = m[ 2] * v.x + m[ 6] * v.y + m[10] * v.z;

		x = x + m[12];
		y = y + m[13];
		z = z + m[14];

		return T(x, y, z);
	}

    /**
     * @brief   Multiplication of Matrix and Vertex types
     */
    template<typename T>
    Normal<T> operator*(const Normal<T> &v) const
    {
        T x = m[ 0] * v.x + m[ 4] * v.y + m[8 ] * v.z;
        T y = m[ 1] * v.x + m[ 5] * v.y + m[9 ] * v.z;
        T z = m[ 2] * v.x + m[ 6] * v.y + m[10] * v.z;

        return Normal<T>(x, y, z);
    }

	/**
	 * @brief	Sets the given index of the Matrix's data field
	 * 			to the provided value.
	 *
	 * @param	i		Field index of the matrix
	 * @param	value	new value
	 */
	void set(int i, ValueType value){m[i] = value;};

	/**
	 * @brief	Transposes the current matrix
	 */
	void transpose()
	{
		ValueType m_tmp[16];
		m_tmp[0]  = m[0];
		m_tmp[4]  = m[1];
		m_tmp[8]  = m[2];
		m_tmp[12] = m[3];
		m_tmp[1]  = m[4];
		m_tmp[5]  = m[5];
		m_tmp[9]  = m[6];
		m_tmp[13] = m[7];
		m_tmp[2]  = m[8];
		m_tmp[6]  = m[9];
		m_tmp[10] = m[10];
		m_tmp[14] = m[11];
		m_tmp[3]  = m[12];
		m_tmp[7]  = m[13];
		m_tmp[11] = m[14];
		m_tmp[15] = m[15];
		for(int i = 0; i < 16; i++) m[i] = m_tmp[i];
	}

	/**
	 * @brief	Computes an Euler representation (x, y, z) plus three
	 * 			rotation values in rad. Rotations are with respect to
	 * 			the x, y, z axes.
	 */
	void toPostionAngle(ValueType pose[6])
	{
		if(pose != 0){
			float _trX, _trY;
			if(m[0] > 0.0) {
				pose[4] = asin(m[8]);
			} else {
				pose[4] = (float)M_PI - asin(m[8]);
			}
			// rPosTheta[1] =  asin( m[8]);      // Calculate Y-axis angle

			float  C    =  cos( pose[4] );
			if ( fabs( C ) > 0.005 )  {          // Gimball lock?
				_trX      =  m[10] / C;          // No, so get X-axis angle
				_trY      =  -m[9] / C;
				pose[3]  = atan2( _trY, _trX );
				_trX      =  m[0] / C;           // Get Z-axis angle
				_trY      = -m[4] / C;
				pose[5]  = atan2( _trY, _trX );
			} else {                             // Gimball lock has occurred
				pose[3] = 0.0;                   // Set X-axis angle to zero
				_trX      =  m[5];  //1          // And calculate Z-axis angle
				_trY      =  m[1];  //2
				pose[5]  = atan2( _trY, _trX );
			}

			// cout << pose[3] << " " << pose[4] << " " << pose[5] << endl;

			pose[0] = m[12];
			pose[1] = m[13];
			pose[2] = m[14];
		}
	}

	/**
	 * @brief	Loads matrix values from a given file.
	 */
	void loadFromFile(string filename)
	{
		ifstream in(filename.c_str());
		for(int i = 0; i < 16; i++){
			if(!in.good()){
                cout << "Warning: Matrix::loadFromFile: File not found or corrupted: " << filename << endl;
				return;
			}
			in >> m[i];
		}
	}

	/**
	 * @brief	Matrix scaling with self assignment.
	 */
	template<typename T>
	void operator*=(const T scale)
	{
		*this = *this * scale;
	}

	/**
	 * @brief 	Matrix-Matrix multiplication with self assigment.
	 */
	template<typename T>
	void operator*=(const Matrix4<T>& other)
	{
		*this = *this * other;
	}

	/**
	 * @brief	Matrix-Matrix multiplication (array based). See \ref{operator*}.
	 */
	template<typename T>
	void operator*=(const T* other)
	{
		*this = *this * other;
	}

	/**
	 * @brief	Returns the internal data array. Unsafe. Will probably
	 * 			removed in one of the next versions.
	 */
	ValueType* getData(){ return m;}

    floatArr toFloatArray()
    {
        floatArr a(new float[16]);
        for(int i = 0; i < 16; i++)
        {
            a[i] = m[i];
        }
        return a;
    }

    std::vector<ValueType> getVector()
    {
        std::vector<ValueType> tmp(16);
        for(int i = 0; i < 16; i++)
        {
            tmp.push_back(m[i]);
        }
        return tmp;
    }

	/**
	 * @brief	Returns the element at the given index.
	 */
	ValueType at(const int i) const;

	/**
	 * @brief	Indexed element (reading) access.
	 */
	ValueType operator[](const int index) const
	{
	    /// TODO: Boundary check
	    return m[index];
	}

	/**
	 * @brief  	Writeable index access
	 */
	ValueType& operator[](const int index)
	{
		return m[index];
	}

	/**
	 * @brief   Returns the matrix's determinant
	 */
	ValueType det()
	{
	    ValueType det, result = 0, i = 1.0;
	    ValueType Msub3[9];
	    int    n;
	    for ( n = 0; n < 4; n++, i *= -1.0 ) {
	        submat( Msub3, 0, n );
	        det     = det3( Msub3 );
	        result += m[n] * det * i;
	    }
	    return( result );
	}

	Matrix4<BaseVecT> inv(bool& success)
	{
	    Matrix4<BaseVecT> Mout;
	    ValueType  mdet = det();
	    if ( fabs( mdet ) < 0.00000000000005 ) {
	        cout << "Error matrix inverting! " << mdet << endl;
	        return Mout;
	    }
	    ValueType  mtemp[9];
	    int     i, j, sign;
	    for ( i = 0; i < 4; i++ ) {
	        for ( j = 0; j < 4; j++ ) {
	            sign = 1 - ( (i +j) % 2 ) * 2;
	            submat( mtemp, i, j );
	            Mout[i+j*4] = ( det3( mtemp ) * sign ) / mdet;
	        }
	    }
	    return Mout;
	}

	ValueType m[16];

private:

    /**
     * @brief   Returns a sub matrix without row \ref i and column \ref j.
     */
	void submat(ValueType* submat, int i, int j)
	{
	    int di, dj, si, sj;
	    // loop through 3x3 submatrix
	    for( di = 0; di < 3; di ++ ) {
	        for( dj = 0; dj < 3; dj ++ ) {
	            // map 3x3 element (destination) to 4x4 element (source)
	            si = di + ( ( di >= i ) ? 1 : 0 );
	            sj = dj + ( ( dj >= j ) ? 1 : 0 );
	            // copy element
	            submat[di * 3 + dj] = m[si * 4 + sj];
	        }
	    }
	}

	/**
	 * @brief    Calculates the determinant of a 3x3 matrix
	 *
	 * @param    M  input 3x3 matrix
	 * @return   determinant of input matrix
	 */
	ValueType det3(const ValueType *M )
	{
	  ValueType det;
	  det = (double)(  M[0] * ( M[4]*M[8] - M[7]*M[5] )
	                 - M[1] * ( M[3]*M[8] - M[6]*M[5] )
	                 + M[2] * ( M[3]*M[7] - M[6]*M[4] ));
	  return ( det );
	}

};

/**
 * @brief Output operator for matrices.
 */
template<typename T>
inline ostream& operator<<(ostream& os, const Matrix4<T> matrix){
	os << "Matrix:" << endl;
	os << fixed;
	for(int i = 0; i < 16; i++){
		os << setprecision(4) << matrix[i] << " ";
		if(i % 4 == 3) os << " " <<  endl;
	}
	os << endl;
	return os;
}

} // namespace lvr2

#endif /* MATRIX_H_ */


#include "scan_material.hpp"
#include <cstdlib>
#include <algorithm>
#include <boost/lexical_cast.hpp>
#include <complex>
#include <memory>
#include <fftw3.h>

#include <vector>
#include <random>
#include <chrono>

#include <DebugOps.hpp>
#include <ScanParams.hpp>

#include <cstddef> // for nullptr
#include <cstdint> // for writing as an int32_t and int16_t

using namespace Constants;

/* Here's main */
int main(int argc, char* argv[])
{
	std::time_t tstart = std::time(nullptr);
	std::cout << "\t\t======================================\n"
		<< "\t\t======= scan_material started ========\n"
		<< "\t\t===== " << std::asctime(std::localtime(&tstart))
		<< "\t\t===== on host " << getenv("HOSTNAME") << "\n"
		<< "\t\t===== for " << getenv("nimages") << " images\n"
		<< "\t\t===== with " << getenv("nfibers") << " fibers\n"
		<< "\t\t======================================\n" << std::flush;

	unsigned nthreads = (unsigned)atoi( getenv("nthreads") );
	std::cout << "Scaling fibers =\t";
	if (getenv("scale_fibers")){
		std::cout << "yes\n";
	} else {
		std::cout << "no\n";
	}
	std::cout << std::flush;

	ScanParams scanparams;
	scanparams.nimages(size_t(atoi(getenv("nimages"))));
	scanparams.filebase(std::string(getenv("filebase")));
	scanparams.calfilebase(std::string(getenv("calfilebase")));

	std::vector<float> imagetimes(scanparams.nimages(),0); // for benchmarking the processors

	scanparams.dalpha((atof(getenv("drifting_alpha")))*pi<double>()/scanparams.nimages());

	if (scanparams.doublepulse(atoi(getenv("doublepulse")) > 0 )){
		scanparams.doublepulsedelay(atof( getenv("doublepulsedelay") ) ) ; // this one gets used directly in atomic units
	}
	scanparams.lambda_0(atof( getenv("lambda0") ));
	scanparams.lambda_width( atof( getenv("lambda_width") ));
	scanparams.lambda_onoff( atof( getenv("lambda_onoff") ));
	scanparams.tspan((atof( getenv("tspan") ) )/fsPau<double>());

	scanparams.ngroupsteps(atoi( getenv("ngroupsteps") ));
	scanparams.groupdelay(atof(getenv("groupdelay")));
	scanparams.backdelay(atof(getenv("backdelay")));
	scanparams.netalon(atoi(getenv("netalon")));

	scanparams.etalonreflectance(atof(getenv("etalon")));
	scanparams.etalondelay(atof(getenv("etalondelay")));
	scanparams.interferedelay((double)atof(getenv("interferedelay")));

	scanparams.chirp(
			( atof( getenv("chirp") ) ) / std::pow(fsPau<float>(),int(2)), // the difference in slopes at omega_low versus omega_high must equal tspan
			( atof( getenv("TOD") ) ) / std::pow(fsPau<float>(),int(3)),
			( atof( getenv("FOD") ) ) / std::pow(fsPau<float>(),int(4)),
			( atof( getenv("fifthOD") ) ) / std::pow(fsPau<float>(),int(5))
			);

	if (scanparams.addchirpnoise(atoi(getenv("usechirpnoise"))>0)){
		scanparams.initchirpnoise(
				( atof( getenv("chirpnoise") ) ) / std::pow(fsPau<float>(),int(2)),
				( atof( getenv("TODnoise") ) ) / std::pow(fsPau<float>(),int(3)),
				( atof( getenv("FODnoise") ) ) / std::pow(fsPau<float>(),int(4)),
				( atof( getenv("fifthODnoise") ) ) / std::pow(fsPau<float>(),int(5))
				);
	}

	FiberBundle masterbundle(boost::lexical_cast<size_t>(atoi(getenv("nfibers"))));
	masterbundle.fiberdiameter(boost::lexical_cast<float>(atof(getenv("fiberdiam"))));
	masterbundle.laserdiameter(boost::lexical_cast<float>(atof(getenv("laserdiam"))));
	masterbundle.xraydiameter(boost::lexical_cast<float>(atof(getenv("xraydiam"))));
	masterbundle.set_fsPmm(boost::lexical_cast<float>(atof(getenv("bundle_fsPmm"))));
	masterbundle.scalePolarCoords();

	std::cout << "\t\tshuffle fibers?\t";
	if (getenv("shuffle_fibers"))
	{
		std::cout << "yes\n";masterbundle.shuffle_output();
	} else {
		std::cout << "no\n";
	}

	masterbundle.Ixray(float(1.));
	masterbundle.Ilaser(float(1.));
	std::string filename = scanparams.filebase() + "fibermap.out";
	std::cout << "fibermap file = " << filename << std::endl << std::flush;
	std::ofstream mapfile(filename.c_str(),std::ios::out);
	//masterbundle.print_mapping(mapfile,double(0.0));
	mapfile.close();

	// file for delay bins
	ofstream outbins(std::string(scanparams.filebase() + "delaybins.out").c_str(),ios::out);
	for (size_t f=0;f<masterbundle.get_nfibers();++f){
		outbins << masterbundle.delay(f) << "\n";
	}
	outbins.close();

	MatResponse masterresponse(
			0,															// stepdelay
			(double)( atof( getenv("stepwidth") ) ),								// stepwidth
			((double)( atof( getenv("attenuation") ) ) - 1.0) * masterbundle.Ixray() / scanparams.ngroupsteps() + 1.0,	// attenuation
			(double)( atof( getenv("phase") ) ) * masterbundle.Ixray() / scanparams.ngroupsteps()				// phase
			);
	masterresponse.aalphabbeta(
			(double)( atof( getenv("a") ) ),		// a
			(double)( atof( getenv("alpha" ) ) ),		// alpha
			(double)( atof( getenv("b") ) ),		// b
			(double)( atof( getenv("beta") ) )		// beta
			);

	masterresponse.setreflectance(scanparams.etalonreflectance());
	masterresponse.setetalondelay(scanparams.etalondelay());

	std::cout << "initializing masterpulse and masterplans" << std::endl << std::flush;
	PulseFreq masterpulse(scanparams.omega0(),scanparams.omega_width(),scanparams.omega_onoff(),scanparams.tspan());
	fftw_plan forward;
	fftw_plan backward;
	fftw_plan plan_r2hc;
	fftw_plan plan_hc2r;
	fftw_plan plan_r2hc_2x;
	fftw_plan plan_hc2r_2x;
	masterpulse.setmasterplans(&forward,&backward);
	masterpulse.setancillaryplans(& plan_r2hc,& plan_hc2r,& plan_r2hc_2x,& plan_hc2r_2x);
	masterpulse.addchirp(scanparams.getchirp());							// chirp that ref pulse

	double xrayphoton_energy = double(atof(getenv("xrayphoton_energy")));
	masterresponse.bandgap(double(atof(getenv("bandgap_eV")))); //

	if (masterresponse.fill_carriersvec(masterpulse,xrayphoton_energy)){
		std::cout << "\t OK, masterresponse.fill_carriersvec(masterpulse,9.5); filled with xrayphoton_energy = " << xrayphoton_energy << "\n" << std::flush;
	} else {
		std::cout << "\t FAILED, masterresponse.fill_carriersvec(masterpulse,9.5); did not fill\n" << std::flush;
	}

	std::time_t tstop = std::time(nullptr);
	std::cout << "\tIt has taken " << (tstop-tstart) << " s so far for initializing masterpulse and building fftw plans\n" << std::flush;

	CalibMat calibration(boost::lexical_cast<size_t>(atoi(getenv("ncalibdelays")))
			, boost::lexical_cast<double>(atof(getenv("fsWindow"))));
	if (!getenv("skipcalibration"))
	{
		std::cout << "\t\t############ entering calibration ###########\n" << std::flush;
		calibration.set_center(boost::lexical_cast<double>(atof(getenv("delays_mean"))));
		std::cout << "\t\t====== delays =======\n";
		for (size_t i = 0 ; i< calibration.get_ndelays(); ++i){
			std::cout << calibration.get_delay(i) << " ";
		}
		std::cout << std::endl << std::flush;
	}

	if (!getenv("skipcalibration"))
	{
		// Setup the shared pulse arrays
		std::vector< PulseFreq > calpulsearray(calibration.get_ndelays(),masterpulse);

#pragma omp parallel num_threads(nthreads) default(shared) shared(masterpulse)
		{ // begin parallel region 1
			size_t tid = omp_get_thread_num();

			// all non-shared objects must be created inside the parallel section for default is shared if defined outside
			// http://pages.tacc.utexas.edu/~eijkhout/pcse/html/omp-data.html
			PulseFreq etalonpulse(masterpulse);
			PulseFreq crossetalonpulse(masterpulse);
			PulseFreq calpulse(masterpulse);
			PulseFreq calcrosspulse(masterpulse);

			// initialize with masterpulse/masterresponse
			MatResponse calibresponse(masterresponse);

#pragma omp for schedule(dynamic)
			for (size_t d=0;d<calpulsearray.size();++d)
			{ // outermost loop for calibration.get_ndelays() to produce //
				//std::cout << "\tinside parallel region for actual loop d = "
				//<< d << "\twith tid = " << tid << "\n" << std::flush;
				std::cout << '+' << std::flush;
				//before each delay, reset to masterpulse
				//std::cerr << "\tmasterpulse.domain() = " << masterpulse.domain() << "\n" << std::flush;
				calpulse = masterpulse;
				calcrosspulse = masterpulse;
				//std::cerr << "\tcalpulse.domain() = " << calpulse.domain() << "\n" << std::flush;
				//std::cerr << "\tcalcrosspulse.domain() = " << calcrosspulse.domain() << "\n" << std::flush;
				//std::cerr << "\n\n\t\t###### Made it here HERE HERE HERE debugging seg fault ######\n\n" << std::flush;

				double startdelay(calibration.get_delay(d));

				calcrosspulse.delay(scanparams.interferedelay()); // delay in the frequency domain

				calpulse.fft_totime();
				calcrosspulse.fft_totime();

				//std::cerr << "\tafter fft_totime() calpulse.domain() = " << calpulse.domain() << "\n" << std::flush;
				//std::cerr << "\tafter fft_totime() calcrosspulse.domain() = " << calcrosspulse.domain() << "\n" << std::flush;

				for(size_t g=0;g<scanparams.ngroupsteps();g++){ // begin groupsteps loop
					calibresponse.setdelay(startdelay - g*scanparams.groupstep()); // forward propagating, x-rays advance on the optical
					calibresponse.setstepvec_amp(calpulse);
					calibresponse.setstepvec_phase(calpulse);
					calibresponse.setstepvec_amp(calcrosspulse);
					calibresponse.setstepvec_phase(calcrosspulse);
					if (scanparams.doublepulse()){
						calibresponse.addstepvec_amp(calpulse,scanparams.doublepulsedelay());
						calibresponse.addstepvec_phase(calpulse,scanparams.doublepulsedelay());
						calibresponse.addstepvec_amp(calcrosspulse,scanparams.doublepulsedelay());
						calibresponse.addstepvec_phase(calcrosspulse,scanparams.doublepulsedelay());
					}
					// this pulls down the tail of the response so vector is periodic on nsamples
					calibresponse.buffervectors(calpulse);
					calibresponse.buffervectors(calcrosspulse);
					calpulse.modulateamp_time();
					calpulse.modulatephase_time();
					calcrosspulse.modulateamp_time();
					calcrosspulse.modulatephase_time();
				}// end groupsteps loop

				for (size_t e=0;e<scanparams.netalon();e++){ // begin etalon loop
					// back propagation step //
					double etalondelay = startdelay - double(e+1) * (calibresponse.getetalondelay());
					// at front surface, x-rays see counter-propagating light from one full etalon delay

					// reset back to calpulse for each round
					etalonpulse = calpulse;
					crossetalonpulse = calcrosspulse;
					//std::cerr << "\t\t\t -- inside etalon: etalonpulse.domain() = " << etalonpulse.domain() << "\n" << std::flush;
					//std::cerr << "\t\t\t -- inside etalon: crossetalonpulse.domain() = " << crossetalonpulse.domain() << "\n" << std::flush;

					for(size_t g=0;g<scanparams.ngroupsteps();g++){
						calibresponse.setdelay(etalondelay + g*scanparams.backstep());
						// counterpropagating, x-rays work backwards through the optical

						calibresponse.setstepvec_amp(etalonpulse);
						calibresponse.setstepvec_phase(etalonpulse);
						calibresponse.setstepvec_amp(crossetalonpulse);
						calibresponse.setstepvec_phase(crossetalonpulse);
						if (scanparams.doublepulse()){
							calibresponse.addstepvec_amp(etalonpulse,scanparams.doublepulsedelay());
							calibresponse.addstepvec_phase(etalonpulse,scanparams.doublepulsedelay());
							calibresponse.addstepvec_amp(crossetalonpulse,scanparams.doublepulsedelay());
							calibresponse.addstepvec_phase(crossetalonpulse,scanparams.doublepulsedelay());
						}
						calibresponse.buffervectors(etalonpulse); // this pulls down the tail of the response so vector is periodic on nsamples
						calibresponse.buffervectors(crossetalonpulse); // this pulls down the tail of the response so vector is periodic on nsamples
						//std::cerr << "\t\t\t -- inside etalon groupsteps: before: etalonpulse.domain() = " << etalonpulse.domain() << "\n" << std::flush;
						//std::cerr << "\t\t\t -- inside etalon groupsteps: before: crossetalonpulse.domain() = " << crossetalonpulse.domain() << "\n" << std::flush;
						etalonpulse.modulateamp_time();
						etalonpulse.modulatephase_time();
						crossetalonpulse.modulateamp_time();
						crossetalonpulse.modulatephase_time();
						//std::cerr << "\t\t\t -- inside etalon groupsteps: after: etalonpulse.domain() = " << etalonpulse.domain() << "\n" << std::flush;
						//std::cerr << "\t\t\t -- inside etalon groupsteps: after: crossetalonpulse.domain() = " << crossetalonpulse.domain() << "\n" << std::flush;
					}
					// forward propagation //
					for(size_t g=0;g<scanparams.ngroupsteps();g++){
						calibresponse.setdelay(startdelay - g*scanparams.groupstep()); // forward propagating, x-rays advance on the optical
						calibresponse.setstepvec_amp(etalonpulse);
						calibresponse.setstepvec_phase(etalonpulse);
						calibresponse.setstepvec_amp(crossetalonpulse);
						calibresponse.setstepvec_phase(crossetalonpulse);
						if (scanparams.doublepulse()){
							calibresponse.addstepvec_amp(etalonpulse,scanparams.doublepulsedelay());
							calibresponse.addstepvec_phase(etalonpulse,scanparams.doublepulsedelay());
							calibresponse.addstepvec_amp(crossetalonpulse,scanparams.doublepulsedelay());
							calibresponse.addstepvec_phase(crossetalonpulse,scanparams.doublepulsedelay());

						}
						calibresponse.buffervectors(etalonpulse); // this pulls down the tail of the response so vector is periodic on nsamples
						calibresponse.buffervectors(crossetalonpulse); // this pulls down the tail of the response so vector is periodic on nsamples
						//std::cerr << "\t\t\t -- inside etalon groupsteps forward prop: before: etalonpulse.domain() = " << etalonpulse.domain() << "\n" << std::flush;
						//std::cerr << "\t\t\t -- inside etalon groupsteps forward prop: before: crossetalonpulse.domain() = " << crossetalonpulse.domain() << "\n" << std::flush;
						etalonpulse.modulateamp_time();
						etalonpulse.modulatephase_time();
						crossetalonpulse.modulateamp_time();
						crossetalonpulse.modulatephase_time();
						//std::cerr << "\t\t\t -- inside etalon groupsteps forward prop: after: etalonpulse.domain() = " << etalonpulse.domain() << "\n" << std::flush;
						//std::cerr << "\t\t\t -- inside etalon groupsteps forward prop: after: crossetalonpulse.domain() = " << crossetalonpulse.domain() << "\n" << std::flush;
					}
					//std::cerr << "\t\t\t -- inside etalon finished groupsteps: etalonpulse.domain() = " << etalonpulse.domain() << "\ttid = "<< tid << "\n" << std::flush;
					//std::cerr << "\t\t\t -- inside etalon finished groupsteps: crossetalonpulse.domain() = " << crossetalonpulse.domain() << "\n" << std::flush;
					etalonpulse.fft_tofreq();
					crossetalonpulse.fft_tofreq();
					etalonpulse.delay(calibresponse.getetalondelay()); // delay and attenuate in frequency domain
					etalonpulse.attenuate(pow(calibresponse.getreflectance(),(int)2));
					crossetalonpulse.delay(calibresponse.getetalondelay()); // delay and attenuate in frequency domain
					crossetalonpulse.attenuate(pow(calibresponse.getreflectance(),(int)2));
					etalonpulse.fft_totime();
					crossetalonpulse.fft_totime();
					//std::cerr << "\t\t\t -- end etalon: etalonpulse.domain() = " << etalonpulse.domain() << "\n" << std::flush;
					//std::cerr << "\t\t\t -- end etalon: crossetalonpulse.domain() = " << crossetalonpulse.domain() << "\n" << std::flush;
					//std::cerr << "\t\t\t -- end etalon: calpulse.domain() = " << calpulse.domain() << "\n" << std::flush;
					//std::cerr << "\t\t\t -- end etalon: calcrosspulse.domain() = " << calcrosspulse.domain() << "\n" << std::flush;
					calpulse += etalonpulse;
					calcrosspulse += crossetalonpulse;
				} // end etalon loop

				//std::cerr << "\t\tbefore fft_tofreq(): calpulse.domain() = " << calpulse.domain() << "\n" << std::flush;
				//std::cerr << "\t\tbefore fft_tofreq(): calcrosspulse.domain() = " << calcrosspulse.domain() << "\n" << std::flush;
				calpulse.fft_tofreq();
				calcrosspulse.fft_tofreq();
				calpulse.delay(scanparams.interferedelay()); // expects this in fs // time this back up to the crosspulse

				calpulse -= calcrosspulse;
				// reversing order for sake of chirp calib matrix
				calpulsearray[calpulsearray.size()-d-1] = calpulse;

				/*
				   std::cerr << "\t\tcalpulse.domain() = " << calpulse.domain() << "\n" << std::flush;
				   std::cerr << "\t\tcalcrosspulse.domain() = " << calcrosspulse.domain() << "\n" << std::flush;
				   std::cerr << "\t\tcalpulsearray[ " << (calpulsearray.size()-d-1) << " ].domain() = "
				   << calpulsearray[calpulsearray.size()-d-1].domain() << "\n" << std::flush;
				   */
			} // end of loop calibration.get_ndelays() to produce //

#pragma omp barrier

#pragma omp master
			{
				std::cout << "|\t done with calibration delays\n" << std::flush;
				//std::cerr << "\t\t###### Made it here too #####\n\t\t##### should call only once in master #######\n" << std::flush;
				// print out the calibration as ascii for now //
				// print rows in order, eventually in tf_record or matrix or so. //
				std::string calfilename = scanparams.calfilebase() + "interference.calibration";
				std::string calfilename_delays = scanparams.calfilebase() + "interference.calibration.delays";
				std::string calfilename_wavelengths = scanparams.calfilebase() + "interference.calibration.wavelengths";
				ofstream calibrationstream(calfilename.c_str(),ios::out);
				ofstream calibrationstream_delays(calfilename_delays.c_str(),ios::out);
				ofstream calibrationstream_wavelengths(calfilename_wavelengths.c_str(),ios::out);
				/*
				std::string bin_calfilename = scanparams.filebase() + "interference.calibration.bin";
				ofstream bin_calibrationstream(bin_calfilename.c_str(),ios::out | ios::binary);
				std::cout << "\tcalibration filename out = " << calfilename << "\n\t and \t" << bin_calfilename << std::endl;
				*/
				calibrationstream << "# wavelengths\n#";
				calpulsearray[0].printwavelengthbins(&calibrationstream);
				calpulsearray[0].printwavelengthbins(&calibrationstream_wavelengths);
				calibrationstream << "# delays\n#";
				calibrationstream_delays << "# delays\n";
				for (size_t i = 0 ; i< calibration.get_ndelays(); ++i){
					calibrationstream << calibration.get_delay(i) << "\t";
					calibrationstream_delays << calibration.get_delay(i) << "\t";
				}
				calibrationstream << "\n";
				calibrationstream_delays << "\n";

				for (size_t n=0;n<calpulsearray.size();++n){
					calpulsearray[n].appendwavelength(&calibrationstream);
					//	calpulsearray[n].appendwavelength_bin(&bin_calibrationstream);
				}

				calibrationstream.close();
				calibrationstream_delays.close();
				calibrationstream_wavelengths.close();
				//bin_calibrationstream.close();
				std::cout << "Finished with the calibration image/matrix\n" << std::flush;

			}

#pragma omp master
			{
				std::cout << "\t\t############ ending parallel region 1 ###########\n" << std::flush;
			}
		} // end parallel region 1

	} // end if (!getenv("skipcalibration"))

	//############## Images section ##############

#pragma omp parallel num_threads(nthreads) default(shared) shared(masterpulse)
		{
	if (!getenv("skipimages"))
	{
		std::cout << "\t\t############ entering parallel/images ###########\n" << std::flush;
			size_t tid = omp_get_thread_num();
			size_t nfibers = masterbundle.get_nfibers();

			FiberBundle parabundle(masterbundle);
			MatResponse pararesponse(masterresponse);

			PulseFreq pulse(masterpulse);
			PulseFreq crosspulse(masterpulse);
			PulseFreq etalonpulse(masterpulse);
			PulseFreq crossetalonpulse(masterpulse);
			std::vector< PulseFreq > pulsearray(nfibers,PulseFreq(masterpulse));
#pragma omp barrier

			if (scanparams.addrandomphase(atoi(getenv("addrandomphase"))>0))
			{
				masterpulse.addrandomphase();
				std::string filename = scanparams.filebase() + "spectralphaseFTpower.dat";
				std::ofstream outfile(filename.c_str(),std::ios::out);
				masterpulse.print_phase_powerspectrum(outfile);
				outfile.close();
				filename = scanparams.filebase() + "spectralphase.dat";
				outfile.open(filename.c_str(),std::ios::out);
				masterpulse.print_phase(outfile);
				outfile.close();
				filename = scanparams.filebase() + "spectralamp.dat";
				outfile.open(filename.c_str(),std::ios::out);
				masterpulse.print_amp(outfile);
				outfile.close();
			}

#pragma omp for schedule(dynamic)
			for (size_t n=0;n<scanparams.nimages();++n)
			{ // outermost loop for nimages to produce //
				//std::cerr << "\tinside the parallel region 2 for images loop n = " << n << " in thread " << tid << "\n" << std::flush;
				if (n==0 & tid==0) {
					std::cout << "========================================================================="
						<<   "\n\t\t ==== http://www.fftw.org/fftw3_doc/Advanced-Complex-DFTs.html ===="
						<<   "\n\t\t ====         use this for defining multiple fibers as         ===="
						<<   "\n\t\t ====         contiguous blocks for row-wise FFT as 2D         ===="
						<<   "\n\t\t ==================================================================\n" << std::flush;
				}

				std::time_t imgstart = std::time(nullptr);

				double t0 = scanparams.delays_uniform();
				double startdelay(0);

				parabundle = masterbundle;

				parabundle.Ixray(scanparams.xray_inten_rand());
				parabundle.Ilaser(scanparams.laser_inten_rand());
				parabundle.delay_angle(scanparams.dalpha()*double(n));
				parabundle.center_Ixray(scanparams.xray_pos_rand(),scanparams.xray_pos_rand());
				parabundle.center_Ilaser(scanparams.laser_pos_rand(),scanparams.laser_pos_rand());

				//DebugOps::pushout(std::string("Running image " + std::to_string(n) + " for t0 = " + std::to_string(t0) + " in threaded for loop, thread " + std::to_string(tid)));
				std::string mapfilename = scanparams.filebase() + "fibermap.out." + std::to_string(n);
				//std::cout << "fibermap file = " << mapfilename << std::endl << std::flush;
				std::ofstream mapfile(mapfilename.c_str(),std::ios::out);
				parabundle.print_mapping(mapfile,t0);
				mapfile.close();

				for(size_t f = 0; f < parabundle.get_nfibers(); f++)
				{ // begin fibers loop
					pulse = masterpulse;
					crosspulse = masterpulse;
					startdelay = t0 + parabundle.delay(f);
					pulse.scale(parabundle.Ilaser(f));
					crosspulse.scale(parabundle.Ilaser(f));

					pararesponse = masterresponse;

					if (getenv("scale_fibers")){
						pararesponse.setscale(parabundle.Ixray(f));
						//std::cerr << "parabundle.Ixray(" << f << ") = " << parabundle.Ixray(f) << "\n" << std::flush;
					}

					if (scanparams.addchirpnoise()){
						std::vector<double> noise(scanparams.getchirpnoise());
						pulse.addchirp(noise);
						crosspulse.addchirp(noise);
					}

					crosspulse.delay(scanparams.interferedelay()); // delay in the frequency domain
					pulse.fft_totime();
					crosspulse.fft_totime();

					for(size_t g=0;g<scanparams.ngroupsteps();g++){ // begin groupsteps loop
						pararesponse.setdelay(startdelay - g*scanparams.groupstep()); // forward propagating, x-rays advance on the optical
						pararesponse.setstepvec_amp(pulse);
						pararesponse.setstepvec_phase(pulse);
						pararesponse.setstepvec_amp(crosspulse);
						pararesponse.setstepvec_phase(crosspulse);
						if (scanparams.doublepulse()){
							pararesponse.addstepvec_amp(pulse,scanparams.doublepulsedelay());
							pararesponse.addstepvec_phase(pulse,scanparams.doublepulsedelay());
							pararesponse.addstepvec_amp(crosspulse,scanparams.doublepulsedelay());
							pararesponse.addstepvec_phase(crosspulse,scanparams.doublepulsedelay());
						}
						// this pulls down the tail of the response so vector is periodic on nsamples
						pararesponse.buffervectors(pulse);
						pararesponse.buffervectors(crosspulse);
						pulse.modulateamp_time();
						pulse.modulatephase_time();
						crosspulse.modulateamp_time();
						crosspulse.modulatephase_time();
					}// end groupsteps loop
					//std::cerr << "tid = " << tid << "\tpulse/crosspulse.domain() = " << pulse.domain() << "/" << crosspulse.domain() << "\n" << std::flush;

					for (size_t e=0;e<scanparams.netalon();e++){ // begin etalon loop
						//std::cerr << "tid = " << tid << "\tpulse/crosspulse.domain() = " << pulse.domain() << "/" << crosspulse.domain() << "\n" << std::flush;
						//std::cerr << "\n\t\t ---- starting etalon at " << e << " ----\n" << std::flush;
						// back propagation step //
						double etalondelay = startdelay - double(e+1) * (pararesponse.getetalondelay());
						// at front surface, x-rays see counter-propagating light from one full etalon delay

						etalonpulse = pulse;
						crossetalonpulse = crosspulse;
						//std::cerr << "etalonpulse/crossetalonpulse.domain() = " << etalonpulse.domain() << "/" << crossetalonpulse.domain() << "\n" << std::flush;

						for(size_t g=0;g<scanparams.ngroupsteps();g++){
							pararesponse.setdelay(etalondelay + g*scanparams.backstep());
							// counterpropagating, x-rays work backwards through the optical

							pararesponse.setstepvec_amp(etalonpulse);
							pararesponse.setstepvec_phase(etalonpulse);
							pararesponse.setstepvec_amp(crossetalonpulse);
							pararesponse.setstepvec_phase(crossetalonpulse);
							if (scanparams.doublepulse()){
								pararesponse.addstepvec_amp(etalonpulse,scanparams.doublepulsedelay());
								pararesponse.addstepvec_phase(etalonpulse,scanparams.doublepulsedelay());
								pararesponse.addstepvec_amp(crossetalonpulse,scanparams.doublepulsedelay());
								pararesponse.addstepvec_phase(crossetalonpulse,scanparams.doublepulsedelay());
							}
							pararesponse.buffervectors(etalonpulse); // this pulls down the tail of the response so vector is periodic on nsamples
							pararesponse.buffervectors(crossetalonpulse); // this pulls down the tail of the response so vector is periodic on nsamples
							etalonpulse.modulateamp_time();
							etalonpulse.modulatephase_time();
							crossetalonpulse.modulateamp_time();
							crossetalonpulse.modulatephase_time();
						}
						// forward propagation //
						//std::cerr << "\t\t\t ########### // forward propagation // #############\n" << std::flush;
						for(size_t g=0;g<scanparams.ngroupsteps();g++){
							pararesponse.setdelay(startdelay - g*scanparams.groupstep()); // forward propagating, x-rays advance on the optical
							pararesponse.setstepvec_amp(etalonpulse);
							pararesponse.setstepvec_phase(etalonpulse);
							pararesponse.setstepvec_amp(crossetalonpulse);
							pararesponse.setstepvec_phase(crossetalonpulse);
							if (scanparams.doublepulse()){
								pararesponse.addstepvec_amp(etalonpulse,scanparams.doublepulsedelay());
								pararesponse.addstepvec_phase(etalonpulse,scanparams.doublepulsedelay());
								pararesponse.addstepvec_amp(crossetalonpulse,scanparams.doublepulsedelay());
								pararesponse.addstepvec_phase(crossetalonpulse,scanparams.doublepulsedelay());

							}
							pararesponse.buffervectors(etalonpulse); // this pulls down the tail of the response so vector is periodic on nsamples
							pararesponse.buffervectors(crossetalonpulse); // this pulls down the tail of the response so vector is periodic on nsamples
							etalonpulse.modulateamp_time();
							etalonpulse.modulatephase_time();
							crossetalonpulse.modulateamp_time();
							crossetalonpulse.modulatephase_time();
						}
						//std::cerr << "etalonpulse/crossetalonpulse.domain() = " << etalonpulse.domain() << "/" << crossetalonpulse.domain() << "\n" << std::flush;
						etalonpulse.fft_tofreq();
						crossetalonpulse.fft_tofreq();
						etalonpulse.delay(pararesponse.getetalondelay()); // delay and attenuate in frequency domain
						etalonpulse.attenuate(pow(pararesponse.getreflectance(),(int)2));
						crossetalonpulse.delay(pararesponse.getetalondelay()); // delay and attenuate in frequency domain
						crossetalonpulse.attenuate(pow(pararesponse.getreflectance(),(int)2));
						etalonpulse.fft_totime();
						crossetalonpulse.fft_totime();
						pulse += etalonpulse;
						crosspulse += crossetalonpulse;
					} // end etalon loop

					pulse.fft_tofreq();
					crosspulse.fft_tofreq();
					pulse.delay(scanparams.interferedelay()); // expects this in fs // time this back up to the crosspulse
					pulse -= crosspulse;
					// std::cerr << "\n\n\t\t\t\t============== testing... just before the push_back() ==============\n\n" << std::flush;
					pulsearray[f] = pulse;
				} // end nfibers loop

				std::string filename = scanparams.filebase() + "interference.out." + std::to_string(n);// + ".tid" + std::to_string(tid);
				//std::cerr << "testing: filename = " << filename << "\n" << std::flush;
				ofstream interferestream(filename.c_str(),ios::out); // use app to append delays to same file.

				//std::cout << "tid = " << tid << ": interfere filename out = " << filename << std::endl;
				std::complex<double> z_laser = parabundle.center_Ilaser();
				std::complex<double> z_xray = parabundle.center_Ixray();
				interferestream << "#delay for image = \t" << t0
					<< "\n#Ilaser = \t" << parabundle.Ilaser()
					<< "\n#Ixray = \t" << parabundle.Ixray()
					<< "\n#center laser = \t" << z_laser.real() << "\t" << z_laser.imag()
					<< "\n#center xray = \t" << z_xray.real() << "\t" << z_xray.imag()
					<< "\n#alpha = \t" << parabundle.delay_angle()
					<< std::endl;
				interferestream << "#";
				pulsearray[0].printwavelengthbins(&interferestream);
				for (size_t f=0;f<pulsearray.size();f++){
					pulsearray[f].scale(parabundle.Ilaser(f));
					pulsearray[f].appendwavelength(&interferestream);
				}
				if (tid % 10 < 2){
					for (size_t f=0;f<parabundle.get_nfibers();f++){
						int max = boost::lexical_cast<double>(getenv("gain")) * pulsearray[f].maxsignal();
						for (size_t i=0;i<std::log(max);++i){
							std::cout << '.';
						}
						std::cout << "|";
					}
					std::cout << "\timg = " << n << " in tid = " << tid << "\n" << std::flush;
				}
				interferestream.close();

				std::time_t imgstop = std::time(nullptr);
				imagetimes[n] = float(imgstop - imgstart);

			} // outermost loop for nimages to produce //

#pragma omp master
			{
				std::cout << "\t\t############ ending parallel region 2 ###########\n" << std::flush;
			}

#pragma omp barrier

			//std::cerr << "\n\t... trying to leave parallel region 2" << std::endl;
	} // end if (!getenv("skipimages")
		} // end parallel region

	//std::cout << "\n ---- just left parallel region ----" << std::endl;
	std::cout << "masterresponse reflectance: " << masterresponse.getreflectance() << std::endl;
	std::cout << "masterbundle fiberdiameter: " << masterbundle.fiberdiameter() << std::endl;
	std::cout << "scanparams lambda_0: " << scanparams.lambda_0() << std::endl;
	fftw_destroy_plan(forward);
	fftw_destroy_plan(backward);

	tstop = std::time(nullptr);
	tstop -= tstart;
	std::cout << "\t\t======================================\n"
		<< "\t\t======== scan_material stopped =======\n"
		<< "\t\t===== " << std::asctime(std::localtime(&tstop))
		<< "\t\t===== in " << tstop << " s \n"
		<< "\t\t======================================\n" << std::flush;
	std::string timesfilename = scanparams.filebase() + "runtimes.log";
	std::ofstream timesout(timesfilename.c_str(),std::ios::app);
	timesout << "#########################################\n" << std::flush;
	timesout << "# HOSTNAME:\t" << getenv("HOSTNAME") << "\n" << std::flush;
	timesout << "# total time (seconds):\t" << tstop << "\n" << std::flush;
	timesout << "# nfibers :\t" << masterbundle.get_nfibers() << "\n" << std::flush;
	timesout << "# nthreads :\t" << nthreads << "\n" << std::flush;
	timesout << "# mean time / image:\t" << DataOps::mean(imagetimes)
		<< "\t/ fiber\t" << DataOps::mean(imagetimes)/float(masterbundle.get_nfibers()) << "\n" << std::flush;
	for (size_t i=0 ;i< imagetimes.size();++i){
		timesout << imagetimes[i] << "\t";
	}
	timesout << "\n" << std::flush;
	timesout.close();

	return 0;
}


/**
 * \ file AllPassReverbFilter.cpp
 */

#include <ATK/Reverberation/AllPassReverbFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

#define PROCESSSIZE (1024*64)

namespace
{
  void compare(float v1, float v2)
  {
    if(std::abs(v1) < 1e-5 || std::abs(v2) < 1e-5)
    {
      BOOST_REQUIRE_SMALL(v1, static_cast<float>(1e-5));
      BOOST_REQUIRE_SMALL(v2, static_cast<float>(1e-5));
    }
    else
    {
      BOOST_REQUIRE_CLOSE(v1, v2, 0.0001);
    }
  }
}

BOOST_AUTO_TEST_CASE( AllPassReverbFilter_sinus_line100_delay50_test )
{
  std::vector<float> data(PROCESSSIZE);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }
  std::vector<float> ref(PROCESSSIZE);
  for(int64_t i = 50; i < PROCESSSIZE; ++i)
  {
    ref[i] = data[i - 50];
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<float> outdata(PROCESSSIZE);

  ATK::AllPassReverbFilter<float> filter(100);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(50);

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    compare(outdata[i], ref[i]);
  }
}

BOOST_AUTO_TEST_CASE( AllPassReverbFilter_sinus_line25_delay24_feedback_0_5_test )
{
  std::vector<float> data(PROCESSSIZE);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }
  std::vector<float> ref(PROCESSSIZE);
  for(int64_t i = 0; i < 24; ++i)
  {
    ref[i] = -0.5*data[i];
  }
  for(int64_t i = 24; i < PROCESSSIZE; ++i)
  {
    ref[i] = 0.5 * (ref[i - 24] - data[i]) + data[i - 24];
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<float> outdata(PROCESSSIZE);

  ATK::AllPassReverbFilter<float> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(24);
  filter.set_feedback(0.5);

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    compare(outdata[i], ref[i]);
  }
}

BOOST_AUTO_TEST_CASE( AllPassReverbFilter_sinus_line25_delay24_feedback_1_test )
{
  std::vector<float> data(PROCESSSIZE);
  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = std::sin(2 * boost::math::constants::pi<float>() * (i+1.)/48000 * 1000);
  }
  std::vector<float> ref(PROCESSSIZE);
  for(int64_t i = 0; i < 24; ++i)
  {
    ref[i] = -data[i];
  }
  for(int64_t i = 24; i < PROCESSSIZE; ++i)
  {
    ref[i] = (ref[i - 24] - data[i]) + data[i - 24];
  }

  ATK::InPointerFilter<float> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::vector<float> outdata(PROCESSSIZE);

  ATK::AllPassReverbFilter<float> filter(25);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_delay(24);
  filter.set_feedback(1);

  ATK::OutPointerFilter<float> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(49);
  output.process(1);
  output.process(51);
  output.process(PROCESSSIZE - 1 - 49 -51);

  for(int64_t i = 0; i < PROCESSSIZE; ++i)
  {
    compare(outdata[i], ref[i]);
  }
}


#ifndef LASER_OBJECTS_H
#define LASER_OBJECTS_H
#include <armadillo>
/*! \file laser_objects.hpp
*/

class laser_object_t
{
bool is_mobile;
std::pair<float, float> pos_centerofmass;
std::pair<float, float> vel_centerofmass;
float immobile_time;
ros::Time last_active;
arma::fmat X, P, K;
bool test_mobile();
public:
	laser_object_t(std::pair<float, float> pos_com,
	               std::pair<float, float> vel_com);
	float distance(std::pair<float,float> coords_to);
	ros::Time get_last_active();
	std::pair<float, float> get_pos_centerofmass();
	std::pair<float, float> get_vel_centerofmass();
	bool get_is_mobile();
	~laser_object_t();
	void run_kalman_filter(std::pair<float,float> new_pos, float dt);
};

class laser_objects
{
private:
	float dt;
	std::vector<laser_object_t> laser_object_repository;
public:
	void update(std::pair<float, float> pos_centerofmass);
	std::vector<float> get_mobiles_x();
	std::vector<float> get_mobiles_y();
	std::vector<float> get_statics_x();
	std::vector<float> get_statics_y();
	void update_repository();
	laser_objects(float scanner_dt);
	~laser_objects();
// std::vector<float> get_mobiles_dir()
// {
//
// }
//
// std::vector<float> get_mobiles_spd()
// {
//
// }
};
#endif /* end of include guard: LASER_OBJECTS_H */


#include "HashTableHandler.h"
#include <stdio.h>
#include <string>
#include <map>
#include <fstream>
#include "llvm/Support/raw_ostream.h"
#include <cstdio>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <array>
#include <regex>
#include "helper.h"
#include "klee/ExecutionState.h"
#include "klee/Internal/ADT/RNG.h"
#include <boost/math/distributions/binomial.hpp>
#include "Executor.h"
#include <cmath>
#include "klee/klee.h"

// TODO: use macros in klee.h instead of hard-coded return values.

using namespace klee;
using namespace std;
using namespace llvm;
using boost::math::binomial;

namespace klee {
  extern RNG theRNG;
}

HashTableHandler::HashTableHandler(Executor &_executor)
: executor(_executor)
{
   printf("new ht handler created!\n");
}

HashTableHandler::HashTableHandler(HashTableHandler &_handler)
: executor(_handler.executor)
{

}

HashTableHandler::~HashTableHandler()
{

}

HashTable::HashTable()
{
}

HashTable::HashTable(int _size):
   status(-1),
   size(_size),
   cur_size(0)
{
   // create an empty table
   LOG(LOG_MASK_HT, "creating an empty hash table, size=%d", _size);
}

HashTable::HashTable(const HashTable &_table):
   status(_table.status),
   size(_table.size),
   cur_size(_table.cur_size),
   dist(_table.dist)
{
}

HashTable::~HashTable()
{
}

int HashTableHandler::ht_init(ExecutionState &cur, size_t sz)
{
   LOG(LOG_MASK_HT, "allocating table size=%ld", sz);
   cur.table = new HashTable(sz);

   // TODO: add deletion of the hash table
   return 0;
}

int HashTableHandler::ht_get_max(ExecutionState &cur)
{
   HashTable *t = cur.table;
   int max = 0;
   for (auto x : t->dist) {
      if (x.first > max) {
         max = x.first;
      }
   }
   LOG(LOG_MASK_HT, "returning max val in ht = %d", max);
   return max;
}

int HashTableHandler::ht_access(ExecutionState &cur,
                                vector<ExecutionState*> &states,
                                vector<int> &rets)
{
   HashTable *t = cur.table;
   assert(t);

   LOG(LOG_MASK_HT, "accessing table, cur size=%d", t->cur_size);

   // processing the first access
   if (t->cur_size == 0) {
      LOG(LOG_MASK_HT, "table is empty, can only insert entry, pathprob=%f",
          cur.getPathProb());
      t->status = GREYBOX_MISS;
      states.push_back(&cur);
      rets.push_back(GREYBOX_MISS);
      return 0;
   }

   // TODO: cannot insert to empty if table is full
   assert(t->cur_size < t->size);

   // Obtain per-path probabilities
   double probEmpty = 1 - ((double) t->cur_size) / ((double) t->size);
   double probNonEmpty = 1 - probEmpty;
   double probCollision = probNonEmpty * (1.0 / (double) t->size);
   double probHit = probNonEmpty - probCollision;
   assert(probEmpty + probCollision + probHit > 0.99);

   LOG(LOG_MASK_HT, "table not empty, pathprob=%g, empty=%g,hit=%g,col=%g",
       cur.getPathProb(), probEmpty, probHit, probCollision);

   // if we are doing Monte Carlo sampling, just pick up one state
   if (cur.isSample == true) {
      assert(cur.isSampleDone == false);

      double r = theRNG.getDouble();

      LOG(LOG_MASK_HT, "r=%f", r);

      // pick up a branch according to per-path prob
      states.push_back(&cur);
      t = states[0]->table;
      if (r < probEmpty) {
         cur.updatePathProb(probEmpty);
         t->status = GREYBOX_MISS;
         rets.push_back(GREYBOX_MISS);
         LOG(LOG_MASK_HT, "access=empty, pathprob=%f", cur.getPathProb());
      } else if (probEmpty <= r && r < probEmpty + probCollision) {
         cur.updatePathProb(probCollision);
         t->status = GREYBOX_COL;
         rets.push_back(GREYBOX_COL);
         LOG(LOG_MASK_HT, "access=col, pathprob=%f", cur.getPathProb());
      } else {
         cur.updatePathProb(probHit);
         t->status = GREYBOX_HIT;
         rets.push_back(GREYBOX_HIT);
         LOG(LOG_MASK_HT, "access=hit, pathprob=%f", cur.getPathProb());
      }

      return 0;
   }

   // fork three states
   executor.branchTableAccessNoCond(cur, 3, states);
   rets.push_back(GREYBOX_MISS);
   rets.push_back(GREYBOX_HIT);
   rets.push_back(GREYBOX_COL);
   assert(states.size() == 3);
   assert(rets.size() == 3);

   // empty prob
   states[0]->updatePathProb(probEmpty);
   // hit prob
   states[1]->updatePathProb(probHit);
   // collision prob
   states[2]->updatePathProb(probCollision);

   // empty
   t = states[0]->table;
   t->status = GREYBOX_MISS;

   // hit
   t = states[1]->table;
   t->status = GREYBOX_HIT;

   // collision
   t = states[2]->table;
   t->status = GREYBOX_COL;

   return 0;
}

int HashTableHandler::ht_check(ExecutionState &cur, int &status)
{
   ERR("not implemented");
   assert(0);
   return 0;
}

// Based the previous access result, return a branch for each possible
// value in the table
int HashTableHandler::ht_read(ExecutionState &cur,
                              vector<ExecutionState*> &states,
                              vector<int> &rets)
{
   HashTable *t = cur.table;

   LOG(LOG_MASK_HT, "reading HT, status=%d, cur_size=%d", t->status,
       t->cur_size);

   // the access misses the table, have to return 0
   if (t->status == GREYBOX_MISS) {
      rets.push_back(HT_READ_FALSE);
      states.push_back(&cur);
      return 0;
   }

   // hit or collision
   assert(t->dist.size() > 0);
   executor.branchTableAccessNoCond(cur, t->dist.size(), states);

   int i = 0;
   for (auto x : t->dist) {
      rets.push_back(x.first);
      states[i]->updatePathProb(x.second);
      LOG(LOG_MASK_HT, "read value=%d, prob=%g, pathProb=%g", x.first,
          x.second, states[i]->getPathProb());
      i ++;
   }

   return 0;
}

// Write val to the table, based on previous access result
// This API doesn't fork states
int HashTableHandler::ht_write(ExecutionState &cur, int val)
{
   HashTable *t = cur.table;
   int num = t->cur_size;
   double p0 = t->dist[0];
   double p1 = t->dist[1];

   // empty table, and we just missed the table
   if (num == 0) {
      assert(t->status == GREYBOX_MISS);
      LOG(LOG_MASK_HT, "writing %d to an empty table", val);
      if (val == 0) {
         t->dist[0] = 1;
         t->dist[1] = 0;
      } else if (val == 1) {
         t->dist[0] = 0;
         t->dist[1] = 1;
      }
      t->cur_size += 1;
      return 0;
   }

   // Table not empty, but we just missed the table
   if (t->status == GREYBOX_MISS) {
      for (auto d: t->dist) {
         if (d.first == val) {
            d.second = (d.second * num + 1) / (num + 1);
         } else {
            d.second = (d.second * num) / (num + 1);
         }
      }
      t->cur_size += 1;
      return 0;
   }

      // write 1 to an empty entry
      /*
      if (val == 1) {
         t->dist[0] = (p0 * num) / (num + 1);
         t->dist[1] = 1 - t->dist[0];
      } else if (val == 0) {
         t->dist[0] = (p0 * num + 1) / (num + 1);
         t->dist[1] = 1 - t->dist[0];
      }
      */

   // Table not empty, and we just hit or collide on an existing entry
   // TODO: only support binary HT
   if (t->dist.size() > 2) {
      t->dump();
   }
   assert(t->dist.size() <= 2);
   if (val == 1) {
      t->dist[0] = p0 * ((p0 * num - 1) / num) + (p0 * p1);
      t->dist[1] = 1 - t->dist[0];
   } else if (val == 0) {
      t->dist[0] = p0 * p0 +
                   p1 * (p0 * num + 1) / num;
      t->dist[1] = 1 - t->dist[0];
   }

   return 0;
}

long long factorial(long number)
{
   long long ret;
   if (number <= 1)
		ret = 1;
	else
		ret = number * factorial(number - 1);

   assert(ret > 0);

   return ret;
}

long long combinator(int n,int m)
{
   // LOG(LOG_MASK_HT, "computing C %d %d", n, m);

   int temp;
	if (n < m) {
      temp = n;
		n = m;
		m = temp;
   }

	long long ret = factorial(n) / (factorial(m) * factorial(n - m));

   // LOG(LOG_MASK_HT, "C %d %d = %lld", n, m, ret);

   return ret;
}

// This API only supports binary HT, e.g., the value must be 0 or 1
double HashTable::test_sum_prob(int sum)
{
   double p0, p1, ret = 0;
   p0 = dist[0];
   p1 = dist[1];

   if (cur_size < sum) {
      return 0;
   }

   const double success_fraction = p1;
   int flips = cur_size;

   binomial flip(flips, success_fraction);

   ret = 1 - cdf(flip, sum - 1);

   assert(ret >= 0);
   assert(ret <= 1);

   /*
   for (int i=sum; i <= cur_size; i ++) {
      long long c = combinator(i, cur_size);
   }
   */

   LOG(LOG_MASK_HT, "test sum prob=%f for sum>=%d, size=%d, curSize=%d, p0=%f, p1=%f",
       ret, sum, size, cur_size, p0, p1);

   return ret;
}

int HashTableHandler::ht_test_sum(ExecutionState &cur, int sum,
                                  vector<ExecutionState*> &states,
                                  vector<int> &rets)
{
   // Compute and update outgoing path probabilities.
   double cur_prob = cur.getPathProb();

   // Using the CDF of binomial distribution to compute p
   double p = cur.table->test_sum_prob(sum);
   assert(p >= 0 && p <= 1);

   // if larger than sum is 0% or 100%
   if (p < 0.0001 || p > 0.9999) {
      LOG(LOG_MASK_HT, "p=%f, test_sum is determinsitic", p);
      states.push_back(&cur);
      rets.push_back(p < 0.0001 ? 0 : 1);
      LOG(LOG_MASK_HT, "continue pcur=%f, test_sum is %s", cur_prob,
          p > 0.9999 ? "100%" : "0%");
      return 0;
   }

   // if we are doing Monte Carlo sampling, just pick up one state
   if (cur.isSample == true) {
      assert(cur.isSampleDone == false);

      double r = theRNG.getDouble();

      states.push_back(&cur);

      // 10% return 1 (>= sum)
      if (r < p) {
         cur.updatePathProb(p);
         rets.push_back(1);
      }
      // 90% return 0 (<= sum)
      else {
         cur.updatePathProb(1-p);
         rets.push_back(0);
      }

      LOG(LOG_MASK_HT, "test sum returning, path prob=%f", cur.getPathProb());
      return 0;
   }

   // if both possible, need to fork two branches
   executor.branchTableAccessNoCond(cur, 2, states);
   assert(states.size() == 2);

   states[0]->updatePathProb(p);
   rets.push_back(1);
   states[1]->updatePathProb(1 - p);
   rets.push_back(0);

   LOG(LOG_MASK_HT, "pcur=%f, p1=%f, p2=%f", cur.getPathProb(),
       states[0]->getPathProb(), states[1]->getPathProb());

   return 0;
}

int HashTableHandler::ht_add_handler(ExecutionState &state)
{
   int status = state.table->status;
   // ht_add returns the prob of hitting/missing the hash table,
   // in this handler we don't need that.
   state.table->ht_add(status);
   return 0;
}

// Add by 1 to each possible entry of HT, based the previous access result
// previous result is provided by flag, so that this API can be used by
// cmin_add_to_ht
double HashTable::ht_add(int flag)
{
   LOG(LOG_MASK_CMIN, "adding one to hash table, cur_size=%d, flag=%d",
       cur_size, flag);

   double ret;
   // hit
   if (flag) {
      assert(cur_size > 0);
      int largest = 0;
      for (auto const x : dist) {
         if (x.second > 0 && x.first > largest) {
            largest = x.first;
         }
      }
      int v = largest;

      if (dist[v] * cur_size > 1) {
         dist[v] = (double)(dist[v] * cur_size - 1) / (double)cur_size;
      } else {
         dist[v] = 0;
      }

      dist[v+1] = (double)(dist[v+1] * cur_size + 1) / (double)cur_size;

      ret = (double)cur_size / (double)size;
      return ret;
   }

   // miss
   ret = ((double)size - (double)cur_size) / (double)size;

   if (dist.size() == 0) {
      dist[1] = 1.0;
   } else {
      for (auto x : dist) {
         if (x.first == 1) {
            x.second = (x.second * cur_size + 1) / (cur_size + 1);
         } else {
            x.second = (x.second * cur_size) / (cur_size + 1);
         }
      }
   }

   cur_size += 1;
   return ret;
}

void HashTable::dump()
{
   LOG(LOG_MASK_HT, "hash table size=%d, cur_size=%d", size, cur_size);
   for (auto x : dist) {
      LOG(LOG_MASK_HT, "[v=%d, p=%f]", x.first, x.second);
   }
}

int HashTableHandler::ht_read_larger_than(ExecutionState &cur,
                                          int val,
                                          vector<ExecutionState*> &states,
                                          vector<int> &rets)
{
   HashTable *t = cur.table;
   double larger = t->ht_get_prob_larger_than_after_hit(val);
   if (larger == 0.0) {
      states.push_back(&cur);
      rets.push_back(0);
      LOG(LOG_MASK_HT, "cannot greater than %d, returning one branch", val);
      return 0;
   } else if (larger == 1.0) {
      states.push_back(&cur);
      rets.push_back(1);
      LOG(LOG_MASK_HT, "always greater than %d, returning one branch", val);
      return 0;
   }

   executor.branchTableAccessNoCond(cur, 2, states);

   // branch 1: greater than val
   rets.push_back(1);
   states[0]->updatePathProb(larger);
   // branch 2: cmin is not greater than val
   rets.push_back(0);
   states[1]->updatePathProb(1 - larger);

   LOG(LOG_MASK_HT, "greater than %d prob=%g, elseprob=%g", val,
       larger, 1-larger);
   return 0;
}

// Get the probability that ht value is greater than val
// This API assumes that the previous access has hit HT
double HashTable::ht_get_prob_larger_than_after_hit(int val)
{
   double ret = 0.0;

   for (auto x : dist) {
      if (x.first >= val) {
         ret += x.second;
      }
   }
   return ret;
}

// Get the probability that ht value is greater than val
// This API doesn't assume that the previous access has hit HT
// Should be used by CminHandler::cmin_larger_than
double HashTable::ht_get_prob_larger_than(int val)
{
   double ret = 0.0;

   for (auto x : dist) {
      if (x.first >= val) {
         ret += (x.second * (double)cur_size / (double)size);
      }
   }
   return ret;
}

int HashTable::getMaxValue()
{
   int ret = 0;
   for (auto e: dist) {
      if (e.second > 0) {
         ret = ret > e.first ? ret : e.first;
      }
   }
   return ret;
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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 Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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 <iostream>

#include <Eigen/Core>

#include <gtest/gtest.h>

#include "kindr/rotations/RotationDiff.hpp"
#include "kindr/common/gtest_eigen.hpp"
#include "kindr/common/common.hpp"

namespace rot = kindr;
namespace quat = kindr;

template <typename Implementation>
struct RotationQuaternionDiffTest: public ::testing::Test {
  typedef Implementation RotationDiff;
  typedef typename Implementation::Scalar Scalar;
  typedef rot::RotationQuaternion<Scalar> Rotation;
  typedef rot::LocalAngularVelocity<Scalar> LocalAngularVelocity;
  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
  typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
  typedef quat::Quaternion<Scalar> Quaternion;
  Vector4 eigenVector4vZero = Vector4::Zero();
  Vector4 eigenVector4v1;

  LocalAngularVelocity angularVelocity1 = LocalAngularVelocity(0.0, 0.0, 0.0);
  LocalAngularVelocity angularVelocity2 = LocalAngularVelocity(0.4, 0.3, 0.8);
  LocalAngularVelocity angularVelocity3 = LocalAngularVelocity(40, 52, 99)*0.001;
  LocalAngularVelocity angularVelocity4 = LocalAngularVelocity(kindr::internal::NumTraits<Scalar>::dummy_precision()/10.0, 0.0, 0.0);
  LocalAngularVelocity angularVelocity5 = LocalAngularVelocity(0.0, kindr::internal::NumTraits<Scalar>::dummy_precision()/10.0, 0.0);
  LocalAngularVelocity angularVelocity6 = LocalAngularVelocity(0.0, 0.0, kindr::internal::NumTraits<Scalar>::dummy_precision()/10.0);
  Rotation rotation1 = Rotation(Quaternion(0.0, 1.0, 0.0, 0.0).toUnitQuaternion());
  Rotation rotation2 = Rotation(Quaternion(0.5, 1.0, 0.0, 0.0).toUnitQuaternion());
  Rotation rotation3 = Rotation(Quaternion(0.5, 0.0, 1.0, 0.0).toUnitQuaternion());
  Rotation rotation4 = Rotation(Quaternion(0.5, 0.0, 0.0, 1.0).toUnitQuaternion());
  Rotation rotation5 = Rotation(Quaternion(kindr::internal::NumTraits<Scalar>::dummy_precision()/10.0, 1.0, 0.0, 0.0).toUnitQuaternion());
  Rotation rotation6 = Rotation(Quaternion(kindr::internal::NumTraits<Scalar>::dummy_precision()/10.0, 0.0, 1.0, 0.0).toUnitQuaternion());
  Rotation rotation7 = Rotation(Quaternion(kindr::internal::NumTraits<Scalar>::dummy_precision()/10.0, 0.0, 0.0, 1.0).toUnitQuaternion());
  Rotation rotation8 = Rotation(Quaternion(0.8, 1.0/sqrt(1+4+9), 2.0/sqrt(1+4+9), 3.0/sqrt(1+4+9)).toUnitQuaternion());

  std::vector<Rotation> rotations;
  std::vector<LocalAngularVelocity> angularVelocities;
  RotationQuaternionDiffTest() {
    rotations.push_back(rotation1);
    rotations.push_back(rotation2);
    rotations.push_back(rotation3);
    rotations.push_back(rotation4);
    rotations.push_back(rotation5);
    rotations.push_back(rotation6);
    rotations.push_back(rotation7);
    rotations.push_back(rotation8);

    angularVelocities.push_back(angularVelocity1);
    angularVelocities.push_back(angularVelocity2);
    angularVelocities.push_back(angularVelocity3);
    angularVelocities.push_back(angularVelocity4);
    angularVelocities.push_back(angularVelocity5);
    angularVelocities.push_back(angularVelocity6);

    eigenVector4v1 << 2.2, 3.3, 4.4, 5.5;
  }
};

typedef ::testing::Types<
    rot::RotationQuaternionDiffPD,
    rot::RotationQuaternionDiffPF
> RotationQuaternionDiffTypes;

TYPED_TEST_CASE(RotationQuaternionDiffTest, RotationQuaternionDiffTypes);

TYPED_TEST(RotationQuaternionDiffTest, testConstructors)
{
  typedef typename TestFixture::RotationDiff RotationDiff;
  typedef typename TestFixture::Scalar Scalar;
  typedef typename TestFixture::Quaternion Quaternion;

  RotationDiff rotDiff;
  ASSERT_EQ(this->eigenVector4vZero(0), rotDiff.w());
  ASSERT_EQ(this->eigenVector4vZero(1), rotDiff.x());
  ASSERT_EQ(this->eigenVector4vZero(2), rotDiff.y());
  ASSERT_EQ(this->eigenVector4vZero(3), rotDiff.z());

  RotationDiff rotDiff2(this->eigenVector4v1(0), this->eigenVector4v1(1), this->eigenVector4v1(2), this->eigenVector4v1(3));
  ASSERT_EQ(this->eigenVector4v1(0), rotDiff2.w());
  ASSERT_EQ(this->eigenVector4v1(1), rotDiff2.x());
  ASSERT_EQ(this->eigenVector4v1(2), rotDiff2.y());
  ASSERT_EQ(this->eigenVector4v1(3), rotDiff2.z());

  RotationDiff rotDiff3(this->eigenVector4v1(0), this->eigenVector4v1.template block<3,1>(1,0));
  ASSERT_EQ(this->eigenVector4v1(0), rotDiff3.w());
  ASSERT_EQ(this->eigenVector4v1(1), rotDiff3.x());
  ASSERT_EQ(this->eigenVector4v1(2), rotDiff3.y());
  ASSERT_EQ(this->eigenVector4v1(3), rotDiff3.z());

  RotationDiff rotDiff4(this->eigenVector4v1);
  ASSERT_EQ(this->eigenVector4v1(0), rotDiff4.w());
  ASSERT_EQ(this->eigenVector4v1(1), rotDiff4.x());
  ASSERT_EQ(this->eigenVector4v1(2), rotDiff4.y());
  ASSERT_EQ(this->eigenVector4v1(3), rotDiff4.z());

  RotationDiff rotDiff5(Quaternion(this->eigenVector4v1));
  ASSERT_EQ(this->eigenVector4v1(0), rotDiff5.w());
  ASSERT_EQ(this->eigenVector4v1(1), rotDiff5.x());
  ASSERT_EQ(this->eigenVector4v1(2), rotDiff5.y());
  ASSERT_EQ(this->eigenVector4v1(3), rotDiff5.z());
}

TYPED_TEST(RotationQuaternionDiffTest, testGetters)
{
  typedef typename TestFixture::RotationDiff RotationDiff;
  typedef typename TestFixture::Scalar Scalar;

  RotationDiff rotDiff2(this->eigenVector4v1(0), this->eigenVector4v1(1), this->eigenVector4v1(2), this->eigenVector4v1(3));
  ASSERT_EQ(this->eigenVector4v1(0), rotDiff2.vector()(0));
  ASSERT_EQ(this->eigenVector4v1(1), rotDiff2.vector()(1));
  ASSERT_EQ(this->eigenVector4v1(2), rotDiff2.vector()(2));
  ASSERT_EQ(this->eigenVector4v1(3), rotDiff2.vector()(3));

  RotationDiff rotDiff3(this->eigenVector4v1(0), this->eigenVector4v1(1), this->eigenVector4v1(2), this->eigenVector4v1(3));
  ASSERT_EQ(this->eigenVector4v1(0), rotDiff3.toQuaternion().w());
  ASSERT_EQ(this->eigenVector4v1(1), rotDiff3.toQuaternion().x());
  ASSERT_EQ(this->eigenVector4v1(2), rotDiff3.toQuaternion().y());
  ASSERT_EQ(this->eigenVector4v1(3), rotDiff3.toQuaternion().z());

  RotationDiff rotDiff4(this->eigenVector4v1(0), this->eigenVector4v1(1), this->eigenVector4v1(2), this->eigenVector4v1(3));
  ASSERT_EQ(this->eigenVector4v1(0), rotDiff3.real());
  ASSERT_EQ(this->eigenVector4v1(1), rotDiff3.imaginary().x());
  ASSERT_EQ(this->eigenVector4v1(2), rotDiff3.imaginary().y());
  ASSERT_EQ(this->eigenVector4v1(3), rotDiff3.imaginary().z());

}

TYPED_TEST(RotationQuaternionDiffTest, testSetters)
{
  typedef typename TestFixture::RotationDiff RotationDiff;
  typedef typename TestFixture::Quaternion Quaternion;
  typedef typename TestFixture::Scalar Scalar;

  RotationDiff rotDiff3;
  rotDiff3.toQuaternion() = Quaternion(this->eigenVector4v1);
  ASSERT_EQ(this->eigenVector4v1(0), rotDiff3.w());
  ASSERT_EQ(this->eigenVector4v1(1), rotDiff3.x());
  ASSERT_EQ(this->eigenVector4v1(2), rotDiff3.y());
  ASSERT_EQ(this->eigenVector4v1(3), rotDiff3.z());

  RotationDiff rotDiff4(this->eigenVector4v1(0), this->eigenVector4v1(1), this->eigenVector4v1(2), this->eigenVector4v1(3));
  rotDiff4.setZero();
  ASSERT_EQ(this->eigenVector4vZero(0), rotDiff4.w());
  ASSERT_EQ(this->eigenVector4vZero(1), rotDiff4.x());
  ASSERT_EQ(this->eigenVector4vZero(2), rotDiff4.y());
  ASSERT_EQ(this->eigenVector4vZero(3), rotDiff4.z());

  RotationDiff rotDiff5;
  rotDiff5.w() = this->eigenVector4v1(0);
  rotDiff5.x() = this->eigenVector4v1(1);
  rotDiff5.y() = this->eigenVector4v1(2);
  rotDiff5.z() = this->eigenVector4v1(3);
  ASSERT_EQ(this->eigenVector4v1(0), rotDiff5.w());
  ASSERT_EQ(this->eigenVector4v1(1), rotDiff5.x());
  ASSERT_EQ(this->eigenVector4v1(2), rotDiff5.y());
  ASSERT_EQ(this->eigenVector4v1(3), rotDiff5.z());

}

TYPED_TEST(RotationQuaternionDiffTest, testFiniteDifference)
{
  typedef typename TestFixture::Scalar Scalar;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::RotationDiff RotationDiff;
  typedef typename TestFixture::RotationDiff::Imaginary Vector3;

  const  double dt = 1e-5;
  for (auto rotation : this->rotations) {
    for (auto angularVelocity : this->angularVelocities) {
      // Finite difference method for checking derivatives
      RotationDiff rotationDiff(rotation, angularVelocity);
      Rotation rotationNext = rotation.boxPlus(dt*rotation.rotate(angularVelocity.vector()));
      Scalar dreal = (rotationNext.real()-rotation.real())/dt;
      Vector3 dimag = (rotationNext.imaginary()-rotation.imaginary())/dt;
      ASSERT_NEAR(rotationDiff.w(),dreal,1e-2) << "rotation: " << rotation << " angular velocity: " << angularVelocity <<     " rdiff: " << dreal << " " << dimag(0) << " " << dimag(1) << " " << dimag(2) << "diff: " << rotationDiff;
      ASSERT_NEAR(rotationDiff.x(),dimag(0),1e-2) << "rotation: " << rotation << " angular velocity: " << angularVelocity  << " rdiff: " << dreal << " " << dimag(0) << " " << dimag(1) << " " << dimag(2) << " diff: " << rotationDiff;
      ASSERT_NEAR(rotationDiff.y(),dimag(1),1e-2) << "rotation: " << rotation << " angular velocity: " << angularVelocity <<  " rdiff: " << dreal << " " << dimag(0) << " " << dimag(1) << " " << dimag(2) << " diff: " << rotationDiff;
      ASSERT_NEAR(rotationDiff.z(),dimag(2),1e-2) << "rotation: " << rotation << " angular velocity: " << angularVelocity <<  " rdiff: " << dreal << " " << dimag(0) << " " << dimag(1) << " " << dimag(2) << " diff: " << rotationDiff;

    }
  }
}

TYPED_TEST(RotationQuaternionDiffTest, testMultiplication)
{
  typedef typename TestFixture::RotationDiff RotationDiff;
  RotationDiff rotDiff1(1.0, 0.0, 0.0, 0.0);
  for (auto rotation : this->rotations) {
    RotationDiff testRotation = rotDiff1 * rotation;
    RotationDiff testRotation1 = rotation * rotDiff1;
    ASSERT_EQ(testRotation.w(), rotation.w());
    ASSERT_EQ(testRotation.x(), rotation.x());
    ASSERT_EQ(testRotation.y(), rotation.y());
    ASSERT_EQ(testRotation.z(), rotation.z());
    ASSERT_EQ(testRotation1.w(), rotation.w());
    ASSERT_EQ(testRotation1.x(), rotation.x());
    ASSERT_EQ(testRotation1.y(), rotation.y());
    ASSERT_EQ(testRotation1.z(), rotation.z());
  }

  RotationDiff rotDiff2(0.0, 5.0, 0.0, 0.0);
  auto testRotationDiff0 = rotDiff2 * this->rotations[1];
  ASSERT_NEAR(testRotationDiff0.w(), -4.472, 1e-2);
  ASSERT_NEAR(testRotationDiff0.x(), 2.236, 1e-2);
  ASSERT_NEAR(testRotationDiff0.y(), 0.0, 1e-2);
  ASSERT_NEAR(testRotationDiff0.z(), 0.0, 1e-2);
  auto testRotationDiff1 = this->rotations[1] * rotDiff2;
  ASSERT_NEAR(testRotationDiff1.w(), -4.472, 1e-2);
  ASSERT_NEAR(testRotationDiff1.x(), 2.236, 1e-2);
  ASSERT_NEAR(testRotationDiff1.y(), 0.0, 1e-2);
  ASSERT_NEAR(testRotationDiff1.z(), 0.0, 1e-2);

  RotationDiff rotDiff3(0.0, 5.0, 1.0, 0.0);
  auto testRotationDiff2 = rotDiff3 * this->rotations[1];
  ASSERT_NEAR(testRotationDiff2.w(), -4.472, 1e-2);
  ASSERT_NEAR(testRotationDiff2.x(), 2.236, 1e-2);
  ASSERT_NEAR(testRotationDiff2.y(), 0.447, 1e-2);
  ASSERT_NEAR(testRotationDiff2.z(), -0.894, 1e-2);
  auto testRotationDiff3 = this->rotations[1] * rotDiff3;
  ASSERT_NEAR(testRotationDiff3.w(), -4.472, 1e-2);
  ASSERT_NEAR(testRotationDiff3.x(), 2.236, 1e-2);
  ASSERT_NEAR(testRotationDiff3.y(), 0.447, 1e-2);
  ASSERT_NEAR(testRotationDiff3.z(), 0.894, 1e-2);
}


#ifndef PARMCB_UTIL_HPP_
#define PARMCB_UTIL_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <set>
#include <map>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>

#include <parmcb/forestindex.hpp>

#define BUFFER_SIZE 1024

namespace parmcb {

    template<class Graph>
    bool has_loops(const Graph &g) {
        auto eRange = boost::edges(g);
        for (auto eit = eRange.first; eit != eRange.second; ++eit) {
            auto e = *eit;
            auto v = boost::source(e, g);
            auto u = boost::target(e, g);
            if (v == u) {
                return true;
            }
        }
        return false;
    }

    template<class Graph>
    bool has_multiple_edges(const Graph &g) {
        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;

        auto vRange = boost::vertices(g);
        for (auto vit = vRange.first; vit != vRange.second; ++vit) {
            auto v = *vit;
            std::set<Vertex> neighbors;

            auto eRange = boost::out_edges(v, g);
            for (auto eit = eRange.first; eit != eRange.second; ++eit) {
                auto e = *eit;
                auto u = boost::opposite(e, v, g);
                if (!neighbors.insert(u).second) {
                    return true;
                }
            }
        }
        return false;
    }

    template<class Graph, class WeightMap>
    bool has_non_positive_weights(const Graph &g, const WeightMap &weight_map) {
        auto eRange = boost::edges(g);
        for (auto eit = eRange.first; eit != eRange.second; ++eit) {
            auto e = *eit;
            if (boost::get(weight_map, e) <= 0.0) {
                return true;
            }
        }
        return false;
    }

    template<class Graph>
    bool is_cycle(const Graph &g, const std::list<typename boost::graph_traits<Graph>::edge_descriptor> &cycle) {
        if (cycle.size() == 0)
            return false;

        std::vector<std::size_t> degrees(boost::num_vertices(g));
        auto index_map = boost::get(boost::vertex_index, g);
        auto degrees_map = boost::make_iterator_property_map(degrees.begin(), index_map);

        for (auto eit = cycle.begin(); eit != cycle.end(); eit++) {
            auto e = *eit;
            degrees_map[boost::source(e, g)]++;
            degrees_map[boost::target(e, g)]++;
        }

        typename boost::graph_traits<Graph>::vertex_iterator vi, viend;
        for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
            if (degrees_map[*vi] % 2 == 1) {
                return false;
            }
        }

        return true;
    }

    template<class Graph, class InputIterator, class OutputIterator>
    void convert_edges(InputIterator inIt, OutputIterator outIt, const ForestIndex<Graph> &forest_index) {
        std::for_each(inIt.begin(), inIt.end(), [&outIt, &forest_index](auto i) {
            outIt = forest_index(i);
        });
    }

    template<class Graph>
    void read_dimacs_from_file(FILE *fp, Graph &graph) {
        typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
        typedef typename boost::graph_traits<Graph>::edge_descriptor edge_descriptor;

        char buffer[BUFFER_SIZE], problem[BUFFER_SIZE];

        std::size_t nnodes, nedges;
        std::map<std::size_t, vertex_descriptor> vertex_map;

        typename boost::property_map<Graph, boost::edge_weight_t>::type weight = get(boost::edge_weight, graph);

        while (fgets(buffer, sizeof(buffer), fp) != NULL) {
            buffer[strlen(buffer) - 1] = '\0'; // eat the newline
            if (buffer[0] == 'c' || buffer[0] == '#') {
                continue;
            } else if (buffer[0] == 'p') {
                sscanf(buffer, "p %s %lu %lu", problem, &nnodes, &nedges);
                for (std::size_t i = 1; i <= nnodes; i++) {
                    vertex_map[i] = boost::add_vertex(graph);
                }
            } else if (buffer[0] == 'a' || buffer[0] == 'e') {
                int rs, rt;
                double rw = 1;
                char fc;
                sscanf(buffer, "%c %d %d %lf", &fc, &rs, &rt, &rw);

                if (vertex_map.find(rs) == vertex_map.end()) {
                    throw std::system_error(EIO, std::generic_category(), "Vertex not found");
                }
                if (vertex_map.find(rt) == vertex_map.end()) {
                    throw std::system_error(EIO, std::generic_category(), "Vertex not found");
                }
                vertex_descriptor sDescriptor = boost::vertex(vertex_map[rs], graph);
                vertex_descriptor tDescriptor = boost::vertex(vertex_map[rt], graph);
                edge_descriptor eDescriptor = boost::add_edge(sDescriptor, tDescriptor, graph).first;
                weight[eDescriptor] = rw;
            }
        }

    }

}

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_POLYNOMIALS_FUNCTIONS_SCALAR_IMPL_HORNER_HPP_INCLUDED
#define NT2_TOOLBOX_POLYNOMIALS_FUNCTIONS_SCALAR_IMPL_HORNER_HPP_INCLUDED

#include <boost/mpl/at.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/pop_back.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/include/functions/scalar/fma.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/include/constants/digits.hpp>
#include <boost/preprocessor/seq/for_each.hpp>
#include <boost/preprocessor/tuple/to_seq.hpp>
#include <boost/dispatch/preprocessor/strip.hpp>

namespace nt2
{
  //////////////////////////////////////////////////////////////////////////////
  // static unrollign of horner scheme
  // Coefficients are given in decreasing powers and hexadecimal form
  // horner< NT2_HORNER_COEFF(float,3,(0x3f800000,0x40000000,0x40400000))>(2.0f)
  // means ((1*x+2)*x+3) or x^2+2x+3
  //////////////////////////////////////////////////////////////////////////////
  namespace details
    {
    template<int N, class Seq> struct static_horner_
    {
      template<class Sig> struct result;
      template<class This,class T> struct result<This(T)> : meta::strip<T> {};

      template<class T> inline
      T operator()(T const& x) const
      {
        static_horner_<N-1,typename boost::mpl::pop_back<Seq>::type> callee;
        return fma( x
                  , callee(x)
                  , Const<T,boost::mpl::at_c<Seq,N-1>::type::value>()
                  );
      }
    };

    template<class Seq> struct static_horner_<1,Seq>
    {
      template<class Sig> struct result;
      template<class This,class T> struct result<This(T)> : meta::strip<T> {};

      template<class T> inline
      T operator()(T const& ) const
      {
        return Const<T, boost::mpl::at_c<Seq,0>::type::value >();
      }
    };
  }

  //////////////////////////////////////////////////////////////////////////////
  /// @brief Static Horner scheme
  //////////////////////////////////////////////////////////////////////////////
  template<class Coeff,class Type>
  static inline Type horner( Type const& x )
  {
    details::static_horner_<boost::mpl::size<Coeff>::value,Coeff> callee;
    return callee(x);
  }
}

#define NT2_COEFF_GEN(z,n,text)                                           \
boost::mpl::integral_c< BOOST_DISPATCH_PP_STRIP(BOOST_PP_TUPLE_ELEM(3,0,text))       \
                      , BOOST_PP_TUPLE_ELEM(BOOST_PP_TUPLE_ELEM(3,1,text) \
                                           ,n                             \
                                           ,BOOST_PP_TUPLE_ELEM(3,2,text) \
                                           )                              \
                      >                                                   \
/**/

////////////////////////////////////////////////////////////////////////////////
// Horner coefficient building macro
////////////////////////////////////////////////////////////////////////////////
#define NT2_HORNER_COEFF(Type, Size, Seq)                                     \
boost::mpl::vector< BOOST_PP_ENUM(Size                                        \
                                 ,NT2_COEFF_GEN                               \
                                 ,((nt2::meta::as_integer<Type, unsigned>::type),Size,Seq)\
                                 ) >                                          \
/**/

////////////////////////////////////////////////////////////////////////////////
// Horner coefficient building macro for template dependant Type
////////////////////////////////////////////////////////////////////////////////
#define NT2_HORNER_COEFF_T(Type, Size, Seq)                                   \
boost::mpl::vector< BOOST_PP_ENUM(Size                                        \
                                 ,NT2_COEFF_GEN                               \
                                 ,((typename nt2::meta::as_integer<Type, unsigned>::type)\
                                  ,Size                                       \
                                  ,Seq)                                       \
                                 ) >                                          \
/**/

#endif


#include "../guia1/particionar.cpp"
#include "../config.hpp"
#include <boost/filesystem.hpp>

int main()
{
    arma_rng::set_seed_random();

    vec datos;
    datos.load(config::sourceDir + "/guia2/datos/merval.csv");

    mat tuplas(datos.n_elem - 5, 6);

    for (unsigned int i = 0; i < datos.n_rows - 5; ++i) {
        tuplas.row(i) = datos.rows(span(i, i + 5)).t();
    }

    // FIXME: ¿Hay que randomizar el orden de las tuplas?

    tuplas.save(config::sourceDir + "/guia2/datos/mervalTuplas.csv", csv_ascii);

    vector<ic::Particion> particiones = ic::leaveKOut(tuplas, 47);

    boost::filesystem::create_directory(config::sourceDir + "/guia2/datos/particionesMerval/");
    ic::guardarParticiones(particiones, config::sourceDir + "/guia2/datos/particionesMerval/");

    return 0;
}


#include <NTL/vec_GF2.h>
#include <utils/utils.h>
#include <NTL/GF2XFactoring.h>
#include "test_utils.h"

int test::utils::test_gf2x_from_gf2() {

    NTL::vec_GF2 input;
    input.SetLength(5);

    {
        input[0] = NTL::GF2(0);
        input[1] = NTL::GF2(1);
        input[2] = NTL::GF2(1);
        input[3] = NTL::GF2(1);
        input[4] = NTL::GF2(0);
    }

    auto out = ::utils::gf2x_from_gf2(input);

    assert(out[0] == input[0]);
    assert(out[1] == input[1]);
    assert(out[2] == input[2]);
    assert(out[3] == input[3]);
    assert(out[4] == input[4]);

    return 0;
}
int test::utils::test_gf2_from_gf2x() {

    NTL::GF2X input;
    input.SetLength(5);

    {
        input[0] = NTL::GF2(0);
        input[1] = NTL::GF2(1);
        input[2] = NTL::GF2(1);
        input[3] = NTL::GF2(1);
        input[4] = NTL::GF2(0);
    }

    auto out = ::utils::gf2_from_gf2x(input, 5);

    assert(out[0] == input[0]);
    assert(out[1] == input[1]);
    assert(out[2] == input[2]);
    assert(out[3] == input[3]);
    assert(out[4] == input[4]);

    return 0;
}

int test::utils::test_gf2x_from_matgf2() {

    NTL::mat_GF2 input;
    input.SetDims(2, 4);

    {
        input[0][0] = NTL::GF2(0);
        input[0][1] = NTL::GF2(1);
        input[0][2] = NTL::GF2(1);
        input[0][3] = NTL::GF2(0);

        input[1][0] = NTL::GF2(1);
        input[1][1] = NTL::GF2(1);
        input[1][2] = NTL::GF2(0);
        input[1][3] = NTL::GF2(1);
    }

    auto out = ::utils::gf2x_from_matgf2(input);

    assert(out[0] == input[0][0]);
    assert(out[1] == input[0][1]);
    assert(out[2] == input[0][2]);
    assert(out[3] == input[0][3]);

    assert(out[4] == input[1][0]);
    assert(out[5] == input[1][1]);
    assert(out[6] == input[1][2]);
    assert(out[7] == input[1][3]);

    return 0;
}

int test::utils::test_gf2x_from_gf2e() {

    int polynomial_degree = 18;
    int length_of_vector = 100;

    auto irred_poly = NTL::BuildIrred_GF2X(polynomial_degree);
    NTL::GF2E::init(irred_poly);

    auto input = NTL::random_vec_GF2E(1);
    auto output = ::utils::gf2x_from_gf2e(input);

    for(int i = 0; i < input[0].degree(); i++) {
        assert(input[0].LoopHole()[i] == output[i]);
    }

    input = NTL::random_vec_GF2E(length_of_vector);
    output = ::utils::gf2x_from_gf2e(input);

    for(int i = 0; i < input.length(); i++) {
        for(int j = 0; j < input[i].degree(); j++) {
            assert(input[i].LoopHole()[j] == output[j + (i * input[i].degree())]);
        }
    }

    return 0;
}

int test::utils::test_gf2_from_gf2e() {

    int polynomial_degree = 19;
    int length_of_vector = 100;

    auto irred_poly = NTL::BuildIrred_GF2X(polynomial_degree);
    NTL::GF2E::init(irred_poly);

    auto input = NTL::random_vec_GF2E(1);
    auto output = ::utils::gf2_from_gf2e(input);

    for(int i = 0; i < input[0].degree(); i++) {
        assert(input[0].LoopHole()[i] == output[i]);
    }

    input = NTL::random_vec_GF2E(length_of_vector);
    output = ::utils::gf2_from_gf2e(input);

    for(int i = 0; i < input.length(); i++) {
        for(int j = 0; j < input[i].degree(); j++) {
            assert(input[i].LoopHole()[j] == output[j + (i * input[i].degree())]);
        }
    }

    return 0;
}

int test::utils::test_gf2e_from_gf2x() {

    int polynomial_degree = 21;
    int vector_length = 250;

    auto irred_poly = NTL::BuildIrred_GF2X(polynomial_degree);
    NTL::GF2E::init(irred_poly);

    auto input = NTL::random_GF2X(vector_length);
    auto output = ::utils::gf2e_from_gf2x(input, vector_length);

    for(int i = 0; i < output.length(); i++) {
        for(int j = 0; j < output[i].degree(); j++) {
            assert(input[j + (i * output[i].degree())] == output[i].LoopHole()[j]);
        }
    }

    return 0;
}

int test::utils::test_gf2e_from_two_gf2() {

    int polynomial_degree = 17;
    int vector_length = 4;

    auto irred_poly = NTL::BuildIrred_GF2X(polynomial_degree);
    NTL::GF2E::init(irred_poly);

    auto input_1 = NTL::random_vec_GF2(vector_length);
    auto input_2 = NTL::random_vec_GF2(vector_length);

    auto output = ::utils::gf2e_from_two_gf2(input_1, input_2);

    NTL::append(input_1, input_2);

    for(int i = 0; i < output.length(); i++) {
        for(int j = 0; j < output[i].degree(); j++) {
            assert(input_1[j + (i * output[i].degree())] == output[i].LoopHole()[j]);
        }
    }

    return 0;
}

int test::utils::test_gf2e_from_vec_gf2() {

    int polynomial_degree = 17;
    int vector_length = 100;

    auto irred_poly = NTL::BuildIrred_GF2X(polynomial_degree);
    NTL::GF2E::init(irred_poly);

    auto input = NTL::random_vec_GF2(vector_length);
    auto output = ::utils::gf2e_from_vec_gf2(input);

    for(int i = 0; i < output.length(); i++) {
        for(int j = 0; j < output[i].degree(); j++) {
            assert(input[j + (i * output[i].degree())] == output[i].LoopHole()[j]);
        }
    }

    return 0;
}

int test::utils::test_mat_gf2_from_vec_gf2e() {

    int polynomial_degree = 17;
    int vector_length = 100;

    auto irred_poly = NTL::BuildIrred_GF2X(polynomial_degree);
    NTL::GF2E::init(irred_poly);

    auto input = NTL::random_vec_GF2E(vector_length);
    auto output = ::utils::mat_gf2_from_vec_gf2e(input);

    for(int i = 0; i < input.length(); i++) {
        for(int j = 0; j < input[i].degree(); j++) {
            assert(output[i][j] == input[i]._GF2E__rep[j]);
        }
    }

    return 0;
}