/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <vector>
#include <string>
#include <map>

#include <boost/lexical_cast.hpp>

#include <hashclash/saveload_bz2.hpp>
#include <hashclash/sha1detail.hpp>
#include <hashclash/sha1differentialpath.hpp>
#include <hashclash/booleanfunction.hpp>

#include "main.hpp"

using namespace hashclash;
using namespace std;

void sha1_backward_differential_step(const sha1differentialpath& path, path_container_autobalance& outpaths)
{
	const int t = int(outpaths.t);
	const unsigned& maxcond = outpaths.maxcond;
	const unsigned& maxsdrs = outpaths.maxsdrs;
	const unsigned& maxweight = outpaths.maxweight;
	const unsigned& minweight = outpaths.minweight;

	booleanfunction* F = 0;
	if (t < 20) F = &SHA1_F1_data; // this implementation only works properly for sha1_f1
	else if (t < 40) F = &SHA1_F2_data;
	else if (t < 60) F = &SHA1_F3_data;
	else if (t < 80) F = &SHA1_F4_data;

	static sha1differentialpath newpath;
	static vector<sdr> sdrs;
	static bitcondition Qtm1b[32], Qtm2b[32], Qtm3b[32];
	static vector<unsigned> bval;
	static uint32 fdiv[32];
	static bf_outcome foutcomes[32];
//	static std::vector<std::pair<uint32,double> > rotateddiff;
	static vector<sdr> deltam;
	static vector<unsigned> Qtm1prev, Qtm1prevn;

	newpath = path;
	newpath[t-4].clear();

	unsigned totprecond = 0;
	unsigned totcond = 0;
	for (int k = t; k <= outpaths.tend && k <= 80; ++k)
		if (k < newpath.tend())
			totprecond += newpath[k].hw();
	totcond = totprecond + newpath[t-1].hw() + newpath[t-2].hw();

	unsigned minextracond = 0;
	Qtm1prev.clear();
	Qtm1prevn.clear();
	bf_outcome outconstant, outplus, outminus;
	for (unsigned b = 0; b < 32; ++b)
	{
		Qtm1b[b] = newpath(t-1,b);
		if (Qtm1b[b] == bc_prev) {
			Qtm1prev.push_back(b);
			Qtm1b[b] = bc_constant;
			newpath[t-1].set(b, bc_constant);
		} else if (Qtm1b[b] == bc_prevn) {
			Qtm1prevn.push_back(b);
			Qtm1b[b] = bc_constant;
			newpath.setbitcondition(t-1,b, bc_constant);
		}
		Qtm2b[b] = newpath(t-2,(b+2)&31);
		if (b < 31) {
			outconstant = F->outcome( Qtm1b[b], Qtm2b[b], bc_constant );
			outplus = F->outcome( Qtm1b[b], Qtm2b[b], bc_plus );
			outminus = F->outcome( Qtm1b[b], Qtm2b[b], bc_minus );
		} else {
			outconstant = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], bc_constant );
			outplus = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], bc_plus );
			outminus = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], bc_minus );
		}
		if (outconstant.size() > 1 && outplus.size() > 1 && outminus.size() > 1)
			++minextracond;
	}
	sdr sdrQtm3 = newpath[t-3].getsdr();
	uint32 Qtm3_hwnaf = hwnaf(sdrQtm3.adddiff());
	if (totcond + Qtm3_hwnaf + minextracond > maxcond) return;

	/* table deltam */
	sdr mmask = outpaths.m_mask[t];
	deltam.clear();
	deltam.reserve(1<<mmask.hw());
	uint32 addmask = (~mmask.mask)+1;
	uint32 andmask = mmask.mask & 0x7FFFFFFF;
	mmask.sign = 0;
	do {
		mmask.sign += addmask; mmask.sign &= andmask;
		deltam.push_back(mmask);
	} while (mmask.sign != 0 && !outpaths.onemessagediff);
#if 1
        unsigned minhw = 32;
        for (unsigned i = 0; i < deltam.size(); ++i)
                if (hwnaf(deltam[i].adddiff()) < minhw)
                        minhw = hwnaf(deltam[i].adddiff());
        unsigned ii = 0;
        while (ii < deltam.size()) {
                if (hwnaf(deltam[ii].adddiff()) > minhw) {
                        swap(deltam[ii], deltam.back());
                        deltam.pop_back();
                } else
                        ++ii;
        }
#endif

	/* table sdrs for delta Qtm1 */
	unsigned w = Qtm3_hwnaf+1;
	if (w < minweight) w = minweight;
        unsigned mincount = 0;
        if (minweight > 0)
                mincount = count_sdrs(sdrQtm3,minweight-1,30);
	while (
			(w < 32)
			&& (w+1 <= maxweight)
			&& (totcond + w + 1 + minextracond <= maxcond)
			&& (count_sdrs(sdrQtm3, w+1,30)-mincount <= maxsdrs)
		)
		++w;
	table_sdrs(sdrs, sdrQtm3, w, 30);

	wordconditions pathQtm1 = newpath[t-1];
	wordconditions& newpathQtm1 = newpath[t-1];
	wordconditions& newpathQtm2 = newpath[t-2];
	wordconditions& newpathQtm3 = newpath[t-3];
	wordconditions& newpathQtm4 = newpath[t-4];

	uint32 dQtm4pc = newpath[t+1].diff() - newpath[t].getsdr().rotate_left(5).adddiff();

	vector<sdr>::const_iterator cit = sdrs.begin(), citend = sdrs.end();
	for (; cit != citend; ++cit)
	{
		sdrQtm3 = *cit;
		unsigned hwQtm3 = sdrQtm3.hw();
		if (hwQtm3 < minweight) continue;
		if (totcond + hwQtm3 + minextracond > maxcond)
			continue;
		newpathQtm3 = sdrQtm3;

		uint32 cnt = 1;
		uint32 dF_fixed = 0;
		unsigned maxextracond = 0;
		bval.clear();
		for (unsigned b = 0; b < 32; ++b)
		{
			Qtm3b[b] = newpathQtm3.get((b+2)&31);
			if (b < 31)
				foutcomes[b] = F->outcome( Qtm1b[b], Qtm2b[b], Qtm3b[b] );
			else
				foutcomes[b] = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], Qtm3b[b] );
			unsigned fsize = foutcomes[b].size();
			if (fsize > 1)
			{
				fdiv[b] = cnt;
				if (fsize == 2) cnt <<= 1;
				else if (fsize == 3) { cnt += cnt<<1; ++ maxextracond; }
				bval.push_back(b);
			} else
				dF_fixed += foutcomes[b](0,b);
		}
		if (totcond + hwQtm3 + bval.size() > maxcond)
			continue;
		if (outpaths.estimatefactor != 0)
		{
			outpaths.estimate(totcond + hwQtm3 + bval.size() + ((maxextracond+1)>>1), cnt*deltam.size());
			continue;
		}

		newpathQtm2 = path[t-2];

		std::reverse(bval.begin(), bval.end());
		bf_conditions newconditions;
		for (uint32 k = 0; k < cnt; ++k)
		{
			uint32 m = k;
			uint32 dF = dF_fixed;
			newpathQtm1 = pathQtm1;
			for (unsigned j = 0; j < bval.size(); ++j)
			{
				const unsigned b = bval[j];
				unsigned i = 0;
				while (m >= fdiv[b])
				{
					m -= fdiv[b];
					++i;
				}
				if (b < 31)
					newconditions = F->backwardconditions( Qtm1b[b], Qtm2b[b], Qtm3b[b], foutcomes[b][i] );
				else {
					newconditions = msb_bf_backwardconditions(*F, Qtm1b[b], Qtm2b[b], Qtm3b[b], foutcomes[b][i] );
					if (msb_bf_outcome(*F, newconditions).size() > 1 || msb_bf_outcome(*F, newconditions)[0] != foutcomes[b][i]) {
						cout << endl << "[" << Qtm1b[b] << Qtm2b[b] << Qtm3b[b] <<"](" << foutcomes[b][i] <<")=>[" << newconditions.first << newconditions.second << newconditions.third << "]" << endl;
						bf_outcome tmp = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], Qtm3b[b]);
						for (unsigned i = 0; i < tmp.size(); ++i)
							cout << tmp[i] << flush;
						cout << endl;
						tmp = F->outcome(Qtm1b[b], Qtm2b[b], Qtm3b[b]);
						for (unsigned i = 0; i < tmp.size(); ++i)
							cout << tmp[i] << flush;
						cout << endl;
						newconditions = F->backwardconditions( Qtm1b[b], Qtm2b[b], Qtm3b[b], bc_constant );
						cout << "(.)=>[" << newconditions.first << newconditions.second << newconditions.third << "]" << endl;
						throw;
					}
				}
				newpathQtm1.set(b, newconditions.first);
				newpathQtm2.set((b+2)&31, newconditions.second);
				newpathQtm3.set((b+2)&31, newconditions.third);

				dF += foutcomes[b](i,b);
			}

			bool contradiction = false;
			for (unsigned i = 0; i < Qtm1prev.size(); ++i) {
				const unsigned b = Qtm1prev[i];
				const bitcondition bcQtm1b = newpathQtm1[b];
				switch (bcQtm1b) {
				case bc_constant:
					newpathQtm1.set(b, bc_prev);
					break;
				case bc_one:
				case bc_zero:
					if (newpathQtm2[b] == bc_prev) {
						newpathQtm3.set(b, bcQtm1b);
					} else if (newpathQtm2[b] == bc_prevn) {
						if (bcQtm1b == bc_one)
							newpathQtm3.set(b, bc_zero);
						else
							newpathQtm3.set(b, bc_one);
					} else if (newpathQtm2[b] != bc_constant && newpathQtm2[b] != bcQtm1b) {
						//cerr << "[^" << newpathQtm1[b] << newpathQtm2[b] << newpathQtm3[b] << "]" << flush;
						contradiction = true;
					}
					newpathQtm2.set(b, bcQtm1b);
					break;
				default:
					cerr << "[^" << newpathQtm1[b] << newpathQtm2[b] << newpathQtm3[b] << "]" << flush;
					contradiction = true;
				}
			}
			for (unsigned i = 0; i < Qtm1prevn.size(); ++i) {
				const unsigned b = Qtm1prevn[i];
				const bitcondition bcQtm1b = newpathQtm1[b];
				switch (bcQtm1b) {
				case bc_constant:
					newpathQtm1.set(b, bc_prevn);
					break;
				case bc_one:
				case bc_zero:
					if (newpathQtm2[b] == bc_prev) {
						if (bcQtm1b == bc_one)
							newpathQtm3.set(b, bc_zero);
						else
							newpathQtm3.set(b, bc_one);
					} else if (newpathQtm2[b] == bc_prevn) {
						newpathQtm3.set(b, bcQtm1b);
					} else if (newpathQtm2[b] != bc_constant && newpathQtm2[b] == bcQtm1b) {
						//cerr << "[^" << newpathQtm1[b] << newpathQtm2[b] << newpathQtm3[b] << "]" << flush;
						contradiction = true;
					}
					if (bcQtm1b == bc_one)
						newpathQtm2.set(b, bc_zero);
					else
						newpathQtm2.set(b, bc_one);
					break;
				default:
					cerr << "[^" << newpathQtm1[b] << newpathQtm2[b] << newpathQtm3[b] << "]" << flush;
					contradiction = true;
				}
			}
			if (contradiction)
				continue;

			unsigned ncond = totprecond + newpathQtm1.hw() + newpathQtm2.hw() + newpathQtm3.hw();
			if (ncond > maxcond) return;
			for (unsigned i = 0; i < deltam.size(); ++i)
			{
				newpath.getme(t) = deltam[i];
				uint32 dQtm4 = dQtm4pc - dF - deltam[i].adddiff();
				newpathQtm4 = naf(dQtm4).rotate_right(30);
				unsigned ncond2 = ncond;
				if (outpaths.includenaf) {
					if (outpaths.halfnafweight)
						ncond2 += (newpathQtm4.hw()>>1);
					else
						ncond2 += newpathQtm4.hw();
				}
				if (ncond2 <= maxcond)
					outpaths.push_back(newpath, ncond2);
			}
		} // for cnt
	} // for sdrs
}


