#include <iostream>
#include <memory>
#include <string>
#include <chrono>

#include <boost/program_options.hpp>
#include <algorithms/Grover.hpp>
#include <algorithms/QFT.hpp>
#include <algorithms/Entanglement.hpp>

#include "Simulator.hpp"
#include "QFRSimulator.hpp"
#include "GroverSimulator.hpp"
#include "ShorFastSimulator.hpp"
#include "ShorSimulator.hpp"

int main(int argc, char** argv) {
    namespace po = boost::program_options;
    unsigned long long seed;

    po::options_description description("JKQ DDSIM by https://iic.jku.at/eda/ -- Allowed options");
    description.add_options()
            ("help,h", "produce help message")
            ("seed", po::value<unsigned long long>(&seed)->default_value(0), "seed for random number generator (default zero is possibly directly used as seed!)")
            ("shots", po::value<unsigned int>()->default_value(0), "number of measurements (if the algorithm does not contain non-unitary gates, weak simulation is used)")
            ("display_vector", "display the state vector")
            ("ps", "print simulation stats (applied gates, sim. time, and maximal size of the DD)")
            ("verbose", "Causes some simulators to print additional information to STDERR")
            ("benchmark", "print simulation stats in a single CSV style line (overrides --ps and suppresses most other output, please don't rely on the format across versions)")

            ("simulate_file", po::value<std::string>(), "simulate a quantum circuit given by file (detection by the file extension)")
            ("simulate_qft", po::value<unsigned int>(), "simulate Quantum Fourier Transform for given number of qubits")
            ("simulate_ghz", po::value<unsigned int>(), "simulate state preparation of GHZ state for given number of qubits")
            ("step_fidelity", po::value<double>()->default_value(1.0), "target fidelity for each approximation run (>=1 = disable approximation)")
            ("steps", po::value<unsigned int>()->default_value(1), "number of approximation steps")
            ("initial_reorder", po::value<int>()->default_value(0), "Try to find a good initial variable order (0=None, 1=Most affected qubits to the top, 2=Most affected targets to the top)")
            ("dynamic_reorder", po::value<int>()->default_value(0), "Apply reordering strategy during simulation (0=None, 1=Sifting, 2=Move2Top)")
            ("post_reorder", po::value<int>()->default_value(0), "Apply a reordering strategy after simulation (0=None, 1=Sifting)")

            ("simulate_grover", po::value<unsigned int>(), "simulate Grover's search for given number of qubits with random oracle")
            ("simulate_grover_emulated", po::value<unsigned int>(), "simulate Grover's search for given number of qubits with random oracle and emulation")
            ("simulate_grover_oracle_emulated", po::value<std::string>(), "simulate Grover's search for given number of qubits with given oracle and emulation")

            ("simulate_shor", po::value<unsigned int>(), "simulate Shor's algorithm factoring this number")
            ("simulate_shor_coprime", po::value<unsigned int>()->default_value(0), "coprime number to use with Shor's algorithm (zero randomly generates a coprime)")
            ("simulate_shor_no_emulation", "Force Shor simulator to do modular exponentiation instead of using emulation (you'll usually want emulation)")

            ("simulate_fast_shor", po::value<unsigned int>(), "simulate Shor's algorithm factoring this number with intermediate measurements")
            ("simulate_fast_shor_coprime", po::value<unsigned int>()->default_value(0), "coprime number to use with Shor's algorithm (zero randomly generates a coprime)")
            ;
    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, description), vm);

        if (vm.count("help")) {
            std::cout << description;
            return 0;
        }
        po::notify(vm);
    } catch (const po::error &e) {
        std::cerr << "[ERROR] " << e.what() << "! Try option '--help' for available commandline options.\n";
        std::exit(1);
    }

    std::unique_ptr<qc::QuantumComputation> quantumComputation;
    std::unique_ptr<Simulator> ddsim{nullptr};

    if (vm.count("simulate_file")) {
        const std::string fname = vm["simulate_file"].as<std::string>();
    	quantumComputation = std::make_unique<qc::QuantumComputation>(fname);
        ddsim = std::make_unique<QFRSimulator>(quantumComputation,
                                               vm["steps"].as<unsigned int>(), vm["step_fidelity"].as<double>(),
                                               vm["initial_reorder"].as<int>(), vm["dynamic_reorder"].as<int>(), vm["post_reorder"].as<int>(),
                                               seed);
    } else if (vm.count("simulate_qft")) {
	    const unsigned int n_qubits = vm["simulate_qft"].as<unsigned int>();
	    quantumComputation = std::make_unique<qc::QFT>(n_qubits);
        ddsim = std::make_unique<QFRSimulator>(quantumComputation,
                                               vm["steps"].as<unsigned int>(), vm["step_fidelity"].as<double>(),
                                               vm["initial_reorder"].as<int>(), vm["dynamic_reorder"].as<int>(), vm["post_reorder"].as<int>(),
                                               seed);
    } else if (vm.count("simulate_fast_shor")) {
        const unsigned int composite_number = vm["simulate_fast_shor"].as<unsigned int>();
        const unsigned int coprime = vm["simulate_fast_shor_coprime"].as<unsigned int>();
        if (seed == 0) {
            ddsim = std::make_unique<ShorFastSimulator>(composite_number, coprime, vm.count("verbose") > 0);
        } else {
            ddsim = std::make_unique<ShorFastSimulator>(composite_number, coprime, seed, vm.count("verbose") > 0);
        }
    } else if (vm.count("simulate_shor")) {
        const unsigned int composite_number = vm["simulate_shor"].as<unsigned int>();
        const unsigned int coprime = vm["simulate_shor_coprime"].as<unsigned int>();
        if (seed == 0) {
            ddsim = std::make_unique<ShorSimulator>(composite_number, coprime, vm.count("verbose") > 0);
        } else {
            ddsim = std::make_unique<ShorSimulator>(composite_number, coprime, seed, vm.count("verbose") > 0);
        }
    } else if (vm.count("simulate_grover")) {
        const unsigned int n_qubits = vm["simulate_grover"].as<unsigned int>();
        quantumComputation = std::make_unique<qc::Grover>(n_qubits, seed);
        ddsim = std::make_unique<QFRSimulator>(quantumComputation,
                                               vm["steps"].as<unsigned int>(), vm["step_fidelity"].as<double>(),
                                               vm["initial_reorder"].as<int>(), vm["dynamic_reorder"].as<int>(), vm["post_reorder"].as<int>(),
                                               seed);
    } else if (vm.count("simulate_grover_emulated")) {
        ddsim = std::make_unique<GroverSimulator>(vm["simulate_grover_emulated"].as<unsigned int>(), seed);
    } else if (vm.count("simulate_grover_oracle_emulated")) {
        ddsim = std::make_unique<GroverSimulator>(vm["simulate_grover_oracle_emulated"].as<std::string>(), seed);
    } else if (vm.count("simulate_ghz")) {
	    const unsigned int n_qubits = vm["simulate_ghz"].as<unsigned int>();
	    quantumComputation = std::make_unique<qc::Entanglement>(n_qubits);
        ddsim = std::make_unique<QFRSimulator>(quantumComputation,
                                               vm["steps"].as<unsigned int>(), vm["step_fidelity"].as<double>(),
                                               vm["initial_reorder"].as<int>(), vm["dynamic_reorder"].as<int>(), vm["post_reorder"].as<int>(),
                                               seed);
    } else {
        std::cerr << "Did not find anything to simulate. See help below.\n"
                  << description << "\n";
        return 1;
    }

    if (quantumComputation && quantumComputation->getNqubits() > dd::MAXN) {
        std::cerr << "Quantum computation contains to many qubits (limit is set to " << dd::MAXN << "). See documentation for details.\n";
        std::exit(1);
    }

    auto t1 = std::chrono::high_resolution_clock::now();
    auto m = ddsim->Simulate(vm["shots"].as<unsigned int>());
    auto t2 = std::chrono::high_resolution_clock::now();

    std::chrono::duration<float> duration_simulation = t2-t1;

    if (vm.count("benchmark")) {
        auto more_info = ddsim->AdditionalStatistics();
        std::cout << ddsim->getName() << ", "
                  << ddsim->getNumberOfQubits() << ", "
                  //<< vm["approximate"].as<float>() << ", "
                  << std::fixed << duration_simulation.count() << std::defaultfloat << ", "
                  //<< more_info["approximation_runs"] << ","
                  //<< more_info["final_fidelity"] << ", "
                  << more_info["coprime_a"] << ", "
                  << more_info["sim_result"] << ", "
                  << more_info["polr_result"] << ", "
                  << ddsim->getSeed() << ", "
                  << ddsim->getNumberOfOps() << ", "
                  << ddsim->getMaxNodeCount()
                  << "\n";
        return 0;
    }

    std::cout << "{\n";

    if (!m.empty()) {
        std::cout << "  \"measurements\": {";
        bool first_element = true;
        for(const auto& element : m)
        {
            std::cout << (first_element ? "" : ",") << "\n    \"" << element.first << "\": " << element.second;
            first_element = false;
        }
        std::cout << "\n  },\n";
    }
    if (vm.count("display_vector")) {
        std::cout << "  \"state_vector\": [";

        bool first_element = true;
        unsigned long long non_zero_entries = 0;
        for(const auto& element : ddsim->getVector()) {
            if (element.r != 0 || element.i != 0) {
                non_zero_entries++;
                std::cout << (first_element ? "" : ",") << "\n    " << std::showpos << element.r << element.i << "i" << std::noshowpos;
            } else {
                std::cout << (first_element ? "" : ",") << "\n    0";
            }
            first_element = false;
        }
        std::cout << "\n  ],\n";
        std::cout << "  \"non_zero_entries\": " << non_zero_entries << ",\n";
    }

    if (vm.count("ps")) {
        std::cout << "  \"statistics\": {\n"
                  << "    \"simulation_time\": " << std::fixed << duration_simulation.count() << std::defaultfloat << ",\n"
                  << "    \"benchmark\": \"" << ddsim->getName() << "\",\n"
                  << "    \"shots\": " << vm["shots"].as<unsigned int>() << ",\n"
                  << "    \"distinct_results\": " << m.size() << ",\n"
                  << "    \"n_qubits\": " << ddsim->getNumberOfQubits() << ",\n"
                  << "    \"applied_gates\": " << ddsim->getNumberOfOps() << ",\n"
                  << "    \"max_nodes\": " << ddsim->getMaxNodeCount() << ",\n"
                  ;
        for(const auto& item : ddsim->AdditionalStatistics()) {
            std::cout << "    \"" << item.first << "\": \"" << item.second << "\",\n";
        }
        std::cout << "    \"seed\": " << ddsim->getSeed() << "\n"
                  << "  },\n";
    }
    std::cout << "  \"dummy\": 0\n}\n"; // trailing element to make json printout easier
}


/**
 * 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;
}


#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 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