/*

Copyright (c) 2005-2020, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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 of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef BACKWARDEULERIVPODESOLVER_HPP_
#define BACKWARDEULERIVPODESOLVER_HPP_

#include "AbstractOneStepIvpOdeSolver.hpp"
#include "AbstractOdeSystemWithAnalyticJacobian.hpp"

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractOneStepIvpOdeSolver.hpp"

/**
 * A concrete one step ODE solver class that employs the backward Euler
 * method. This numerical method is implicit and hence unconditionally stable.
 */
class BackwardEulerIvpOdeSolver  : public AbstractOneStepIvpOdeSolver
{
private:
    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the abstract IVP Solver, never used directly - boost uses this.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        // This calls serialize on the base class.
        archive & boost::serialization::base_object<AbstractOneStepIvpOdeSolver>(*this);
        //archive & mSizeOfOdeSystem; - this done in save and load construct now.
        archive & mNumericalJacobianEpsilon;
        archive & mForceUseOfNumericalJacobian;
    }

    /** The number of state variables in the ODE system. */
    unsigned mSizeOfOdeSystem;

    /** The epsilon to use in calculating the numerical Jacobian of the ODE system. */
    double mNumericalJacobianEpsilon;

    /**
     * Whether to force the solver to use the numerical Jacobian even if
     * the ODE system object provides an analytical Jacobian.
     */
    bool mForceUseOfNumericalJacobian;

    /*
     * NOTE: we use (unsafe) double pointers here rather than
     * std::vectors because using std::vectors would lead to a
     * slow down by a factor of about 4.
     */

    /** Working memory : residual vector */
    double* mResidual;

    /** Working memory : Jacobian matrix */
    double** mJacobian;

    /** Working memory : update vector */
    double* mUpdate;

    /**
     * Compute the current residual.
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param timeStep  dt
     * @param time  the current time
     * @param rCurrentYValues  the current (initial) state
     * @param rCurrentGuess  current guess for the state at the next timestep
     */
    void ComputeResidual(AbstractOdeSystem* pAbstractOdeSystem,
                         double timeStep,
                         double time,
                         std::vector<double>& rCurrentYValues,
                         std::vector<double>& rCurrentGuess);

    /**
     * Compute the Jacobian of the ODE system.
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param timeStep  dt
     * @param time  the current time
     * @param rCurrentYValues  the current (initial) state
     * @param rCurrentGuess  current guess for the state at the next timestep
     */
    void ComputeJacobian(AbstractOdeSystem* pAbstractOdeSystem,
                         double timeStep,
                         double time,
                         std::vector<double>& rCurrentYValues,
                         std::vector<double>& rCurrentGuess);

    /**
     * Solve a linear system of equations to update the
     * current guess for the solution to the ODE system at
     * the next timestep.
     * Used by the method CalculateNextYValue.
     */
    void SolveLinearSystem();

    /**
     * Compute the infinity/maximum norm of a vector.
     * Used by the method CalculateNextYValue.
     *
     * @param pVector  a pointer to a vector
     * @return the vector's norm.
     */
    double ComputeNorm(double* pVector);

    /**
     * Compute the Jacobian of the ODE system numerically.
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param timeStep  dt
     * @param time  the current time
     * @param rCurrentYValues  the current (initial) state
     * @param rCurrentGuess  current guess for the state at the next timestep
     */
    void ComputeNumericalJacobian(AbstractOdeSystem* pAbstractOdeSystem,
                                  double timeStep,
                                  double time,
                                  std::vector<double>& rCurrentYValues,
                                  std::vector<double>& rCurrentGuess);

protected:

    /**
     * Calculate the solution to the ODE system at the next timestep.
     *
     * A usage example:
     *     BackwardEulerIvpOdeSolver mySolver;
     *     OdeSolution solution = mySolver.Solve(pMyOdeSystem, yInit, StartTime, EndTime, TimeStep, SamplingTime);
     *
     * @param pAbstractOdeSystem  the ODE system to solve
     * @param timeStep  dt
     * @param time  the current time
     * @param rCurrentYValues  the current (initial) state
     * @param rNextYValues  the state at the next timestep
     */
    void CalculateNextYValue(AbstractOdeSystem* pAbstractOdeSystem,
                             double timeStep,
                             double time,
                             std::vector<double>& rCurrentYValues,
                             std::vector<double>& rNextYValues);

public:

    /**
     * Constructor.
     *
     * @param sizeOfOdeSystem  the number of state variables in the ODE system
     */
    BackwardEulerIvpOdeSolver(unsigned sizeOfOdeSystem);

    /**
     * Destructor.
     */
    ~BackwardEulerIvpOdeSolver();

    /**
     * Set the epsilon to use in calculating the
     * numerical Jacobian of the ODE system.
     *
     * @param epsilon
     */
    void SetEpsilonForNumericalJacobian(double epsilon);

    /**
     * Force the solver to use the numerical Jacobian even if
     * the ODE system object provides an analytical Jacobian.
     */
    void ForceUseOfNumericalJacobian();

    /**
     * Public method used in archiving.
     *
     * @return the size of the system
     */
     unsigned GetSystemSize() const {return mSizeOfOdeSystem;};
};

#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(BackwardEulerIvpOdeSolver)

namespace boost
{
namespace serialization
{
/**
 * Allow us to not need a default constructor, by specifying how Boost should
 * instantiate a BackwardEulerIvpOdeSolver instance.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const BackwardEulerIvpOdeSolver * t, const unsigned int file_version)
{
    const unsigned system_size = t->GetSystemSize();
    ar & system_size;
}

/**
 * Allow us to not need a default constructor, by specifying how Boost should
 * instantiate a BackwardEulerIvpOdeSolver instance (using existing constructor)
 *
 * NB this constructor allocates memory for the other member variables too.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, BackwardEulerIvpOdeSolver * t, const unsigned int file_version)
{
     unsigned ode_system_size;
     ar >> ode_system_size;
     ::new(t)BackwardEulerIvpOdeSolver(ode_system_size);
}
}
} // namespace ...

#endif /*BACKWARDEULERIVPODESOLVER_HPP_*/


//
//
// The MIT License (MIT)
//
// Copyright (c) 2015  Peter Fisk, Michael J. Wouters
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <cmath>
#include <cstring>
#include <time.h>
#include <gsl/gsl_multifit.h>

#include <boost/regex.hpp>

#include "Utility.h"

std::string Utility::trim(std::string const& str)
{
	std::size_t first = str.find_first_not_of(' ');

	// If there is no non-whitespace character, both first and last will be std::string::npos (-1)
	// There is no point in checking both, since if either doesn't work, the
	// other won't work, either.
	if(first == std::string::npos)
		return "";

	std::size_t last  = str.find_last_not_of(' ');

	return str.substr(first, last-first+1);
}

void Utility::MJDtoDate(int mjd,int *year,int *mon, int *mday, int *yday)
{
	time_t tt = (mjd - 40587)*86400;
	struct tm *utc = gmtime(&tt);
	*year = 1900 + utc->tm_year;
	*mon  = utc->tm_mon+1;
	*mday = utc->tm_mday;
	*yday = utc->tm_yday+1;
}

bool Utility::TODStrtoTOD(std::string const& todstr,int *hh,int *mm,int *ss)
{
	// Two formats OK
	// HHMMSS or HH:MM:SS
	boost::regex re1("^(\\d{2})(\\d{2})(\\d{2})$");
	boost::smatch matches;
	if (boost::regex_search(todstr,matches,re1)){
		*hh = atoi(matches[1].str().c_str()); // regex has already checked input is valid
		*mm = atoi(matches[2].str().c_str());
		*ss = atoi(matches[3].str().c_str());
		return (*hh <= 23 && *mm <= 59 && *ss <= 59);
	}
	boost::regex re2("^(\\d{2}):(\\d{2}):(\\d{2})$");
	if (boost::regex_search(todstr,matches,re2)){
		*hh = atoi(matches[1].str().c_str()); // regex has already checked input is valid
		*mm = atoi(matches[2].str().c_str());
		*ss = atoi(matches[3].str().c_str());
		return (*hh <= 23 && *mm <= 59 && *ss <= 59);
	}
	return false;
}

bool Utility::linearFit(double x[], double y[],int n,double xinterp,double *yinterp,double *c,double *m,double *rmsResidual)
{
	double sx = 0, sy = 0, sr2 = 0, xbar, ybar, sxxbary=0,sxxbarsq=0;

	for (int i=0;i<n;i++){
		sx += x[i];
		sy += y[i];
	}

	xbar = sx/n;
	ybar = sy/n;

	for (int i=0;i<n;i++){
		sxxbary += (x[i] - xbar) * y[i];
		sxxbarsq += (x[i] - xbar)*(x[i] - xbar);
	}

	// Slope and intercept
	*m = sxxbary/sxxbarsq;
	*c = ybar - *m * xbar;

	// Sum of residuals^2

	for (int i=0;i<n;i++)
		sr2 += pow(y[i] - *m * x[i] - *c,2);

	// RMS residuals
	*rmsResidual = sqrt(sr2/n);

	*yinterp = *m * xinterp + *c;

	return true;
}

bool Utility::quadFit(double x[], double y[],int n,double xinterp,double *yinterp){
	//double c,m,rmsResidual;
	//linearFit(x,y,n,xinterp,yinterp,&c,&m,&rmsResidual); // FIXME

	double chisq;
  gsl_matrix *X, *cov;
  gsl_vector *Y, *c;

	X = gsl_matrix_alloc (n, 3);
	Y = gsl_vector_alloc (n);
	c = gsl_vector_alloc (3);
	cov = gsl_matrix_alloc (3, 3);

	for (int i = 0; i < n; i++){
		gsl_matrix_set (X, i, 0, 1.0);
		gsl_matrix_set (X, i, 1, x[i]);
		gsl_matrix_set (X, i, 2, x[i]*x[i]);
		gsl_vector_set (Y, i, y[i]);
	}

	gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (n, 3);
	gsl_multifit_linear (X, Y, c, cov,&chisq, work);
	gsl_multifit_linear_free (work);

	#define C(i) (gsl_vector_get(c,(i)))

	*yinterp = C(0) + C(1)*xinterp + C(2)*xinterp*xinterp;

	gsl_matrix_free (X);
  gsl_vector_free (Y);
  gsl_vector_free (c);
  gsl_matrix_free (cov);

	return true;
}

void Utility::ECEFtoLatLonH(double X, double Y, double Z,
	double *lat, double *lon, double *ht)
{
	// Parameters for WGS84 ellipsoid
	double a = 6378137.00; // semi-major axis
	double inverse_flattening = 298.257223563;
	double latitude,longitude;
	double p=sqrt(X*X + Y*Y);
	double r=sqrt(p*p + Z*Z);
	double f=1/inverse_flattening;
	double esq=2*f-f*f;
	double u=atan2(Z/p , 1.0/(1-f+esq*a/r));

	longitude = atan2(Y,X);
	latitude = atan2(Z*(1-f) + esq * a * pow(sin(u),3) ,
		(1-f)*(p  - esq * a * pow(cos(u),3)));
	*ht = p*cos(latitude) + Z*sin(latitude) -
			a*sqrt(1-esq* pow(sin(latitude),2));

	// Convert to degrees
	*lat=latitude*180.0/M_PI;
	*lon=longitude*180.0/M_PI;

	return;
}


// Copyright Nick Thompson, 2020
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_INTERPOLATORS_CUBIC_HERMITE_HPP
#define BOOST_MATH_INTERPOLATORS_CUBIC_HERMITE_HPP
#include <memory>
#include <boost/math/interpolators/detail/cubic_hermite_detail.hpp>

namespace boost::math::interpolators {

template<class RandomAccessContainer>
class cubic_hermite {
public:
    using Real = typename RandomAccessContainer::value_type;

    cubic_hermite(RandomAccessContainer && x, RandomAccessContainer && y, RandomAccessContainer && dydx)
    : impl_(std::make_shared<detail::cubic_hermite_detail<RandomAccessContainer>>(std::move(x), std::move(y), std::move(dydx)))
    {}

    inline Real operator()(Real x) const {
        return impl_->operator()(x);
    }

    inline Real prime(Real x) const {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const cubic_hermite & m)
    {
        os << *m.impl_;
        return os;
    }

    void push_back(Real x, Real y, Real dydx)
    {
        impl_->push_back(x, y, dydx);
    }

    int64_t bytes() const
    {
        return impl_->bytes() + sizeof(impl_);
    }

    std::pair<Real, Real> domain() const
    {
        return impl_->domain();
    }

private:
    std::shared_ptr<detail::cubic_hermite_detail<RandomAccessContainer>> impl_;
};

template<class RandomAccessContainer>
class cardinal_cubic_hermite {
public:
    using Real = typename RandomAccessContainer::value_type;

    cardinal_cubic_hermite(RandomAccessContainer && y, RandomAccessContainer && dydx, Real x0, Real dx)
    : impl_(std::make_shared<detail::cardinal_cubic_hermite_detail<RandomAccessContainer>>(std::move(y), std::move(dydx), x0, dx))
    {}

    inline Real operator()(Real x) const
    {
        return impl_->operator()(x);
    }

    inline Real prime(Real x) const
    {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const cardinal_cubic_hermite & m)
    {
        os << *m.impl_;
        return os;
    }

    int64_t bytes() const
    {
        return impl_->bytes() + sizeof(impl_);
    }

    std::pair<Real, Real> domain() const
    {
        return impl_->domain();
    }

private:
    std::shared_ptr<detail::cardinal_cubic_hermite_detail<RandomAccessContainer>> impl_;
};

template<class RandomAccessContainer>
class cardinal_cubic_hermite_aos {
public:
    using Point = typename RandomAccessContainer::value_type;
    using Real = typename Point::value_type;

    cardinal_cubic_hermite_aos(RandomAccessContainer && data, Real x0, Real dx)
    : impl_(std::make_shared<detail::cardinal_cubic_hermite_detail_aos<RandomAccessContainer>>(std::move(data), x0, dx))
    {}

    inline Real operator()(Real x) const
    {
        return impl_->operator()(x);
    }

    inline Real prime(Real x) const
    {
        return impl_->prime(x);
    }

    friend std::ostream& operator<<(std::ostream & os, const cardinal_cubic_hermite_aos & m)
    {
        os << *m.impl_;
        return os;
    }

    int64_t bytes() const
    {
        return impl_->bytes() + sizeof(impl_);
    }

    std::pair<Real, Real> domain() const
    {
        return impl_->domain();
    }

private:
    std::shared_ptr<detail::cardinal_cubic_hermite_detail_aos<RandomAccessContainer>> impl_;
};

}
#endif


#pragma once
#include <Eigen/Dense>

///
/// @struct Vechicle 3D Twist in rectangular coordinate system
///
struct Twist
{
    double x;  // Vehicle Longitudinal speed
    double y;  // Vehicle Lateral Speed
    double z;
    double roll;
    double pitch;
    double yaw;

    Twist(const double& x, const double& y, const double& z, const double& roll, const double& pitch, const double& yaw) : x(x), y(y), z(z), roll(roll), pitch(pitch), yaw(yaw)
    {
    }
    Twist() : x(0), y(0), z(0), roll(0), pitch(0), yaw(0)
    {
    }

    inline Twist operator=(const Twist& p)
    {
        this->x     = p.x;
        this->y     = p.y;
        this->z     = p.z;
        this->roll  = p.roll;
        this->pitch = p.pitch;
        this->yaw   = p.yaw;
        return *this;
    }

    inline Twist operator-(const Twist& p) const
    {
        Twist diff;
        diff.x     = this->x - p.x;
        diff.y     = this->y - p.y;
        diff.z     = this->z - p.z;
        diff.roll  = this->roll - p.roll;
        diff.pitch = this->pitch - p.pitch;
        diff.yaw   = this->yaw - p.yaw;
        return diff;
    }

    inline Twist operator+(const Twist& p) const
    {
        Twist diff;
        diff.x     = this->x + p.x;
        diff.y     = this->y + p.y;
        diff.z     = this->z + p.z;
        diff.roll  = this->roll + p.roll;
        diff.pitch = this->pitch + p.pitch;
        diff.yaw   = this->yaw + p.yaw;
        return diff;
    }

    inline Twist operator+=(const Twist& p)
    {
        this->x += p.x;
        this->y += p.y;
        this->z += p.z;
        this->roll += p.roll;
        this->pitch += p.pitch;
        this->yaw += p.yaw;
        return *this;
    }

    inline Twist operator*(double d) const
    {
        Twist diff;
        diff.x     = this->x * d;
        diff.y     = this->y * d;
        diff.z     = this->z * d;
        diff.roll  = this->roll * d;
        diff.pitch = this->pitch * d;
        diff.yaw   = this->yaw * d;
        return diff;
    }

    inline Twist operator/(double d) const
    {
        Twist diff;
        diff.x     = this->x / d;
        diff.y     = this->y / d;
        diff.z     = this->z / d;
        diff.roll  = this->roll / d;
        diff.pitch = this->pitch / d;
        diff.yaw   = this->yaw / d;
        return diff;
    }

    inline Twist operator/=(double d)
    {
        this->x /= d;
        this->y /= d;
        this->z /= d;
        this->roll /= d;
        this->pitch /= d;
        this->yaw /= d;
        return *this;
    }

    inline bool operator==(const Twist& p)
    {
        bool is_equal = true;
        is_equal      = is_equal && (this->x == p.x);
        is_equal      = is_equal && (this->y == p.y);
        is_equal      = is_equal && (this->z == p.z);
        is_equal      = is_equal && (this->roll == p.roll);
        is_equal      = is_equal && (this->pitch == p.pitch);
        is_equal      = is_equal && (this->yaw == p.yaw);
        return is_equal;
    }

    Eigen::Vector3d xyz() const;
    Eigen::Vector2d xy() const;
    Eigen::Vector3d rpy() const;
    Eigen::Vector3d xyyaw() const;
};

inline Eigen::Vector3d Twist::xyz() const
{
    return Eigen::Vector3d(x, y, z);
}

inline Eigen::Vector2d Twist::xy() const
{
    return Eigen::Vector2d(x, y);
}

inline Eigen::Vector3d Twist::rpy() const
{
    return Eigen::Vector3d(roll, pitch, yaw);
}

inline Eigen::Vector3d Twist::xyyaw() const
{
    return Eigen::Vector3d(x, y, yaw);
}


#include <cstdio>
#include <cstdlib>
#include <cmath>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include "ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver.h"

#include "ch_ethz_bhepp_ode_boost_NativeStepperType.h"
#include "Utilities.hpp"

using namespace boost::numeric::odeint;

typedef boost::numeric::ublas::vector<double> vector_type;
typedef double time_type;
typedef vector_type state_type;

template< class ode_system >
class stepper_wrapper {
public:

	virtual ~stepper_wrapper() { };

	virtual void initialize(const state_type& x0, const time_type t0, const time_type dt0) = 0;

	virtual std::pair< time_type, time_type > do_step(ode_system& sys) = 0;

	virtual void calc_state(const time_type t_inter, state_type& x) = 0;

	virtual const time_type current_time() = 0;

	virtual const state_type& current_state() = 0;

	virtual const time_type current_time_step() = 0;

};

template < typename base_stepper_type, typename ode_system >
class stepper_wrapper_impl : public stepper_wrapper< ode_system > {
	base_stepper_type base_stepper;

public:

	stepper_wrapper_impl(base_stepper_type base_stepper)
	: base_stepper(base_stepper) { }

	void initialize(const state_type& x0, const time_type t0, const time_type dt0) {
		base_stepper.initialize(x0, t0, dt0);
	}

	std::pair< time_type, time_type > do_step(ode_system& sys) {
		return base_stepper.do_step(sys);
	}

	void calc_state(const time_type t_inter, state_type& x) {
		base_stepper.calc_state(t_inter, x);
	}

	const time_type current_time() {
		return base_stepper.current_time();
	}

	const state_type& current_state() {
		return base_stepper.current_state();
	}

	const time_type current_time_step() {
		return base_stepper.current_time_step();
	}

};

struct ode_system_wrapper;

struct boost_solver_data {

//	dense_output_runge_kutta< runge_kutta_dopri5< state_type > > stepper;
//	runge_kutta_dopri5< state_type > stepper;
	stepper_wrapper< ode_system_wrapper >* stepper;
	ode_system_wrapper* ode_system;
//	dense_dopri5_stepper_type stepper;
    JNIEnv* env;
    jobject ode;
    jmethodID vectorFieldMethodID;
    jdouble initialStepSize;
    jdouble t;
    jdouble t1;
    jdouble* x;
    jdouble* xTmp;
    jdouble* xDot;
    state_type y;
    state_type yTmp;
    state_type yDot;
	jdouble lastStepSize;
	jdouble currentStepSize;

	void operator()(const state_type &x, state_type &dxdt, double t) {
//	    for (state_type::const_iterator it=x.begin(); it != x.end(); ++it) {
		for (int i=0; i < x.size(); i++)
	    	xTmp[i] = x[i];
	    env->CallVoidMethod(ode, vectorFieldMethodID, t);
	    if (env->ExceptionCheck() == JNI_TRUE) {
	        fprintf(stderr, "f_direct_bridge: Failed to call computeVectorField!\n");
	        return;
	    }
//	    for (state_type::const_iterator it=x.begin(); it != x.end(); ++it)
		for (int i=0; i < dxdt.size(); i++)
	    	dxdt[i] = xDot[i];
    }

};

struct ode_system_wrapper {
	boost_solver_data* data;

	ode_system_wrapper(boost_solver_data* data)
	: data(data) { }

	void operator()(const state_type &x, state_type &dxdt, double t) {
		data->operator()(x, dxdt, t);
	}
};

typedef result_of::make_dense_output<
    runge_kutta_dopri5< state_type > >::type dense_dopri5_stepper_type;
typedef bulirsch_stoer_dense_out< state_type > dense_bulirsch_stoer_stepper_type;
//typedef result_of::make_dense_output<
//    bulirsch_stoer< state_type > >::type dense_bulirsch_stoer_stepper_type;
//typedef result_of::make_dense_output<
//	rosenbrock4< double > >::type dense_rosenbrock4_stepper_type;

template class stepper_wrapper_impl< dense_dopri5_stepper_type, ode_system_wrapper >;
template class stepper_wrapper_impl< dense_bulirsch_stoer_stepper_type, ode_system_wrapper >;
//template class stepper_wrapper_impl< dense_rosenbrock4_stepper_type, ode_system_wrapper >;

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_initialize
 * Signature: (Lch/ethz/bhepp/ode/BufferOdeAdapter;Ljava/nio/DoubleBuffer;Ljava/nio/DoubleBuffer;Ljava/nio/DoubleBuffer;DDDI)J
 */
JNIEXPORT jlong JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1initialize
	(JNIEnv *env, jobject obj, jobject ode, jobject xBuffer, jobject xTmpBuffer, jobject xDotBuffer,
			jdouble initialStepSize, jdouble relTol, jdouble absTol, jint stepperType) {
    jclass odeCls = env->GetObjectClass(ode);
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to get class of ode object\n");
        return 0;
    }
    jmethodID methodID = env->GetMethodID(odeCls, "getDimensionOfVectorField", "()I");
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to get method ID of getDimensionOfVectorField\n");
        return 0;
    }
    jint jdimension = env->CallIntMethod(ode, methodID);
    state_type::size_type dimension = jdimension;
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to call getDimensionOfVectorField\n");
        return 0;
    }

    // Allocate and initialize boost_solver_data
    boost_solver_data* data = new boost_solver_data();

    // FIXME
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to allocate boost_solver_data\n");
        return 0;
    }

    switch (stepperType) {
    case ch_ethz_bhepp_ode_boost_NativeStepperType_BOOSTSTEPPERTYPE_DORMANDPRINCE5:
    {
    	dense_dopri5_stepper_type base_stepper1 = make_dense_output< runge_kutta_dopri5< state_type > >(absTol, relTol);
    	data->stepper = new stepper_wrapper_impl< dense_dopri5_stepper_type, ode_system_wrapper >(base_stepper1);
    	break;
    }
    case ch_ethz_bhepp_ode_boost_NativeStepperType_BOOSTSTEPPERTYPE_BULIRSCHSTOER:
    {
    	dense_bulirsch_stoer_stepper_type base_stepper2 = dense_bulirsch_stoer_stepper_type(absTol, relTol);
    	data->stepper = new stepper_wrapper_impl< dense_bulirsch_stoer_stepper_type, ode_system_wrapper >(base_stepper2);
    	break;
    }
//    case ch_ethz_bhepp_ode_boost_NativeStepperType_BOOSTSTEPPERTYPE_ROSENBROCK4:
//    {
//    	dense_rosenbrock4_stepper_type base_stepper3 = make_dense_output< rosenbrock4< double > >(absTol, relTol);
//    	data->stepper = new stepper_wrapper_impl< dense_rosenbrock4_stepper_type, ode_system_wrapper >(base_stepper3);
//    	break;
//    }
    default:
	{
        throw_java_exception(env, "Unknown stepper type\n");
    	delete data;
    	return 0;
	}
    }

    // FIXME
    data->ode_system = new ode_system_wrapper(data);
    data->initialStepSize = initialStepSize;

    data->env = env;
    // Acquire method IDs of callbacks and put them into boost_solver_data
    data->ode = env->NewGlobalRef(ode);
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to create global reference to ode object\n");
        delete data;
        return 0;
    }
    data->vectorFieldMethodID = env->GetMethodID(odeCls, "computeVectorField", "(D)V");
    if (env->ExceptionCheck() == JNI_TRUE) {
        fprintf(stderr, "jni_initialize: Failed to get method ID of computeVectorField\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }

    // Check capacities of direct buffer objects
    if (env->GetDirectBufferCapacity(xBuffer) < dimension) {
        throw_java_exception(env, "jni_initialize: Direct buffer xBuffer is not big enough\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    if (env->GetDirectBufferCapacity(xTmpBuffer) < dimension) {
        throw_java_exception(env, "jni_initialize: Direct buffer xTmpBuffer is not big enough\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    if (env->GetDirectBufferCapacity(xDotBuffer) < dimension) {
        throw_java_exception(env, "jni_initialize: Direct buffer xDotBuffer is not big enough\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    // Get addresses of direct buffer objects
    data->x = static_cast<jdouble*>(env->GetDirectBufferAddress(xBuffer));
    if (data->x == NULL) {
        fprintf(stderr, "jni_initialize: Failed to get direct address of xBuffer\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    data->xTmp = static_cast<jdouble*>(env->GetDirectBufferAddress(xTmpBuffer));
    if (data->xTmp == NULL) {
        fprintf(stderr, "jni_initialize: Failed to get direct address of xTmpBuffer\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }
    data->xDot = static_cast<jdouble*>(env->GetDirectBufferAddress(xDotBuffer));
    if (data->xDot == NULL) {
        fprintf(stderr, "jni_initialize: Failed to get direct address of xDotBuffer\n");
        // Delete global java references
        env->DeleteGlobalRef(data->ode);
        delete data;
        return 0;
    }

    data->y.resize(dimension, false);
    data->yTmp.resize(dimension, false);
    data->yDot.resize(dimension, false);

    data->lastStepSize = data->initialStepSize;
    data->currentStepSize = data->initialStepSize;

    // Pass pointer-address of boost_solver_data back to Java
    jlong jni_pointer = (jlong)data;
    return jni_pointer;
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_dispose
 * Signature: (J)V
 */
JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1dispose
  (JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = (boost_solver_data*)jni_pointer;
    if (data != NULL) {
        // Delete global java references
        if (data->ode != NULL)
            env->DeleteGlobalRef(data->ode);
        // Free data structure
        delete data;
    }
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_prepareStep
 * Signature: (JDD)V
 */
JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1prepareStep
	(JNIEnv *env, jobject obj, jlong jni_pointer, jdouble t0, jdouble t1) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    data->t = t0;
    data->t1 = t1;
    for (int i=0; i < data->y.size(); i++)
    	data->y[i] = data->x[i];
    data->stepper->initialize(data->y, data->t, data->currentStepSize);
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_getCurrentState
 * Signature: (J)D
 */
JNIEXPORT jdouble JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1getCurrentState
  (JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    const dense_dopri5_stepper_type::state_type& current_state = data->stepper->current_state();
    for (int i=0; i < data->y.size(); i++)
    	data->xTmp[i] = current_state[i];
    return data->stepper->current_time();
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_integrateStep
 * Signature: (J)D
 */
JNIEXPORT jdouble JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1integrateStep
(JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    std::pair< double , double > times = data->stepper->do_step(*data->ode_system);
    data->lastStepSize = data->currentStepSize;
    data->currentStepSize = data->stepper->current_time_step();
    return times.second;
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_computeInterpolatedSolution
 * Signature: (JD)V
 */
JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1computeInterpolatedSolution
  (JNIEnv *env, jobject obj, jlong jni_pointer, jdouble t) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    data->stepper->calc_state(t, data->yTmp);
    for (int i=0; i < data->yTmp.size(); i++)
    	data->xTmp[i] = data->yTmp[i];
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_setCurrentStepSize
 * Signature: (JD)V
 */
JNIEXPORT void JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1setCurrentStepSize
  (JNIEnv *env, jobject obj, jlong jni_pointer, jdouble stepSize) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    data->currentStepSize = stepSize;
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_getCurrentStepSize
 * Signature: (J)D
 */
JNIEXPORT jdouble JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1getCurrentStepSize
  (JNIEnv *env, jobject obj, jlong jni_pointer) {
    boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
    return data->currentStepSize;
}

/*
 * Class:     ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver
 * Method:    jni_getLastStepSize
 * Signature: (J)D
 */
JNIEXPORT jdouble JNICALL Java_ch_ethz_bhepp_ode_boost_AdaptiveBoostOdeintSolver_jni_1getLastStepSize
(JNIEnv *env, jobject obj, jlong jni_pointer) {
  boost_solver_data* data = reinterpret_cast< boost_solver_data* >(jni_pointer);
  return data->lastStepSize;
}


//
// Created by christoph on 02.10.18.
//

#include <fstream>
#include <iostream>
#include <chrono>

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

#include <GL/glew.h>

#include <Utils/Convert.hpp>
#include <Utils/File/Logfile.hpp>
#include <Math/Math.hpp>
#include <Graphics/Renderer.hpp>
#include <Graphics/Shader/ShaderManager.hpp>
#include <Graphics/Texture/TextureManager.hpp>
#include <Graphics/OpenGL/GeometryBuffer.hpp>
#include <Graphics/OpenGL/Texture.hpp>

#include "Utils/HairLoader.hpp"
#include "Utils/TrajectoryFile.hpp"
#include "VoxelCurveDiscretizer.hpp"

#define BIAS 0.001

/**
 * Helper function for rayBoxIntersection (see below).
 */
bool rayBoxPlaneIntersection(float rayOriginX, float rayDirectionX, float lowerX, float upperX,
                             float &tNear, float &tFar)
{
    if (std::abs(rayDirectionX) < BIAS) {
        // Ray is parallel to the x planes
        if (rayOriginX < lowerX || rayOriginX > upperX) {
            return false;
        }
    } else {
        // Not parallel to the x planes. Compute the intersection distance to the planes.
        float t0 = (lowerX - rayOriginX) / rayDirectionX;
        float t1 = (upperX - rayOriginX) / rayDirectionX;
        if (t0 > t1) {
            // Since t0 intersection with near plane
            float tmp = t0;
            t0 = t1;
            t1 = tmp;
        }

        if (t0 > tNear) {
            // We want the largest tNear
            tNear = t0;
        }
        if (t1 < tFar) {
            // We want the smallest tFar
            tFar = t1;
        }
        if (tNear > tFar) {
            // Box is missed
            return false;
        }
        if (tFar < 0) {
            // Box is behind ray
            return false;
        }
    }
    return true;
}

/**
 * Implementation of ray-box intersection (idea from A. Glassner et al., "An Introduction to Ray Tracing").
 * For more details see: https://www.siggraph.org//education/materials/HyperGraph/raytrace/rtinter3.htm
 */
bool rayBoxIntersection(glm::vec3 rayOrigin, glm::vec3 rayDirection, glm::vec3 lower, glm::vec3 upper,
                        float &tNear, float &tFar)
{
    tNear = -1e7;
    tFar = 1e7;
    for (int i = 0; i < 3; i++) {
        if (!rayBoxPlaneIntersection(rayOrigin[i], rayDirection[i], lower[i], upper[i], tNear, tFar)) {
            return false;
        }
    }

    //entrancePoint = rayOrigin + tNear * rayDirection;
    //exitPoint = rayOrigin + tFar * rayDirection;
    return true;
}

bool VoxelDiscretizer::addPossibleIntersections(const glm::vec3 &v1, const glm::vec3 &v2, float a1, float a2)
{
    float tNear, tFar;
    glm::vec3 voxelLower = glm::vec3(index);
    glm::vec3 voxelUpper = glm::vec3(index + glm::ivec3(1,1,1));
    if (rayBoxIntersection(v1, (v2 - v1), voxelLower, voxelUpper, tNear, tFar)) {
        bool intersectionNear = 0.0f <= tNear && tNear <= 1.0f;
        bool intersectionFar = 0.0f <= tFar && tFar <= 1.0f;
        if (intersectionNear) {
            glm::vec3 entrancePoint = v1 + tNear * (v2 - v1);
            float interpolatedAttribute = a1 + tNear * (a2 - a1);
            currentCurveIntersections.emplace_back(entrancePoint, interpolatedAttribute);
        }
        if (intersectionFar) {
            glm::vec3 exitPoint = v1 + tFar * (v2 - v1);
            float interpolatedAttribute = a1 + tFar * (a2 - a1);
            currentCurveIntersections.emplace_back(exitPoint, interpolatedAttribute);
        }
        if (intersectionNear || intersectionFar) {
            return true; // Intersection found
        }
    }

    return false;
}

void VoxelDiscretizer::setIndex(glm::ivec3 index)
{
    this->index = index;
}

float VoxelDiscretizer::computeDensity(float maxVorticity)
{
    float density = 0.0f;
    for (LineSegment &line : lines) {
        density += line.length() * line.avgOpacity(maxVorticity);
    }
    return density;
}

float VoxelDiscretizer::computeDensityHair(float opacity)
{
    float density = 0.0f;
    for (LineSegment &line : lines) {
        density += line.length() * opacity;
    }
    return density;
}

VoxelCurveDiscretizer::VoxelCurveDiscretizer(const glm::ivec3 &gridResolution, const glm::ivec3 &quantizationResolution)
        : gridResolution(gridResolution), quantizationResolution(quantizationResolution)
{
    voxels = nullptr;
}

VoxelCurveDiscretizer::~VoxelCurveDiscretizer()
{
    delete[] voxels;
}

void VoxelCurveDiscretizer::setVoxelGrid(const sgl::AABB3 &aabb)
{
    glm::vec3 gridDimensions = aabb.getDimensions();
    glm::vec3 gridResolutionCubic = gridResolution;

    float maxDimensionLength = 0.0f;
    for (int i = 0; i < 3; i++) {
        maxDimensionLength = std::max(maxDimensionLength, gridDimensions[i]);
    }
    for (int i = 0; i < 3; i++) {
        float sideLengthFactor = gridDimensions[i] / maxDimensionLength;
        gridResolution[i] = (int)std::ceil(gridResolution[i] * sideLengthFactor);
    }

    voxels = new VoxelDiscretizer[gridResolution.x * gridResolution.y * gridResolution.z];
    for (int z = 0; z < gridResolution.z; z++) {
        for (int y = 0; y < gridResolution.y; y++) {
            for (int x = 0; x < gridResolution.x; x++) {
                int index = x + y*gridResolution.x + z*gridResolution.x*gridResolution.y;
                voxels[index].setIndex(glm::ivec3(x, y, z));
            }
        }
    }
}

VoxelGridDataCompressed VoxelCurveDiscretizer::createFromTrajectoryDataset(const std::string &filename,
        TrajectoryType trajectoryType, std::vector<float> &attributes, float &_maxVorticity,
        unsigned int maxNumLinesPerVoxel, bool useGPU)
{
    linesBoundingBox = sgl::AABB3();
    std::vector<Curve> curves;
    Curve currentCurve;
    maxVorticity = 0.0f;
    isHairDataset = false;
    uint64_t numLineSegments = 0;
    uint64_t numLines = 0;

    bool isRings = boost::starts_with(filename, "Data/Rings");
    bool isConvectionRolls = boost::starts_with(filename, "Data/ConvectionRolls/output");
    bool isConvectionRollsSmall = boost::starts_with(filename, "Data/ConvectionRolls/turbulence20000");

    Trajectories trajectories = loadTrajectoriesFromFile(filename, trajectoryType);

    for (size_t i = 0; i < trajectories.size(); i++) {
        Trajectory &trajectory = trajectories.at(i);

        currentCurve = Curve();
        for (size_t j = 0; j < trajectory.positions.size(); j++) {
            glm::vec3 &position = trajectory.positions.at(j);
            linesBoundingBox.combine(position);
            currentCurve.points.push_back(position);
            currentCurve.attributes.push_back(trajectory.attributes.at(0).at(j));
        }

        currentCurve.lineID = numLines;
        curves.push_back(currentCurve);
        numLineSegments += currentCurve.points.size() - 1;
        numLines++;
    }

    std::cout << "Num Lines: " << numLines << std::endl;
    std::cout << "Num LineSegments: " << numLineSegments << std::endl << std::flush;

    /*if (isRings) {
        linesBoundingBox = sgl::AABB3();
        linesBoundingBox.combine(glm::vec3(-2));
        linesBoundingBox.combine(glm::vec3(2));
    } else if (isConvectionRolls) {
        linesBoundingBox = sgl::AABB3();
        //linesBoundingBox.combine(glm::vec3(-1.5, -0.5, -1.5));
        //linesBoundingBox.combine(glm::vec3( 1.5, 0.5, 1.5));
        linesBoundingBox.combine(glm::vec3(-1.5, -1.5, -1.5));
        linesBoundingBox.combine(glm::vec3( 1.5, 1.5, 1.5));
    } else if (isConvectionRollsSmall) {
        linesBoundingBox = sgl::AABB3();
        linesBoundingBox.combine(glm::vec3(0.0, 0.0, 0.0));
        linesBoundingBox.combine(glm::vec3(1.0, 1.0, 1.0)); // 1.0, 1.0, 0.03
    }*/

    _maxVorticity = maxVorticity;
    this->attributes = attributes;

    // Move to origin and scale to range from (0, 0, 0) to (rx, ry, rz).
    setVoxelGrid(linesBoundingBox);
    linesToVoxel = sgl::matrixScaling(1.0f / linesBoundingBox.getDimensions() * glm::vec3(gridResolution))
            * sgl::matrixTranslation(-linesBoundingBox.getMinimum());
    voxelToLines = glm::inverse(linesToVoxel);

    std::cout << "Grid resolution: " << gridResolution.x << " " << gridResolution.y
              << " " << gridResolution.z << std::endl;
    std::cout << "Bounding box: " << linesBoundingBox.getMaximum().x << " " << linesBoundingBox.getMaximum().y
              << " " << linesBoundingBox.getMaximum().z << std::endl << std::flush;

    // Transform curves to voxel grid space
    for (Curve &curve : curves) {
        for (glm::vec3 &v : curve.points) {
            v = sgl::transformPoint(linesToVoxel, v);
        }
    }

    if (!useGPU) {
        // Insert lines into voxel representation
        //int lineNum = 0;
        for (const Curve &curve : curves) {
            /*if (lineNum == 1000) {
                break;
            }*/
            nextStreamline(curve);
            //lineNum++;
        }
        return compressData();
    } else {
        return createVoxelGridGPU(curves, maxNumLinesPerVoxel);
    }
}

VoxelGridDataCompressed VoxelCurveDiscretizer::createFromHairDataset(const std::string &filename, float &lineRadius,
        glm::vec4 &hairStrandColor, unsigned int maxNumLinesPerVoxel, bool useGPU)
{
    HairData hairData;
    loadHairFile(filename, hairData);
    downscaleHairData(hairData, HAIR_MODEL_SCALING_FACTOR);

    // Assume default thickness, opacity and color for now to simplify the implementation
    lineRadius = hairData.defaultThickness;
    hairStrandColor = glm::vec4(hairData.defaultColor, hairData.defaultOpacity);
    this->hairThickness = lineRadius;
    this->hairStrandColor = hairStrandColor;
    this->hairOpacity = hairData.defaultOpacity;

    linesBoundingBox = sgl::AABB3();
    std::vector<Curve> curves;
    Curve currentCurve;
    maxVorticity = 0.0f;
    int lineCounter = 0;
    isHairDataset = true;

    // Process all strands and convert them to curves
    for (HairStrand &strand : hairData.strands) {
        lineCounter++;
        if (lineCounter % 1000 == 999) {
            sgl::Logfile::get()->writeInfo(std::string() + "Parsing hair strand " + sgl::toString(lineCounter) + "...");
        }

        for (glm::vec3 point : strand.points) {
            glm::vec3 scaledPoint = point;
            currentCurve.points.push_back(scaledPoint);
            currentCurve.attributes.push_back(this->hairOpacity);
            linesBoundingBox.combine(scaledPoint);
        }

        curves.push_back(currentCurve);
        currentCurve = Curve();
        currentCurve.lineID = lineCounter;
    }

    // Move to origin and scale to range from (0, 0, 0) to (rx, ry, rz).
    setVoxelGrid(linesBoundingBox);
    linesToVoxel = sgl::matrixScaling(1.0f / linesBoundingBox.getDimensions() * glm::vec3(gridResolution))
                   * sgl::matrixTranslation(-linesBoundingBox.getMinimum());
    voxelToLines = glm::inverse(linesToVoxel);

    // Transform curves to voxel grid space
    for (Curve &curve : curves) {
        for (glm::vec3 &v : curve.points) {
            v = sgl::transformPoint(linesToVoxel, v);
        }
    }

    if (!useGPU) {
        // Insert lines into voxel representation
        for (const Curve &curve : curves) {
            nextStreamline(curve);
        }
        return compressData();
    } else {
        return createVoxelGridGPU(curves, maxNumLinesPerVoxel);
    }
}

VoxelGridDataCompressed VoxelCurveDiscretizer::compressData()
{
    VoxelGridDataCompressed dataCompressed;
    dataCompressed.gridResolution = gridResolution;
    dataCompressed.quantizationResolution = quantizationResolution;
    dataCompressed.worldToVoxelGridMatrix = this->getWorldToVoxelGridMatrix();
    dataCompressed.dataType = isHairDataset ? 1u : 0u;

    if (isHairDataset) {
        dataCompressed.hairStrandColor = hairStrandColor;
        dataCompressed.hairThickness = hairThickness;
    } else {
        dataCompressed.attributes = attributes;
        dataCompressed.maxVorticity = maxVorticity;
    }

    int n = gridResolution.x * gridResolution.y * gridResolution.z;
    std::vector<float> voxelDensities;
    std::vector<uint32_t> usedVoxels;
    voxelDensities.reserve(n);
    usedVoxels.reserve(n);

    size_t lineOffset = 0;
    dataCompressed.voxelLineListOffsets.reserve(n);
    dataCompressed.numLinesInVoxel.reserve(n);
    dataCompressed.lineSegments.clear();

    for (int i = 0; i < n; i++) {
        dataCompressed.voxelLineListOffsets.push_back(lineOffset);
        size_t numLines = voxels[i].lines.size();
        dataCompressed.numLinesInVoxel.push_back(numLines);
        if (isHairDataset) {
            voxelDensities.push_back(voxels[i].computeDensityHair(hairOpacity));
        } else {
            voxelDensities.push_back(voxels[i].computeDensity(maxVorticity));
        }
        usedVoxels.push_back(numLines > 0 ? 1 : 0);

#ifdef PACK_LINES
        std::vector<LineSegmentCompressed> lineSegments;
        lineSegments.resize(voxels[i].lines.size());
        for (size_t j = 0; j < voxels[i].lines.size(); j++) {
            compressLine(voxels[i].getIndex(), voxels[i].lines[j], lineSegments[j]);

            // Test
            /*LineSegment originalLine = voxels[i].lines[j];
            LineSegment decompressedLine;
            decompressLine(glm::vec3(voxels[i].getIndex()), lineSegments[j], decompressedLine);
            if (!checkLinesEqual(originalLine, decompressedLine)) {
                compressLine(voxels[i].getIndex(), voxels[i].lines[j], lineSegments[j]);
                decompressLine(glm::vec3(voxels[i].getIndex()), lineSegments[j], decompressedLine);
            }*/
        }
        dataCompressed.lineSegments.insert(dataCompressed.lineSegments.end(),
                lineSegments.begin(), lineSegments.end());
#else
        dataCompressed.lineSegments.insert(dataCompressed.lineSegments.end(),
                voxels[i].lines.begin(), voxels[i].lines.end());
#endif

        lineOffset += numLines;
    }

    std::vector<float> voxelAOFactors;
    voxelAOFactors.resize(n);
    generateVoxelAOFactorsFromDensity(voxelDensities, voxelAOFactors, gridResolution, isHairDataset);

    dataCompressed.voxelDensities = voxelDensities;
    dataCompressed.voxelAOFactors = voxelAOFactors;
    return dataCompressed;
}

std::vector<VoxelDiscretizer*> VoxelCurveDiscretizer::getVoxelsInAABB(const sgl::AABB3 &aabb)
{
    std::vector<VoxelDiscretizer*> voxelsInAABB;
    glm::vec3 minimum = aabb.getMinimum();
    glm::vec3 maximum = aabb.getMaximum();

    glm::ivec3 lower = glm::ivec3(minimum); // Round down
    glm::ivec3 upper = glm::ivec3(ceil(maximum.x), ceil(maximum.y), ceil(maximum.z)); // Round up
    lower = glm::max(lower, glm::ivec3(0));
    upper = glm::min(upper, gridResolution - glm::ivec3(1));

    for (int z = lower.z; z <= upper.z; z++) {
        for (int y = lower.y; y <= upper.y; y++) {
            for (int x = lower.x; x <= upper.x; x++) {
                int index = x + y*gridResolution.x + z*gridResolution.x*gridResolution.y;
                voxelsInAABB.push_back(voxels + index);
            }
        }
    }
    return voxelsInAABB;
}

void VoxelCurveDiscretizer::nextStreamline(const Curve &line)
{
    int N = line.points.size();

    // Add intersections to voxels
    std::set<VoxelDiscretizer*> usedVoxels;
    for (int i = 0; i < N-1; i++) {
        // Get line segment
        glm::vec3 v1 = line.points.at(i);
        glm::vec3 v2 = line.points.at(i+1);
        float a1 = line.attributes.at(i);
        float a2 = line.attributes.at(i+1);

        // Remove invalid line points (used in many scientific datasets to indicate invalid lines).
        const float MAX_VAL = 1e10;
        if (std::fabs(v1.x) > MAX_VAL || std::fabs(v1.y) > MAX_VAL || std::fabs(v1.z) > MAX_VAL
                || std::fabs(v2.x) > MAX_VAL || std::fabs(v2.y) > MAX_VAL || std::fabs(v2.z) > MAX_VAL) {
            continue;
        }

        // Compute AABB of current segment
        sgl::AABB3 segmentAABB = sgl::AABB3();
        segmentAABB.combine(v1);
        segmentAABB.combine(v2);

        // Iterate over all voxels with possible intersections
        std::vector<VoxelDiscretizer*> voxelsInAABB = getVoxelsInAABB(segmentAABB);

        for (VoxelDiscretizer *voxel : voxelsInAABB) {
            // Line-voxel intersection
            if (voxel->addPossibleIntersections(v1, v2, a1, a2)) {
                // Intersection(s) added to "currentLineIntersections", voxel used
                usedVoxels.insert(voxel);
            }
        }
    }

    // Convert intersections to clipped line segments
    for (VoxelDiscretizer *voxel : usedVoxels) {
        if (voxel->currentCurveIntersections.size() < 2) {
            voxel->currentCurveIntersections.clear();
            continue;
        }
        auto it1 = voxel->currentCurveIntersections.begin();
        auto it2 = voxel->currentCurveIntersections.begin();
        it2++;
        while (it2 != voxel->currentCurveIntersections.end()) {
            voxel->lines.push_back(LineSegment(it1->v, it1->a, it2->v, it2->a, line.lineID));
            it1++; it1++;
            if (it1 == voxel->currentCurveIntersections.end()) break;
            it2++; it2++;
        }
        voxel->currentCurveIntersections.clear();
    }
}

template<typename T>
T clamp(T x, T a, T b) {
    if (x < a) {
        return a;
    } else if (x > b) {
        return b;
    } else {
        return x;
    }
}

void VoxelCurveDiscretizer::quantizeLine(const glm::vec3 &voxelPos, const LineSegment &line,
        LineSegmentQuantized &lineQuantized, int faceIndex1, int faceIndex2)
{
    lineQuantized.a1 = line.a1;
    lineQuantized.a2 = line.a2;

    glm::ivec2 facePosition3D1, facePosition3D2;
    quantizePoint(line.v1 - voxelPos, facePosition3D1, faceIndex1);
    quantizePoint(line.v2 - voxelPos, facePosition3D2, faceIndex2);
    lineQuantized.lineID = line.lineID;
    lineQuantized.faceIndex1 = faceIndex1;
    lineQuantized.faceIndex2 = faceIndex2;
    lineQuantized.facePositionQuantized1 = facePosition3D1.x + facePosition3D1.y*quantizationResolution.x;
    lineQuantized.facePositionQuantized2 = facePosition3D2.x + facePosition3D2.y*quantizationResolution.x;
}

int intlog2(int x) {
    int exponent = 0;
    while (x > 1) {
        x /= 2;
        exponent++;
    }
    return exponent;
}

void VoxelCurveDiscretizer::compressLine(const glm::ivec3 &voxelIndex, const LineSegment &line,
        LineSegmentCompressed &lineCompressed)
{
    LineSegmentQuantized lineQuantized;
    int faceIndex1 = computeFaceIndex(line.v1, voxelIndex);
    int faceIndex2 = computeFaceIndex(line.v2, voxelIndex);
    quantizeLine(glm::vec3(voxelIndex), line, lineQuantized, faceIndex1, faceIndex2);

    uint8_t attr1Unorm = std::round(lineQuantized.a1*255.0f);
    uint8_t attr2Unorm = std::round(lineQuantized.a2*255.0f);

    int c = round(2*intlog2(quantizationResolution.x));
    lineCompressed.linePosition = lineQuantized.faceIndex1;
    lineCompressed.linePosition |= lineQuantized.faceIndex2 << 3;
    lineCompressed.linePosition |= lineQuantized.facePositionQuantized1 << 6;
    lineCompressed.linePosition |= lineQuantized.facePositionQuantized2 << (6 + c);
    lineCompressed.attributes = 0;
    if (c > 12) {
        // Quantization resolution of 128 or 256
        lineCompressed.attributes |= lineQuantized.facePositionQuantized2 >> (c - (6 + 2*c - 32));
    }
    lineCompressed.attributes |= (lineQuantized.lineID & 31u) << 11;
    lineCompressed.attributes |= attr1Unorm << 16;
    lineCompressed.attributes |= attr2Unorm << 24;
}

void VoxelCurveDiscretizer::quantizePoint(const glm::vec3 &v, glm::ivec2 &qv, int faceIndex)
{
    int dimensions[2];
    if (faceIndex == 0 || faceIndex == 1) {
        // x face
        dimensions[0] = 1;
        dimensions[1] = 2;
    } else if (faceIndex == 2 || faceIndex == 3) {
        // y face
        dimensions[0] = 0;
        dimensions[1] = 2;
    } else {
        // z face
        dimensions[0] = 0;
        dimensions[1] = 1;
    }

    // Iterate over all dimensions
    for (int i = 0; i < 2; i++) {
        int quantizationPos = std::floor(v[dimensions[i]] * quantizationResolution[dimensions[i]]);
        qv[i] = glm::clamp(quantizationPos, 0, quantizationResolution[dimensions[i]]-1);
    }
}

int VoxelCurveDiscretizer::computeFaceIndex(const glm::vec3 &v, const glm::ivec3 &voxelIndex)
{
    glm::ivec3 lower = voxelIndex, upper = voxelIndex + glm::ivec3(1);
    for (int i = 0; i < 3; i++) {
        if (std::abs(v[i] - lower[i]) < 0.00001f) {
            return 2*i;
        }
        if (std::abs(v[i] - upper[i]) < 0.00001f) {
            return 2*i+1;
        }
    }
    sgl::Logfile::get()->writeError(std::string() + "Error in VoxelCurveDiscretizer::computeFaceIndex: "
            + "Invalid position.");
    return 0;
}

glm::vec3 VoxelCurveDiscretizer::getQuantizedPositionOffset(uint32_t faceIndex, uint32_t quantizedPos1D)
{
    glm::vec2 quantizedFacePosition = glm::vec2(
            float(quantizedPos1D % quantizationResolution.x),
            float(quantizedPos1D / quantizationResolution.x))
                    / float(quantizationResolution.x);

    // Whether the face is the face in x/y/z direction with greater dimensions (offset factor)
    float face0or1 = float(faceIndex % 2);

    glm::vec3 offset;
    if (faceIndex <= 1) {
        offset = glm::vec3(face0or1, quantizedFacePosition.x, quantizedFacePosition.y);
    } else if (faceIndex <= 3) {
        offset = glm::vec3(quantizedFacePosition.x, face0or1, quantizedFacePosition.y);
    } else if (faceIndex <= 5) {
        offset = glm::vec3(quantizedFacePosition.x, quantizedFacePosition.y, face0or1);
    }
    return offset;
}

void VoxelCurveDiscretizer::decompressLine(const glm::vec3 &voxelPosition, const LineSegmentCompressed &compressedLine,
        LineSegment &decompressedLine)
{
    const uint32_t c = 2*intlog2(quantizationResolution.x);
    const uint32_t bitmaskQuantizedPos = quantizationResolution.x*quantizationResolution.x-1;
    uint32_t faceStartIndex = compressedLine.linePosition & 0x7u;
    uint32_t faceEndIndex = (compressedLine.linePosition >> 3) & 0x7u;
    uint32_t quantizedStartPos1D = (compressedLine.linePosition >> 6) & bitmaskQuantizedPos;
    uint32_t quantizedEndPos1D = (compressedLine.linePosition >> 6+c) & bitmaskQuantizedPos;
    if (c > 12) {
        quantizedEndPos1D |= (compressedLine.attributes << (c - (6 + 2*c - 32))) & bitmaskQuantizedPos;
    }
    uint32_t lineID = (compressedLine.attributes >> 11) & 31u;
    uint32_t attr1 = (compressedLine.attributes >> 16) & 0xFFu;
    uint32_t attr2 = (compressedLine.attributes >> 24) & 0xFFu;

    decompressedLine.v1 = voxelPosition + getQuantizedPositionOffset(faceStartIndex, quantizedStartPos1D);
    decompressedLine.v2 = voxelPosition + getQuantizedPositionOffset(faceEndIndex, quantizedEndPos1D);
    decompressedLine.a1 = float(attr1) / 255.0f;
    decompressedLine.a2 = float(attr2) / 255.0f;
    decompressedLine.lineID = lineID;
}

bool VoxelCurveDiscretizer::checkLinesEqual(const LineSegment &originalLine, const LineSegment &decompressedLine)
{
    bool linesEqual = true;
    if (originalLine.lineID % 256 != decompressedLine.lineID) {
        linesEqual = false;
        sgl::Logfile::get()->writeError("VoxelCurveDiscretizer::checkLinesEqual: lineID");
    }

    if (std::abs(originalLine.a1 - decompressedLine.a1) > 0.01f
            || std::abs(originalLine.a2 - decompressedLine.a2) > 0.01f) {
        linesEqual = false;
        sgl::Logfile::get()->writeError("VoxelCurveDiscretizer::checkLinesEqual: attribute");
    }

    if (glm::length(originalLine.v1 - decompressedLine.v1) > 0.5f) {
        linesEqual = false;
        sgl::Logfile::get()->writeError(std::string() + "VoxelCurveDiscretizer::checkLinesEqual: position2, error: "
                + sgl::toString(glm::length(originalLine.v2 - decompressedLine.v2)));
    }

    if (glm::length(originalLine.v2 - decompressedLine.v2) > 0.5f) {
        linesEqual = false;
        sgl::Logfile::get()->writeError(std::string() + "VoxelCurveDiscretizer::checkLinesEqual: position1, error: "
                                        + sgl::toString(glm::length(originalLine.v2 - decompressedLine.v2)));
    }

    return linesEqual;
}

struct LinePoint {
    LinePoint(glm::vec3 linePoint, float lineAttribute) : linePoint(linePoint), lineAttribute(lineAttribute) {}
    glm::vec3 linePoint;
    float lineAttribute;
};

std::string ivec3ToString(const glm::ivec3 &v) {
    return std::string() + "ivec3(" + sgl::toString(v.x) + ", " + sgl::toString(v.y) + ", " + sgl::toString(v.z) + ")";
}

void VoxelCurveDiscretizer::recreateDensityAndAOFactors(VoxelGridDataCompressed &dataCompressed,
        VoxelGridDataGPU &dataGPU, unsigned int maxNumLinesPerVoxel)
{
    glm::ivec3 numWorkGroupsVoxel = glm::ivec3(sgl::iceil(gridResolution.x, 64), sgl::iceil(gridResolution.y, 4),
                                               gridResolution.z);
    uint32_t gridSize1D = gridResolution.x *gridResolution.y *gridResolution.z;
    uint32_t zeroData = 0u;
    void *bufferMemory;

    // Set preprocessor defines for the shaders.
    sgl::ShaderManager->addPreprocessorDefine("MAX_NUM_LINES_PER_VOXEL", maxNumLinesPerVoxel);
    sgl::ShaderManager->addPreprocessorDefine("gridResolution", ivec3ToString(gridResolution));
    sgl::ShaderManager->addPreprocessorDefine(
            "GRID_RESOLUTION_LOG2", sgl::toString(sgl::intlog2(gridResolution.x)));
    sgl::ShaderManager->addPreprocessorDefine("GRID_RESOLUTION", gridResolution.x);
    sgl::ShaderManager->addPreprocessorDefine("quantizationResolution", ivec3ToString(quantizationResolution));
    sgl::ShaderManager->addPreprocessorDefine("QUANTIZATION_RESOLUTION", sgl::toString(quantizationResolution.x));
    sgl::ShaderManager->addPreprocessorDefine(
            "QUANTIZATION_RESOLUTION_LOG2", sgl::toString(sgl::intlog2(quantizationResolution.x)));

    // PART 3: Compute the densities
    auto startDensity = std::chrono::system_clock::now();

    sgl::ShaderProgramPtr computeDensityShader = sgl::ShaderManager->getShaderProgram({"RecomputeDensity.Compute"});
    computeDensityShader->setUniformImageTexture(0, dataGPU.densityTexture, GL_R32F, GL_READ_WRITE, 0, true, 0);
    computeDensityShader->dispatchCompute(numWorkGroupsVoxel.x, numWorkGroupsVoxel.y, numWorkGroupsVoxel.z);
    glMemoryBarrier(GL_SHADER_IMAGE_ACCESS_BARRIER_BIT);

    auto endDensity = std::chrono::system_clock::now();
    auto elapsedDensity = std::chrono::duration_cast<std::chrono::milliseconds>(endDensity - startDensity);
    sgl::Logfile::get()->writeInfo(std::string() + "Computational time to compute the densities: "
                                   + std::to_string(elapsedDensity.count()));

    // PART 5: Compute the ambient occlusion factors on the GPU using the density texture.
    auto startAO_GPU = std::chrono::system_clock::now();

    const int FILTER_SIZE = 7;
    const int FILTER_EXTENT = (FILTER_SIZE - 1) / 2;
    const int FILTER_NUM_FIELDS = FILTER_SIZE*FILTER_SIZE*FILTER_SIZE;
    float blurKernel[FILTER_NUM_FIELDS];
    generateGaussianBlurKernel(blurKernel, FILTER_SIZE, FILTER_EXTENT);
    sgl::GeometryBufferPtr gaussianKernelBuffer = sgl::Renderer->createGeometryBuffer(
            FILTER_NUM_FIELDS * sizeof(float), &blurKernel,
            sgl::UNIFORM_BUFFER, sgl::BUFFER_STATIC);

    sgl::ShaderProgramPtr computeAOShader = sgl::ShaderManager->getShaderProgram({"ComputeAO.Compute"});
    computeAOShader->setUniformImageTexture(0, dataGPU.densityTexture, GL_R32F, GL_READ_WRITE, 0, true, 0);
    computeAOShader->setUniform("densityTexture", dataGPU.densityTexture, 0);
    sgl::ShaderManager->bindShaderStorageBuffer(6, gaussianKernelBuffer);
    computeAOShader->dispatchCompute(numWorkGroupsVoxel.x, numWorkGroupsVoxel.y, numWorkGroupsVoxel.z);
    glMemoryBarrier(GL_SHADER_IMAGE_ACCESS_BARRIER_BIT);

    auto endAO_GPU = std::chrono::system_clock::now();
    auto elapsedAO_GPU = std::chrono::duration_cast<std::chrono::milliseconds>(endAO_GPU - startAO_GPU);
    sgl::Logfile::get()->writeInfo(std::string() + "Computational time to compute the ambient occlusion factors (GPU): "
                                   + std::to_string(elapsedAO_GPU.count()));

    glUseProgram(0); // For ImGui to stop complaining when binding last_program...
}

VoxelGridDataCompressed VoxelCurveDiscretizer::createVoxelGridGPU(
        std::vector<Curve> &curves, unsigned int maxNumLinesPerVoxel)
{
    glm::ivec3 numWorkGroupsVoxel = glm::ivec3(sgl::iceil(gridResolution.x, 64), sgl::iceil(gridResolution.y, 4),
            gridResolution.z);
    uint32_t gridSize1D = gridResolution.x *gridResolution.y *gridResolution.z;
    uint32_t zeroData = 0u;
    void *bufferMemory;

    // Set preprocessor defines for the shaders.
    sgl::ShaderManager->addPreprocessorDefine("MAX_NUM_LINES_PER_VOXEL", maxNumLinesPerVoxel);
    sgl::ShaderManager->addPreprocessorDefine("gridResolution", ivec3ToString(gridResolution));
    sgl::ShaderManager->addPreprocessorDefine(
            "GRID_RESOLUTION_LOG2", sgl::toString(sgl::intlog2(gridResolution.x)));
    sgl::ShaderManager->addPreprocessorDefine("GRID_RESOLUTION", gridResolution.x);
    sgl::ShaderManager->addPreprocessorDefine("quantizationResolution", ivec3ToString(quantizationResolution));
    sgl::ShaderManager->addPreprocessorDefine("QUANTIZATION_RESOLUTION", sgl::toString(quantizationResolution.x));
    sgl::ShaderManager->addPreprocessorDefine(
            "QUANTIZATION_RESOLUTION_LOG2", sgl::toString(sgl::intlog2(quantizationResolution.x)));
    if (isHairDataset) {
        sgl::ShaderManager->addPreprocessorDefine("HAIR_RENDERING", maxNumLinesPerVoxel);
    } else {
        sgl::ShaderManager->removePreprocessorDefine("HAIR_RENDERING");
    }

    // PART 1: Create the LinePointBuffer, LineOffsetBuffer, NumSegmentsBuffer (empty) and LineSegmentsBuffer.
    auto startBuffers = std::chrono::system_clock::now();
    std::vector<LinePoint> linePoints;
    std::vector<uint32_t> lineOffsets;
    lineOffsets.push_back(0);
    uint32_t offsetCounter = 0;
    for (Curve &curve : curves) {
        size_t curveNumPoints = curve.points.size();
        for (size_t i = 0; i < curveNumPoints; i++) {
            linePoints.push_back(LinePoint(curve.points.at(i), curve.attributes.at(i)));
        }
        offsetCounter += curveNumPoints;
        lineOffsets.push_back(offsetCounter);
    }
    sgl::GeometryBufferPtr linePointBuffer = sgl::Renderer->createGeometryBuffer(
            (linePoints.size()+1) * sizeof(LinePoint), &linePoints.front(),
            sgl::SHADER_STORAGE_BUFFER, sgl::BUFFER_STATIC);
    sgl::GeometryBufferPtr lineOffsetBuffer = sgl::Renderer->createGeometryBuffer(
            (curves.size()+1) * sizeof(uint32_t), &lineOffsets.front(),
            sgl::SHADER_STORAGE_BUFFER, sgl::BUFFER_STATIC);
    sgl::GeometryBufferPtr numSegmentsBuffer = sgl::Renderer->createGeometryBuffer(
            gridSize1D * sizeof(uint32_t),
            sgl::SHADER_STORAGE_BUFFER, sgl::BUFFER_STATIC);
    GLuint bufferID = ((sgl::GeometryBufferGL*)numSegmentsBuffer.get())->getBuffer();
    glClearNamedBufferData(bufferID, GL_R32UI, GL_RED_INTEGER, GL_UNSIGNED_INT, (const void*)&zeroData);
    sgl::GeometryBufferPtr lineSegmentsBuffer = sgl::Renderer->createGeometryBuffer(
            maxNumLinesPerVoxel * gridSize1D * sizeof(LineSegmentCompressed),
            sgl::SHADER_STORAGE_BUFFER, sgl::BUFFER_STATIC);

    auto endBuffers = std::chrono::system_clock::now();
    auto elapsedBuffers = std::chrono::duration_cast<std::chrono::milliseconds>(endBuffers - startBuffers);
    sgl::Logfile::get()->writeInfo(std::string() + "Computational time to create the buffers: "
                                   + std::to_string(elapsedBuffers.count()));

    // PART 2: Discretize, quantize and voxelize the lines.
    auto startVoxelize = std::chrono::system_clock::now();
    unsigned int numWorkGroupsLines = sgl::iceil(curves.size(), 256);
    sgl::ShaderProgramPtr discretizeLinesShader = sgl::ShaderManager->getShaderProgram({"DiscretizeLines.Compute"});
    sgl::ShaderManager->bindShaderStorageBuffer(2, linePointBuffer);
    sgl::ShaderManager->bindShaderStorageBuffer(3, lineOffsetBuffer);
    sgl::ShaderManager->bindShaderStorageBuffer(4, numSegmentsBuffer);
    sgl::ShaderManager->bindShaderStorageBuffer(5, lineSegmentsBuffer);
    discretizeLinesShader->setUniform("numLines", (unsigned int)curves.size());
    discretizeLinesShader->dispatchCompute(numWorkGroupsLines);
    glMemoryBarrier(GL_SHADER_STORAGE_BARRIER_BIT);
    glMemoryBarrier(GL_SHADER_IMAGE_ACCESS_BARRIER_BIT);

    // End of PART 2: Read the line segment buffer & number of line segments per voxel buffer back from the GPU.
    std::vector<LineSegmentCompressed> compressedLineSegments;
    bufferMemory = lineSegmentsBuffer->mapBuffer(sgl::BUFFER_MAP_READ_ONLY);
    compressedLineSegments.resize(maxNumLinesPerVoxel * gridSize1D);
    memcpy(&compressedLineSegments.front(), bufferMemory, compressedLineSegments.size() * sizeof(LineSegmentCompressed));
    lineSegmentsBuffer->unmapBuffer();

    std::vector<uint32_t> numSegmentsPerVoxel;
    bufferMemory = numSegmentsBuffer->mapBuffer(sgl::BUFFER_MAP_READ_ONLY);
    numSegmentsPerVoxel.resize(gridSize1D);
    memcpy(&numSegmentsPerVoxel.front(), bufferMemory, numSegmentsPerVoxel.size() * sizeof(uint32_t));
    numSegmentsBuffer->unmapBuffer();

    auto endVoxelize = std::chrono::system_clock::now();
    auto elapsedVoxelize = std::chrono::duration_cast<std::chrono::milliseconds>(endVoxelize - startVoxelize);
    sgl::Logfile::get()->writeInfo(std::string() + "Computational time to voxelize the lines: "
                                   + std::to_string(elapsedVoxelize.count()));

    // PART 3: Compute the densities
    auto startDensity = std::chrono::system_clock::now();

    sgl::TextureSettings densityTextureSettings = sgl::TextureSettings();
    densityTextureSettings.type = sgl::TEXTURE_3D;
    densityTextureSettings.internalFormat = GL_R32F;
    sgl::TexturePtr densityTexture = sgl::TextureManager->createEmptyTexture(
            gridResolution.x, gridResolution.y, gridResolution.z, densityTextureSettings);
    sgl::ShaderProgramPtr computeDensityShader = sgl::ShaderManager->getShaderProgram({"ComputeDensity.Compute"});
    computeDensityShader->setUniformImageTexture(0, densityTexture, GL_R32F, GL_READ_WRITE, 0, true, 0);
    computeDensityShader->dispatchCompute(numWorkGroupsVoxel.x, numWorkGroupsVoxel.y, numWorkGroupsVoxel.z);
    glMemoryBarrier(GL_SHADER_IMAGE_ACCESS_BARRIER_BIT);

    // End of PART 3: Read the density values back from the GPU.
    std::vector<float> voxelDensities;
    voxelDensities.resize(gridSize1D);
    sgl::TextureGL *densityTextureGL = (sgl::TextureGL*)densityTexture.get();
    glGetTextureImage(densityTextureGL->getTexture(), 0, GL_RED, GL_FLOAT,
            sizeof(float) * gridSize1D, (void*)&voxelDensities.front());

    auto endDensity = std::chrono::system_clock::now();
    auto elapsedDensity = std::chrono::duration_cast<std::chrono::milliseconds>(endDensity - startDensity);
    sgl::Logfile::get()->writeInfo(std::string() + "Computational time to compute the densities: "
                                   + std::to_string(elapsedDensity.count()));

    // PART 4: Reduce the size of the buffer using a prefix sum (on the CPU for now).
    auto startPrefixSum = std::chrono::system_clock::now();

    uint32_t lineSegmentOffset = 0;
    std::vector<uint32_t> lineSegmentOffsets;
    std::vector<LineSegmentCompressed> reducedLineSegmentBuffer;
    for (size_t i = 0; i < numSegmentsPerVoxel.size(); i++) {
        lineSegmentOffsets.push_back(lineSegmentOffset);
        size_t numSegmentsCurrentVoxel = numSegmentsPerVoxel.at(i);
        for (size_t j = 0; j < numSegmentsCurrentVoxel; j++) {
            reducedLineSegmentBuffer.push_back(compressedLineSegments.at(i*maxNumLinesPerVoxel+j));
        }
        lineSegmentOffset += numSegmentsCurrentVoxel;
    }

    uint32_t offsetTest = lineSegmentOffsets.back();
    LineSegmentCompressed lineSegmentTest = reducedLineSegmentBuffer.back();
    std::vector<LineSegmentCompressed> testArray = reducedLineSegmentBuffer;
    std::reverse(testArray.begin(), testArray.end());

    auto endPrefixSum = std::chrono::system_clock::now();
    auto elapsedPrefixSum = std::chrono::duration_cast<std::chrono::milliseconds>(endPrefixSum - startPrefixSum);
    sgl::Logfile::get()->writeInfo(std::string() + "Computational time to reduce the buffers: "
                                   + std::to_string(elapsedPrefixSum.count()));

    // PART 5: Compute the ambient occlusion factors. For now, do this on CPU (legacy).
    /*auto startAO_CPU = std::chrono::system_clock::now();

    std::vector<float> voxelAOFactors;
    voxelAOFactors.resize(gridSize1D);
    generateVoxelAOFactorsFromDensity(voxelDensities, voxelAOFactors, gridResolution, isHairDataset);

    auto endAO_CPU = std::chrono::system_clock::now();
    auto elapsedAO_CPU = std::chrono::duration_cast<std::chrono::milliseconds>(endAO_CPU - startAO_CPU);
    sgl::Logfile::get()->writeInfo(std::string() + "Computational time to compute the ambient occlusion factors (CPU): "
                                   + std::to_string(elapsedAO_CPU.count()));*/

    // PART 5: Compute the ambient occlusion factors on the GPU using the density texture.
    auto startAO_GPU = std::chrono::system_clock::now();
    const int FILTER_SIZE = 7;
    const int FILTER_EXTENT = (FILTER_SIZE - 1) / 2;
    const int FILTER_NUM_FIELDS = FILTER_SIZE*FILTER_SIZE*FILTER_SIZE;
    float blurKernel[FILTER_NUM_FIELDS];
    generateGaussianBlurKernel(blurKernel, FILTER_SIZE, FILTER_EXTENT);
    sgl::GeometryBufferPtr gaussianKernelBuffer = sgl::Renderer->createGeometryBuffer(
            FILTER_NUM_FIELDS * sizeof(float), &blurKernel,
            sgl::SHADER_STORAGE_BUFFER, sgl::BUFFER_STATIC);

    sgl::TextureSettings aoTextureSettings = sgl::TextureSettings();
    aoTextureSettings.type = sgl::TEXTURE_3D;
    aoTextureSettings.internalFormat = GL_R32F;
    sgl::TexturePtr aoTexture = sgl::TextureManager->createEmptyTexture(
            gridResolution.x, gridResolution.y, gridResolution.z, aoTextureSettings);
    sgl::ShaderProgramPtr computeAOShader = sgl::ShaderManager->getShaderProgram({"ComputeAO.Compute"});
    computeAOShader->setUniformImageTexture(0, aoTexture, GL_R32F, GL_READ_WRITE, 0, true, 0);
    computeAOShader->setUniform("densityTexture", densityTexture, 0);
    sgl::ShaderManager->bindShaderStorageBuffer(6, gaussianKernelBuffer);
    computeAOShader->dispatchCompute(numWorkGroupsVoxel.x, numWorkGroupsVoxel.y, numWorkGroupsVoxel.z);
    glMemoryBarrier(GL_SHADER_IMAGE_ACCESS_BARRIER_BIT);

    // End of PART 5: Read the AO factors back from the GPU.
    std::vector<float> voxelAOFactors;
    voxelAOFactors.resize(gridSize1D);
    sgl::TextureGL *aoTextureGL = (sgl::TextureGL*)aoTexture.get();
    glGetTextureImage(aoTextureGL->getTexture(), 0, GL_RED, GL_FLOAT,
                      sizeof(float) * gridSize1D, (void*)&voxelAOFactors.front());
    normalizeVoxelAOFactors(voxelAOFactors, gridResolution, isHairDataset);

    auto endAO_GPU = std::chrono::system_clock::now();
    auto elapsedAO_GPU = std::chrono::duration_cast<std::chrono::milliseconds>(endAO_GPU - startAO_GPU);
    sgl::Logfile::get()->writeInfo(std::string() + "Computational time to compute the ambient occlusion factors (GPU): "
                                   + std::to_string(elapsedAO_GPU.count()));

    glUseProgram(0); // For ImGui to stop complaining when binding last_program...

    // FINAL STEP: Now, write the data to the struct.
    VoxelGridDataCompressed dataCompressed;
    dataCompressed.gridResolution = gridResolution;
    dataCompressed.quantizationResolution = quantizationResolution;
    dataCompressed.worldToVoxelGridMatrix = this->getWorldToVoxelGridMatrix();
    dataCompressed.dataType = isHairDataset ? 1u : 0u;

    if (isHairDataset) {
        dataCompressed.hairStrandColor = hairStrandColor;
        dataCompressed.hairThickness = hairThickness;
    } else {
        dataCompressed.attributes = attributes;
        dataCompressed.maxVorticity = maxVorticity;
    }

    dataCompressed.voxelLineListOffsets = lineSegmentOffsets;
    dataCompressed.numLinesInVoxel = numSegmentsPerVoxel;
    dataCompressed.lineSegments = reducedLineSegmentBuffer;

    dataCompressed.voxelDensities = voxelDensities;
    dataCompressed.voxelAOFactors = voxelAOFactors;
    return dataCompressed;
}