/*
 * Copyright (c) 2020 Filipe Tostevin, Manon Wigbers
 *
 * This file contains the main routine that reads the simulation parameters
 * from the input file, sets up the simulation, and calls the propagation
 * routines.
 */

#include <iostream>
#include <cmath>
#include <cstdlib>
#include <boost/property_tree/ini_parser.hpp>
#include "parameters.h"
#include "propagator.h"

using namespace std;

bool WITH_DATA=false;
bool WITH_SWITCH=false;

int main(
	int argc,
	char **argv
){
	int opt;
	while ((opt = getopt(argc, argv, "ts")) != -1) {
		switch (opt) {
			case 't':
				WITH_DATA=true;
				cout << "Outputting trajectories" << endl;
				break;
			case 's':
				WITH_SWITCH=true;
				cout << "Outputting switching times" << endl;
				break;
			default: /* '?' */
				cerr << "Usage: " << argv[0] << " [-t] [-s]" << endl;
				cerr << "       -t : Output trajectories" << endl;
				cerr << "       -s : Output switching times" << endl;
				exit(EXIT_FAILURE);
		}
	}

	boost::property_tree::ptree P;
	try{
		boost::property_tree::read_ini("params.in",P);
	} catch(...) {
		cerr << "Input file not found: params.in" << endl;
		exit(EXIT_FAILURE);
	}
	const int Nrep=P.get<int>("N_rep");                               // Number of realizations to run
	const double Xt_basal=P.get<double>("X_basal");                   // Xt level before and after the signal
	const double t_burnin=P.get<double>("t_burnin");                  // Time at which signal is applied
	const double t_total=P.get<double>("t_total");                    // Total simulation time
	const double N=P.get<double>("copy_number");                      // Inverse noise level (particle number)
	const int PMOD=P.get<int>("modified_parameter");                  // Index of reaction that is modified by X
	const bool down=P.get<bool>("down");                              // Whether X decreases rate

	const double Xt_signal=P.get<double>("signal.X_signal");          // Xt level during signal
	const double t_signal=P.get<double>("signal.duration");           // Signal duration
	const string profile=P.get<string>("signal.profile");
	double Xrate;                                                     // rate at which XT changes

	if(PMOD>=NPARAMS-2) {
		cerr << "Modified parameter is out of range, PMOD=" << PMOD << ">" << NPARAMS-3 << endl;
		exit(EXIT_FAILURE);
	}

	//RDE parameters
	params_array base_params;
	base_params[k_rA]=400;
	base_params[k_a]=2;
	base_params[k_ba]=400;
	base_params[k_B]=2;
	base_params[k_bB]=30;
	base_params[k_b]=2.8;
	base_params[k_M]=0.3;
	base_params[k_ab]=0.5*30;
	base_params[k_R]=0.1;
	base_params[k_bR]=1.5;
	base_params[k_r]=0.4;
	base_params[k_X]=20;
	base_params[k_x]=3;

	propagator p(PMOD, down, WITH_DATA, WITH_SWITCH);

	if(profile == "linear") {
		const double t_change=P.get<double>("signal.transition_time");
		if(t_change==0) {
			p.set_Xt_func(Xt_step);
			cout << "Using step Xt profile" << endl;
		} else {
			Xrate=(Xt_signal-Xt_basal)/t_change;
			p.set_Xt_func(Xt_linear);
			cout << "Using linear Xt profile" << endl;
		}
	} else {
		if(profile == "step") {
			p.set_Xt_func(Xt_step);
			cout << "Using step Xt profile" << endl;
		} else {
			if(profile == "exp") {
				Xrate=P.get<double>("signal.decay_rate");
				p.set_Xt_func(Xt_exp);
				cout << "Using exponential Xt profile" << endl;
			} else {
				cout << "Unknown Xt profile" << endl;
				exit(EXIT_FAILURE);
			}
		}
	}
	ofstream omegas;
	omegas.open("omegas.txt");

	//Loop over realizations
	for(int n=0; n<Nrep; ++n) {
		if(p.output_data) {
			if(n<100) {
				p.data.open("data.txt." + to_string(n));
				p.data << "t A1 A2 At B1 B2 Bt R1 R2 Rt X1 X2 Xt" << endl;
			} else {
				p.output_data=false;
			}
		}
		if(p.output_switches) {
			p.switches.open("switches.txt." + to_string(n));
		}

		//initialize all polar concentrations at ~0.3*total
		state conc;
		conc[A][TOTAL]=1.;
		conc[A][LEFT]=0.35*conc[A][TOTAL];
		conc[A][RIGHT]=0.3*conc[A][TOTAL];
		conc[B][TOTAL]=1.;
		conc[B][LEFT]=0.35*conc[B][TOTAL];
		conc[B][RIGHT]=0.3*conc[B][TOTAL];
		conc[R][TOTAL]=1.;
		conc[R][LEFT]=0.35*conc[R][TOTAL];
		conc[R][RIGHT]=0.3*conc[R][TOTAL];
		conc[X][TOTAL]=Xt_basal;
		conc[X][LEFT]=0.35*conc[X][TOTAL];
		conc[X][RIGHT]=0.3*conc[X][TOTAL];

		double t=0;

		//Run until beginning of signal period
		p.propagate(conc, base_params, t, t_burnin, N, 0., Xt_basal);

		//Calculate omegas before signal is applied
		for(int i=0; i<NSPEC; ++i) {
			omegas << calculate_omega(conc,i) << " ";
		}

		//Run for the duration of the signal
		p.propagate(conc, base_params, t, t_burnin+t_signal, N, Xrate, Xt_signal);

		//Calculate omegas at the end of the signal period
		for(int i=0; i<NSPEC; ++i) {
			omegas << calculate_omega(conc,i) << " ";
		}

		//Run until the end of the simulation time
		p.propagate(conc, base_params, t, t_total, N, -Xrate, Xt_basal);

		//Calculate the final omegas
		for(int i=0; i<NSPEC; ++i) {
			omegas << calculate_omega(conc,i) << " ";
		} omegas << endl;

		if(p.output_switches) {
			p.switches.close();
		}
		if(p.output_data) {
			p.data.close();
		}
	}

	omegas.close();
	return EXIT_SUCCESS;
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <string>
#include <algorithm>
#include <iterator>
#include <numeric>
#include <ext/numeric>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

#include <stdlib.h>
#include <math.h>
#include <assert.h>

#include "mpi_kmeans.h"

namespace po = boost::program_options;

static uInt count_lines(const std::string& filename) {
	std::ifstream in(filename.c_str());
	if (in.fail()) {
		std::cerr << "count_lines, failed to open \"" << filename << "\"."
				<< std::endl;
		exit(EXIT_FAILURE);
	}
	uInt lines = 0;
	std::string line;
	while (in.eof() == false) {
		std::getline(in, line);
		if (line.size() == 0)
			continue;
		lines += 1;
	}
	in.close();
	return (lines);
}

static void write_cluster_centers(const std::string& output_filename,
		const std::vector<std::vector<PREC> >& data_CX) {

	std::cout << "Writing cluster centers to \"" << output_filename << "\""
			<< std::endl;

	std::ofstream wout(output_filename.c_str());
	if (wout.fail()) {
		std::cerr << "Failed to open \"" << output_filename
				<< "\" for writing." << std::endl;
		exit(EXIT_FAILURE);
	}

	wout << std::setprecision(12);
	for (uInt m = 0; m < data_CX.size(); ++m) {
		for (uInt n = 0; n < data_CX[m].size(); ++n) {
			wout << (n == 0 ? "" : " ") << data_CX[m][n];
		}
		wout << std::endl;
	}
	wout.close();

	return;
}

static void write_cluster_centers(const std::string& output_filename,
		PREC *data_CX, uInt nof_clusters, uInt dims) {

	std::cout << "Writing cluster centers to \"" << output_filename << "\""
			<< std::endl;

	std::ofstream wout(output_filename.c_str());
	if (wout.fail()) {
		std::cerr << "Failed to open \"" << output_filename
				<< "\" for writing." << std::endl;
		exit(EXIT_FAILURE);
	}

	wout << std::setprecision(12);
	uInt cntr = 0;
	for (uInt m = 0; m < nof_clusters; ++m) {
		for (uInt n = 0; n < dims; ++n) {
			wout << (n == 0 ? "" : " ") << data_CX[cntr];
			cntr += 1;
		}
		wout << std::endl;
	}
	wout.close();

	return;
}

static int read_problem_data(const std::string& train_filename, std::vector<
		std::vector<PREC> >& data_X) {
	data_X.clear();

	std::ifstream in(train_filename.c_str());
	if (in.fail()) {
		std::cerr << "Failed to open file \"" << train_filename
				<< "\" for reading." << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	std::string line;
	uInt ndims = 0;
	while (in.eof() == false) {
		std::getline(in, line);
		if (line.size() == 0)
			continue; // skip over empty lines

		// remove trailing whitespaces
		line.erase(line.find_last_not_of(" ") + 1);

		std::vector<PREC> current_data;
		std::istringstream is(line);
		while (is.eof() == false) {
			PREC value;
			is >> value;
			current_data.push_back(value);
		}

		// Ensure the same number of dimensions for each point
		if (ndims == 0)
			ndims = current_data.size();
		assert(ndims == current_data.size());
		data_X.push_back(current_data);
	}
	in.close();

	if (data_X.size() == 0) {
		std::cerr << "No points read from file \"" << train_filename << "\""
				<< std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	return (data_X.size());

}
static void Write2File(const std::string &fname, uInt *pcount, uInt size) {
	std::ofstream ofile(fname.c_str(), std::ios::out);

	if (!ofile) {
		std::cout << "\n Writing Assignments to File Failed " << fname << "\n";
	} else {
		uInt i = 0;
		for (; i < size && ofile; ++i)
			ofile << pcount[i] << std::endl;
		assert(i == size);
		ofile.close();
	}

}
static void ReadPointCount(const std::string &fname, uInt *pcount, uInt size) {
	std::ifstream ifile(fname.c_str(), std::ios::in);

	if (!ifile) {
		std::cout
				<< "\n No Count File Found So BY default 1 count for each data point \n";
	} else {
		uInt i = 0;
		for (; i < size && ifile; ++i)
			ifile >> pcount[i];
		assert(i == size);
		ifile.close();
	}
}
static int read_problem_data(const std::string& train_filename, PREC *data_X) {

	std::ifstream in(train_filename.c_str());
	if (in.fail()) {
		std::cerr << "Failed to open file \"" << train_filename
				<< "\" for reading." << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	uInt nof_points = count_lines(train_filename);
	if (nof_points == 0) {
		std::cerr << "No points read from file \"" << train_filename << "\""
				<< std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	std::string line;
	uInt ndims = 0;
	uInt cntr = 0;
	while (in.eof() == false) {
		std::getline(in, line);
		if (line.size() == 0)
			continue; // skip over empty lines

		// remove trailing whitespaces
		line.erase(line.find_last_not_of(" ") + 1);

		std::vector<PREC> current_data;
		std::istringstream is(line);
		while (is.eof() == false) {
			PREC value;
			is >> value;
			current_data.push_back(value);
		}

		// Ensure the same number of dimensions for each point
		if (ndims == 0)
			ndims = current_data.size();
		assert(ndims == current_data.size());
		if (data_X == NULL)
			data_X = (PREC *) malloc(nof_points * ndims * sizeof(PREC));

		for (uInt i = 0; i < ndims; ++i) {
			data_X[cntr] = current_data[i];
			cntr += 1;
		}
	}
	in.close();

	return (nof_points);

}

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

	std::string train_filename;
	std::string output_filename;
	int nof_clusters;
	int nof_restarts;
	int maxiter;

	// Set Program options
	po::options_description generic("Generic Options");
	generic.add_options()("help", "Produce help message")("verbose",
			"Verbose output");

	po::options_description input_options("Input/Output Options");
	input_options.add_options()("data",
			po::value<std::string>(&train_filename)->default_value("data.txt"),
			"Training file, one datum per line")("output", po::value<
			std::string>(&output_filename)->default_value("output.txt"),
			"Output file, one cluster center per line");

	po::options_description kmeans_options("K-Means Options");
	kmeans_options.add_options()("k",
			po::value<int>(&nof_clusters)->default_value(100),
			"Number of clusters to generate")("restarts", po::value<int>(
			&nof_restarts)->default_value(0),
			"Number of K-Means restarts. (0: single run)")("maxiter",
			po::value<int>(&maxiter)->default_value(0),
			"Maximum number of K-Means iterations. (0: infinity)");

	po::options_description all_options;
	all_options.add(generic).add(input_options).add(kmeans_options);
	po::variables_map vm;
	po::store(po::command_line_parser(argc, argv).options(all_options).run(),
			vm);
	po::notify(vm);

	bool verbose = vm.count("verbose");

	if (vm.count("help")) {
		std::cerr << "K-Means clustering" << std::endl;
		std::cerr << all_options << std::endl;
		std::cerr << std::endl;
		std::cerr << "Example:" << std::endl;
		std::cerr
				<< "  mpi_kmeans --k 2 --data example.txt --output clusters.txt"
				<< std::endl;
		exit(EXIT_SUCCESS);
	}

	// read in the problem
	std::cout << "Training file: " << train_filename << std::endl;
	std::vector<std::vector<PREC> > data_X; // so far kmeans does not support std::<vector>
	int nof_points = read_problem_data(train_filename, data_X);
	assert(nof_points>0);

	uInt dims = data_X[0].size(), *pcount = new uInt[nof_points];
	std::fill(pcount, pcount + nof_points, 1); // by default all points contain a 1 count...
	assert(dims>0);
	ReadPointCount("example_count.txt", pcount, nof_points);
	// convert points to PREC*
	//PREC *X = (PREC *) malloc(nof_points * dims * sizeof(PREC));
	PREC *X = new PREC[nof_points * dims];
	uInt cntr = 0;
	for (uInt m = 0; m < data_X.size(); ++m) {
		for (uInt n = 0; n < data_X[m].size(); ++n) {
			X[cntr] = data_X[m][n];
			cntr += 1;
		}
		data_X[m].clear();
	}
	data_X.clear();

	// start K-Means
	std::cout << "Starting Kmeans ..." << std::endl;
	std::cout << " ... with " << nof_points << " training points " << std::endl;
	std::cout << " ... for " << nof_clusters << " clusters " << std::endl;

	uInt *assignment = new uInt[nof_points];//(uInt *) malloc(nof_points* sizeof(uInt));
	std::fill(assignment, assignment + nof_points, 0);
	//	PREC *CX = (PREC *) calloc(nof_clusters * dims, sizeof(PREC));
	PREC *CX = new PREC[nof_clusters * dims];
	std::fill(CX, CX + nof_clusters * dims, 0);
	PREC sse = kmeans(CX, X, assignment, dims, nof_points, nof_clusters,
			maxiter, nof_restarts, pcount);
	Write2File("bestAssignement.txt", assignment, nof_points);
	//	free(X);
	delete[] X;
	assert(CX);

	std::cout << "Done!" << std::endl;
	std::cout << "Sum of Squared Error : " << sse << std::endl;

	// write the clusters
	// write_cluster_centers(output_filename,data_X);
	write_cluster_centers(output_filename, CX, nof_clusters, dims);
	delete[] CX;
	// done
	exit(EXIT_SUCCESS);
}


#include <cassert>
#include <boost/hana/assert.hpp>

#include <gem/gem.hpp>

using namespace gem;
using namespace boost::hana;

auto main(void) -> int
{
    // {
    //     auto d1 = Dim(3);
    //     auto d2 = Dim(1);

    //     auto d3 = Dim(2);
    //     auto d4 = Dim(1);
    //     auto d5 = Dim(5);

    //     auto d6 = Dim(2_c);
    //     auto d7 = Dim(1_c);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ d3);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^le^ d5);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^geq^ (d4 - d4));

    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ 2u);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ 2_u);

    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^leq^ d6);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^ge^ d7);
    // }

    // {
    //     auto d1 = Dim(3_u);
    //     auto d2 = Dim(1_c);

    //     auto d3 = Dim(2);
    //     auto d4 = Dim(1);
    //     auto d5 = Dim(5);

    //     auto d6 = Dim(2_c);
    //     auto d7 = Dim(1_c);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ d3);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^le^ d5);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^geq^ (d4 - d4));

    //     BOOST_HANA_RUNTIME_CHECK ((d1 - d2) ^eq^ 2u);
    //     BOOST_HANA_CONSTANT_CHECK((d1 - d2) ^eq^ 2_u);

    //     BOOST_HANA_CONSTANT_CHECK((d1 - d2) ^leq^ d6);
    //     BOOST_HANA_CONSTANT_CHECK((d1 - d2) ^ge^ d7);
    // }

    // {
    //     auto d1 = Dim(3_u);
    //     auto d2 = Dim(1);

    //     auto d3 = Dim(2);
    //     auto d4 = Dim(1);
    //     auto d5 = Dim(5);

    //     auto d6 = Dim(2_c);
    //     auto d7 = Dim(1_c);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ d3);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^le^ d5);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^geq^ (d4 - d4));

    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ 2u);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ 2_u);

    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^leq^ d6);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^ge^ d7);
    // }

    // {
    //     auto d1 = Dim(3);
    //     auto d2 = Dim(1_c);

    //     auto d3 = Dim(2);
    //     auto d4 = Dim(1);
    //     auto d5 = Dim(5);

    //     auto d6 = Dim(2_c);
    //     auto d7 = Dim(1_c);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ d3);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^le^ d5);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^geq^ (d4 - d4));

    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ 2u);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^eq^ 2_u);

    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^leq^ d6);
    //     BOOST_HANA_RUNTIME_CHECK((d1 - d2) ^ge^ d7);
    // }
}


//
//  Copyright © 2015 Claus Christmann <hcc |ä| gatech.edu>.
//
//  Licensed under the Apache License, Version 2.0 (the "License");
//  you may not use this file except in compliance with the License.
//  You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
//

#include "Geometry/geometry.h"

#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>
#include <boost/geometry/strategies/cartesian/distance_projected_point.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/range/iterator_range.hpp>

#include <boost/numeric/conversion/bounds.hpp>

#include <assert.h>
// #include <osapi/osapi_ntptime.h>
#include <amg.hpp>

#include "XSD/xmltools.h"

#include <QtCore/qglobal.h>
#include <QtCore/QDebug>

namespace Geometry {

Coordinates_ECEF geodetic2ecef(const Coordinates_Geodetic& geoCoords)
{
  Q_ASSERT(AMG::CoSy::getECEF()!=nullptr );

  using AMG::GIS::geodetic2ecef;
  using AMG::Units::degree2radian;

  auto lambda = get<0>(geoCoords);
  auto phi = get<1>(geoCoords);
  auto h = get<2>(geoCoords);

  double x,y,z;
  geodetic2ecef(degree2radian(lambda),degree2radian(phi),h, x,y,z);

//   // debug information
//   Coordinates_ECEF ecefCoords(x,y,z);
//   std::cout << geoCoords << " -> " << ecefCoords << std::endl;

  return Coordinates_ECEF(x,y,z);
}

void geodetic2ecef(const Coordinates_Geodetic& geoCoords, Coordinates_ECEF& ecefCoords)
{
  ecefCoords = geodetic2ecef(geoCoords);
}

Coordinates_Datum ecef2datum(const Coordinates_ECEF& ecefCoords)
{
  Q_ASSERT(AMG::CoSy::getDatumFrame()!=nullptr);

  AMG::Vector ecefVector(get<0>(ecefCoords),
                         get<1>(ecefCoords),
                         get<2>(ecefCoords),
                         AMG::CoSy::getECEF() );

  AMG::CoordinateTupel datumTupel = ecefVector.absoluteCoordsIn(AMG::CoSy::getDatumFrame());

//   // debug output
//   Coordinates_Datum datumCoords(datumTupel(0),datumTupel(1),datumTupel(2));
//   std::cout << ecefCoords << " -> " << datumCoords << std::endl;

  return Coordinates_Datum(datumTupel(0),  // x-coordinate
                           datumTupel(1),  // y-coordinate
                           datumTupel(2)); // z-coordinate`

}

Box_2D boundingBox ( const Polygon_2D& polygon )
{
  Box_2D box;
  boost::geometry::envelope(polygon,box);
  return box;
}

Point_2D closestPointOnLine(Point_2D const& p, Point_2D const& a, Point_2D const& b,
                            bool truncateAtA, bool truncateAtB )
{

  double dist_ab = boost::geometry::distance(a,b);

  // Missusing a point to store the unit "vector" from a to p.
  Point_2D n( (b.x()-a.x())/dist_ab ,
              (b.y()-a.y())/dist_ab );

  // a minus p
  Point_2D amp(a.x()-p.x(),a.y()-p.y());

  double ampDotN = amp.x()*n.x()+amp.y()*n.y();

  Point_2D closest( a.x() - ampDotN*n.x() ,
                    a.y() - ampDotN*n.y() );

  if( truncateAtA
      and( (closest.x()-a.x())/n.x() < 0  or (closest.y()-a.y())/n.y() < 0 )
    )
  {
//     qDebug() << "Truncation at A kicked in.";
    return a;
  }

  if( truncateAtB
      and( (closest.x()-b.x())/n.x() > 0  or (closest.y()-b.y())/n.y() > 0 )
    )
  {
//     qDebug() << "Truncation at B kicked in.";
    return b;
  }

  return closest;
}

Point_2D closestBoundaryPoint(const Point_2D& point, const Polygon_2D& polygon)
{
  return closestPointOnRing(point, polygon.outer()); ///< \todo Implement the case where point is inside a hole of the polygon
}

Point_2D closestPointOnRing(const Point_2D& point, const Polygon_2D::ring_type& ring)
{
  Q_ASSERT(ring.size() > 2);

  using namespace boost;
  using Ring = Polygon_2D::ring_type;

  double min_comparable_dist = numeric::bounds<double>::highest();
  Point_2D closestOnRing;

  Ring::const_iterator i_corner,i_before;
  for(  i_corner = ++ring.begin(); i_corner != ring.end(); ++i_corner)
  {
    i_before = i_corner-1;

    Point_2D closestOnLine = closestPointOnLine(point,*i_corner,*i_before,
                                      /*truncateAtA*/ true,
                                      /*truncateAtB*/ true);

    double distanceToLine = boost::geometry::comparable_distance(point, closestOnLine);

    if( distanceToLine < min_comparable_dist )
    {
      closestOnRing = closestOnLine;
      min_comparable_dist = distanceToLine;
    }

  }

  // NOTE: Rings are assumed to be closed (otherwise they would be LineStrings),
  // the question is simply how this is represented in the underlying collection
  // of points of the ring!
  if( geometry::closure< Ring >::value == geometry::open ) //  i.e. first_point != last_point, i.e. we have to manually wrap around.
  {
    Point_2D closestOnLine = closestPointOnLine(point,*(ring.end()-1),*ring.begin(),
                                      /*truncateAtA*/ true,
                                      /*truncateAtB*/ true);

    double distanceToLine = boost::geometry::comparable_distance(point, closestOnLine);

    if( distanceToLine < min_comparable_dist )
    {
      closestOnRing = closestOnLine;
      min_comparable_dist = distanceToLine;
    }
  };

  return closestOnRing;
}

Point_2D kmlStringToPoint(const std::string& coordinateString, const std::string& coordinateSeperator)
{
  return kmlStringToPoint(QString(coordinateString.c_str()),
                          QString(coordinateSeperator.c_str()));
}

Point_2D kmlStringToPoint(const QString& coordinateString, const QString& coordinateSeperator)
{
  auto coordinateList = QString(coordinateString).split(coordinateSeperator);
  Point_2D point(coordinateList[0].toDouble(), // latitude in degrees
                 coordinateList[1].toDouble());// longitude in degrees
  return point;
}

/** \todo Incorporate the altitude reference in the KML node */
Coordinates_Geodetic kmlStringToPointCoordinate(const QString& coordinateString, const QString& coordinateSeperator)
{
  auto coordinateList = QString(coordinateString).split(coordinateSeperator);
  Coordinates_Geodetic coordTuple(coordinateList[0].toDouble(), // longitude in degrees
                                  coordinateList[1].toDouble(), // latitude in degrees
                                  coordinateList[2].toDouble());// altitude in meters
  return coordTuple;
}

Coordinates_Geodetic kmlStringToPointCoordinate(const std::string& coordinateString, const std::string& coordinateSeperator)
{
  return kmlStringToPointCoordinate(QString(coordinateString.c_str()),
                                    QString(coordinateSeperator.c_str()));
}

Polygon_2D kmlStringToPolygon(const std::string& coordinateString,
                              const std::string& pointSeperator,
                              const std::string& coordinateSeperator  )
{
  return kmlStringToPolygon(QString(coordinateString.c_str()),
                            QString(pointSeperator.c_str()),
                            QString(coordinateSeperator.c_str()) );
}

Polygon_2D kmlStringToPolygon(const QString& coordinatString, const QString& pointSeperator, const QString& coordinateSeperator)
{
  Polygon_2D polygon;

  auto pointStringList = QString(coordinatString).split(pointSeperator);
  QString pointCoordinateString;
  foreach( pointCoordinateString, pointStringList)
  {
    boost::geometry::append(polygon, kmlStringToPoint(pointCoordinateString,coordinateSeperator));
  }

  boost::geometry::correct(polygon);
  return polygon;

}

Polygon_2D::ring_type getBoostEqualSimplified(Polygon_2D::ring_type const& original
                                             ,double const& zeroAreaThreshold )
{
  using Ring = Polygon_2D::ring_type;

  Ring simplified = original;
  boost::geometry::correct(simplified);

  bool redoLoopSearch = false;

  do
  {
    redoLoopSearch = false;

    auto i_vertex_end = simplified.end();
    int iv_count = 0;
    for( auto i_vertex = simplified.begin(); i_vertex != i_vertex_end; ++i_vertex )
    {
      int ic_count = iv_count+1;
      for( auto i_compare = i_vertex+1; i_compare != i_vertex_end; ++i_compare)
      {
        if(*i_vertex == *i_compare)
        {

          // create a new ring comprised of all the vertices from i_vertex to
          // i_compare
          auto loopVertices = boost::make_iterator_range(i_vertex,i_compare+1);
          Ring loop;
          boost::geometry::append(loop,loopVertices);

// DEBUG
//           std::cout << "Found a loop: vertex " << iv_count
//             << " is equal to vertex " << ic_count
//             << " (total #vertices: "<< simplified.size() << ")" << std::endl;
//           std::cout << "The loop has " << loopVertices.size()
//             << " vertices which span an area of " << boost::geometry::area(loop) << std::endl;
//           if(i_vertex == simplified.begin() and i_compare== simplified.end()-1)
//           { std::cout << "(But that might be the complete ring.)"<<std::endl;}

          if( boost::geometry::area(loop) <= zeroAreaThreshold )
          {
            // if the area of that loop is zero(-ish), remove it from simplified
            // and restart the process

            Ring temp;
            boost::geometry::append(temp, boost::make_iterator_range(simplified.begin(), i_vertex+1) );
            boost::geometry::append(temp, boost::make_iterator_range(i_compare+1,simplified.end())   );
            boost::geometry::correct(temp);

            simplified = temp;
//             std::cout << "Restarting loop detection..." << std::endl;
            redoLoopSearch = true;
            break;
          }
          else if( loopVertices.size() < simplified.size() ) // this check ensures that 'complete ring' loops are skipped
          {
            // check if the loop made from the vertices _not_ in the detected
            // loop has a zero-ish area

            Ring temp;
            boost::geometry::append(temp, boost::make_iterator_range(i_compare, simplified.end())   );
            boost::geometry::append(temp, boost::make_iterator_range(simplified.begin(),i_vertex+1) );
            boost::geometry::correct(temp);

            if( boost::geometry::area(temp) < zeroAreaThreshold )
            {
              simplified = loop;
//               std::cout << "Restarting loop detection..." << std::endl;
              redoLoopSearch = true;
              break;
            }
          }
        }

        ++ic_count;
      }
      if( redoLoopSearch )
      { break; }
      iv_count++;
    }
  }while(redoLoopSearch);

  return simplified;
}

//############################################################################//
//                                                                            //
//  I/O Support                                                               //
//                                                                            //
//############################################################################//

QString toQString(Geometry::Coordinates_Geodetic const & coords)
{
  return QString() + "("
    + boost::geometry::get<0>(coords) + "°, "
    + boost::geometry::get<1>(coords) + "°, "
    + boost::geometry::get<2>(coords) + " m)";
}

QString toQString(Geometry::Coordinates_ECEF const & coords)
{
  return QString() + "("
    + boost::geometry::get<0>(coords) + " m, "
    + boost::geometry::get<1>(coords) + " m, "
    + boost::geometry::get<2>(coords) + " m)";
}

} // end namespace Geometry


#include <boost/math/tools/real_cast.hpp>


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/range/combine.hpp>
#include <boost/tuple/tuple.hpp>
#include <cstddef>
#include <optional>

#include "DataStructures/Index.hpp"
#include "DataStructures/SliceIterator.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TypeTraits.hpp"

// @{
/*!
 * \ingroup DataStructuresGroup
 * \brief Slices the data within `volume_tensor` to a codimension 1 slice. The
 * slice has a constant logical coordinate in direction `sliced_dim`,
 * slicing the volume at `fixed_index` in that dimension.  For
 * example, to get the lower boundary of `sliced_dim`, pass `0` for
 * `fixed_index`; to get the upper boundary, pass
 * `extents[sliced_dim] - 1`.
 *
 * \see add_slice_to_data
 *
 * Returns Tensor class sliced to a hypersurface.
 */
template <std::size_t VolumeDim, typename VectorType, typename... Structure>
void data_on_slice(
    const gsl::not_null<Tensor<VectorType, Structure...>*> interface_tensor,
    const Tensor<VectorType, Structure...>& volume_tensor,
    const Index<VolumeDim>& element_extents, const size_t sliced_dim,
    const size_t fixed_index) noexcept {
  const size_t interface_grid_points =
      element_extents.slice_away(sliced_dim).product();
  if (interface_tensor->begin()->size() != interface_grid_points) {
    *interface_tensor = Tensor<VectorType, Structure...>(interface_grid_points);
  }

  for (SliceIterator si(element_extents, sliced_dim, fixed_index); si; ++si) {
    for (decltype(auto) interface_and_volume_tensor_components :
         boost::combine(*interface_tensor, volume_tensor)) {
      boost::get<0>(interface_and_volume_tensor_components)[si.slice_offset()] =
          boost::get<1>(
              interface_and_volume_tensor_components)[si.volume_offset()];
    }
  }
}

template <std::size_t VolumeDim, typename VectorType, typename... Structure>
Tensor<VectorType, Structure...> data_on_slice(
    const Tensor<VectorType, Structure...>& volume_tensor,
    const Index<VolumeDim>& element_extents, const size_t sliced_dim,
    const size_t fixed_index) noexcept {
  Tensor<VectorType, Structure...> interface_tensor(
      element_extents.slice_away(sliced_dim).product());
  data_on_slice(make_not_null(&interface_tensor), volume_tensor,
                element_extents, sliced_dim, fixed_index);
  return interface_tensor;
}

template <std::size_t VolumeDim, typename VectorType, typename... Structure>
void data_on_slice(
    const gsl::not_null<std::optional<Tensor<VectorType, Structure...>>*>
        interface_tensor,
    const std::optional<Tensor<VectorType, Structure...>>& volume_tensor,
    const Index<VolumeDim>& element_extents, const size_t sliced_dim,
    const size_t fixed_index) noexcept {
  if (volume_tensor.has_value()) {
    if (not(*interface_tensor)) {
      *interface_tensor = Tensor<VectorType, Structure...>{
          element_extents.slice_away(sliced_dim).product()};
    }
    data_on_slice(make_not_null(&**interface_tensor), *volume_tensor,
                  element_extents, sliced_dim, fixed_index);
  } else {
    *interface_tensor = std::nullopt;
  }
}

template <std::size_t VolumeDim, typename VectorType, typename... Structure>
std::optional<Tensor<VectorType, Structure...>> data_on_slice(
    const std::optional<Tensor<VectorType, Structure...>>& volume_tensor,
    const Index<VolumeDim>& element_extents, const size_t sliced_dim,
    const size_t fixed_index) noexcept {
  if (volume_tensor.has_value()) {
    Tensor<VectorType, Structure...> interface_tensor(
        element_extents.slice_away(sliced_dim).product());
    data_on_slice(make_not_null(&interface_tensor), *volume_tensor,
                  element_extents, sliced_dim, fixed_index);
    return interface_tensor;
  }
  return std::nullopt;
}
// @}


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>
#include <vector>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/container/vector.hpp>

namespace compute = boost::compute;

// this example demonstrates how to print the values in a vector
int main()
{
    // get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);
    std::cout << "device: " << gpu.name() << std::endl;

    // create vector on the device and fill with the sequence 1..10
    compute::vector<int> vector(10, context);
    compute::iota(vector.begin(), vector.end(), 1, queue);

//[print_vector_example
    std::cout << "vector: [ ";
    boost::compute::copy(
        vector.begin(), vector.end(),
        std::ostream_iterator<int>(std::cout, ", "),
        queue
    );
    std::cout << "]" << std::endl;
//]

    return 0;
}


#ifndef GUIDANCE_CONTROLLER
#define GUIDANCE_CONTROLLER

#include "auv_core/auv_core_headers.hpp"
#include "auv_core/eigen_ros.hpp"
#include "auv_control/auv_lqr.hpp"

#include "auv_guidance/basic_trajectory.hpp"
#include "auv_guidance/waypoint.hpp"

#include "auv_msgs/SixDoF.h"
#include "auv_msgs/Thrust.h"
#include "auv_msgs/Trajectory.h"
#include "auv_msgs/TrajectoryGeneratorAction.h"

#include <actionlib/server/simple_action_server.h>
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include "auv_control/LQRWeightsConfig.h"

#include <yaml-cpp/yaml.h>
#include <algorithm>
#include <boost/thread.hpp>
#include <vector>
#include "math.h"

namespace acc = auv_core::constants;

namespace auv_gnc
{
class GuidanceController
{
private:
   // AUV Model Parameters
   int numActiveThrusters_;
   std::vector<std::string> activeThrusterNames_, inactiveThrusterNames_;
   std::string auvConfigFile_;
   YAML::Node auvConfig_;
   auv_core::auvParameters *auvParams_;

   // Dynamic Reconfigure Setup
   typedef dynamic_reconfigure::Server<auv_control::LQRWeightsConfig> DynamicReconfigServer;
   boost::shared_ptr<DynamicReconfigServer> paramReconfigServer_;
   DynamicReconfigServer::CallbackType paramReconfigCB_;
   boost::recursive_mutex paramReconfigMutex_;
   bool dynamicReconfigInit_;

   // LQR Parameters
   auv_control::AUVLQR *auvLQR_;
   auv_core::Matrix18d Q_Aug_;
   auv_core::Matrix8d R_;
   std::vector<double> Q_Diag_, Q_DiagIntegrator_, R_Diag_;
   bool initLQR_;
   bool enableLQRIntegrator_;
   double loopRate_;

   // Trajectory Generator Parameters
   auv_core::auvConstraints *auvConstraints_;
   auv_msgs::Trajectory desiredTrajectory_;
   auv_guidance::Waypoint *startWaypoint_, *endWaypoint_;
   auv_guidance::BasicTrajectory *basicTrajectory_;
   auv_core::Vector13d state_;
   auv_core::Vector13d ref_;
   auv_core::Vector6d accel_;
   Eigen::Vector3d linearAccel_;
   Eigen::Quaterniond quaternion_;
   int tgenType_;
   bool tgenInit_, newTrajectory_;
   ros::Time startTime_;
   auv_core::Vector8d thrust_;

   // ROS Node Parameters
   ros::NodeHandle nh_;
   ros::Subscriber sixDofSub_;
   ros::Publisher thrustPub_;
   std::string subTopic_, pubTopic_, actionName_;
   double trajectoryDuration_;
   bool resultMessageSent_;

   typedef actionlib::SimpleActionServer<auv_msgs::TrajectoryGeneratorAction> TGenActionServer;
   typedef std::shared_ptr<TGenActionServer> TGenActionServerPtr;
   TGenActionServerPtr tgenActionServer_;

   // Private Methods
   void loadAUVParams();
   void loadAUVConstraints();
   void loadAUVLQR();

   void initDynamicReconfigure();
   void updateDynamicReconfig(auv_control::LQRWeightsConfig config);
   void dynamicReconfigCB(auv_control::LQRWeightsConfig &config, uint32_t levels);

   void sixDofCB(const auv_msgs::SixDoF::ConstPtr &state);
   void tgenActionGoalCB();
   void tgenActionPreemptCB();
   bool isActionServerActive();
   bool isTrajectoryTypeValid(int type);
   void initNewTrajectory();
   void publishThrustMessage();

public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

   GuidanceController(ros::NodeHandle nh);
   void runController();
};
} // namespace auv_gnc

#endif


/**
 * @file pam.cpp
 * @date 2021-07-25
 *
 * This file contains the primary C++ implementation of the PAM code.
 *
 */
#include "pam.hpp"

#include <carma>
#include <armadillo>
#include <unordered_map>
#include <regex>

/**
 * \brief Runs naive PAM algorithm.
 *
 * Run the PAM algorithm to identify a dataset's medoids.
 *
 * @param input_data Input data to find the medoids of
 */
void PAM::fit_naive(const arma::mat& input_data) {
  data = input_data;
  data = arma::trans(data);
  arma::rowvec medoid_indices(n_medoids);
  // runs build step
  PAM::build_naive(data, medoid_indices);
  steps = 0;
  medoid_indices_build = medoid_indices;
  arma::rowvec assignments(data.n_cols);
  size_t i = 0;
  bool medoidChange = true;
  while (i < max_iter && medoidChange) {
    auto previous(medoid_indices);
    // runs swap step as necessary
    PAM::swap_naive(data, medoid_indices, assignments);
    medoidChange = arma::any(medoid_indices != previous);
    i++;
  }
  medoid_indices_final = medoid_indices;
  this->labels = assignments;
  this->steps = i;
}

/**
 * \brief Build step for the PAM algorithm
 *
 * Runs build step for the PAM algorithm. Loops over all datapoint and
 * checks its distance from every other datapoint in the dataset, then checks if
 * the total cost is less than that of the medoid (if a medoid exists yet).
 *
 * @param data Transposed input data to find the medoids of
 * @param medoid_indices Uninitialized array of medoids that is modified in place
 * as medoids are identified
 */
void PAM::build_naive(
  const arma::mat& data,
  arma::rowvec& medoid_indices) {
  size_t N = data.n_cols;
  size_t p = (buildConfidence * N); // reciprocal
  bool use_absolute = true;
  arma::rowvec estimates(N, arma::fill::zeros);
  arma::rowvec best_distances(N);
  best_distances.fill(std::numeric_limits<double>::infinity());
  arma::rowvec sigma(N); // standard deviation of induced losses on reference points
  for (size_t k = 0; k < n_medoids; k++) {
    double minDistance = std::numeric_limits<double>::infinity();
    size_t best = 0;
    sigma = km::KMedoids::build_sigma(
            data, best_distances, batchSize, use_absolute); // computes std dev amongst batch of reference points
    // fixes a base datapoint
    for (size_t i = 0; i < data.n_cols; i++) {
      double total = 0;
      for (size_t j = 0; j < data.n_cols; j++) {
        // computes distance between base and all other points
        double cost = (this->*lossFn)(data, i, j);
        // compares this with the cached best distance
        if (best_distances(j) < cost) {
          cost = best_distances(j);
        }
        total += cost;
      }
      if (total < minDistance) {
        minDistance = total;
        best = i;
      }
    }
    // update the medoid index for that of lowest cost
    medoid_indices(k) = best;

    // update the medoid assignment and best_distance for this datapoint
    for (size_t l = 0; l < N; l++) {
        double cost = (this->*lossFn)(data, l, medoid_indices(k));
        if (cost < best_distances(l)) {
            best_distances(l) = cost;
        }
    }
    use_absolute = false; // use difference of loss for sigma and sampling,
                          // not absolute
    logHelper.loss_build.push_back(minDistance/N);
    logHelper.p_build.push_back(static_cast<float>(1)/static_cast<float>(p));
    logHelper.comp_exact_build.push_back(N);
  }
}

/**
 * \brief Swap step for the PAM algorithm
 *
 * Runs build step for the PAM algorithm. Loops over all datapoint and
 * checks its distance from every other datapoint in the dataset, then checks if
 * the total cost is less than that of the medoid.
 *
 * @param data Transposed input data to find the medoids of
 * @param medoid_indices Array of medoid indices created from the build step
 * that is modified in place as better medoids are identified
 * @param assignments Uninitialized array of indices corresponding to each
 * datapoint assigned the index of the medoid it is closest to
 */
void PAM::swap_naive(
  const arma::mat& data,
  arma::rowvec& medoid_indices,
  arma::rowvec& assignments) {
  double minDistance = std::numeric_limits<double>::infinity();
  size_t best = 0;
  size_t medoid_to_swap = 0;
  size_t N = data.n_cols;
  size_t p = (N * n_medoids * swapConfidence); // reciprocal
  arma::mat sigma(n_medoids, N, arma::fill::zeros);
  arma::rowvec best_distances(N);
  arma::rowvec second_distances(N);

  // calculate quantities needed for swap, best_distances and sigma
  km::KMedoids::calc_best_distances_swap(
    data, medoid_indices, best_distances, second_distances, assignments);

  sigma = km::KMedoids::swap_sigma(data,
                                   batchSize,
                                   best_distances,
                                   second_distances,
                                   assignments);

  // write the sigma distribution to logfile
  km::KMedoids::sigma_log(sigma);

  // iterate across the current medoids
  for (size_t k = 0; k < n_medoids; k++) {
    // for every point in our dataset, let it serve as a "base" point
    for (size_t i = 0; i < data.n_cols; i++) {
      double total = 0;
      for (size_t j = 0; j < data.n_cols; j++) {
        // compute distance between base point and every other datapoint
        double cost = (this->*lossFn)(data, i, j);
        // (i) if x_j is not assigned to k: compares this with the cached best distance
        // (ii) if x_j is assigned to k: compares this with the cached second best distance
        if (assignments(j) != k) {
          if (best_distances(j) < cost) {
            cost = best_distances(j);
          }
        } else {
          if (second_distances(j) < cost) {
          cost = second_distances(j);
          }
        }
        total += cost;
      }
      // if total distance for new base point is better than that of the medoid,
      // update the best index identified so far
      if (total < minDistance) {
        minDistance = total;
        best = i;
        medoid_to_swap = k;
      }
    }
  }
  medoid_indices(medoid_to_swap) = best;
  logHelper.loss_swap.push_back(minDistance/N);
  logHelper.p_swap.push_back(static_cast<float>(1)/static_cast<float>(p));
  logHelper.comp_exact_swap.push_back(N*n_medoids);
}


/*
 *            Copyright 2009-2017 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

// Overload of uBLAS prod function with MKL/GSL implementations
#include <votca/tools/linalg.h>
#include <votca/xtp/aomatrix.h>
#include <votca/xtp/overlap.h>

#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <votca/tools/constants.h>
#include <boost/format.hpp>
#include <boost/progress.hpp>

namespace votca { namespace xtp {

namespace ub = boost::numeric::ublas;

using boost::format;

double Overlap::getCouplingElement( int levelA, int levelB,  Orbitals* _orbitalsA,
    Orbitals* _orbitalsB, ub::matrix<double>* _JAB, double  _energy_difference ) {

    int _levelsA = _orbitalsA->getNumberOfLevels();

    if ( _energy_difference != 0 ) {
        std::vector<int> list_levelsA = *_orbitalsA->getDegeneracy( levelA, _energy_difference );
        std::vector<int> list_levelsB = *_orbitalsA->getDegeneracy( levelB, _energy_difference );

        double _JAB_sq = 0; double _JAB_one_level;

        for (std::vector<int>::iterator iA = list_levelsA.begin()++; iA != list_levelsA.end(); iA++) {
                for (std::vector<int>::iterator iB = list_levelsB.begin()++; iB != list_levelsB.end(); iB++) {
                    //cout << *iA << ':' << *iB << endl;
                    _JAB_one_level = _JAB->at_element( *iA - 1  , *iB -1 + _levelsA );
                    _JAB_sq +=  _JAB_one_level*_JAB_one_level ;
                }
        }

        return sqrt(_JAB_sq / ( list_levelsA.size() * list_levelsB.size() ) ) * tools::conv::hrt2ev ;

    } else {

        return _JAB->at_element( levelA - 1  , levelB -1 + _levelsA ) * tools::conv::hrt2ev;

    }
    // the  matrix should be symmetric, could also return this element
    // _JAB.at_element( _levelsA + levelB - 1  , levelA - 1 );
}

/**
 * \brief evaluates electronic couplings
 *
 * This is a fast version with a rather large block matrix AxB
 *
 * @param _orbitalsA molecular orbitals of molecule A
 * @param _orbitalsB molecular orbitals of molecule B
 * @param _orbitalsAB molecular orbitals of the dimer AB
 * @param _JAB matrix with electronic couplings
 * @return false if failed
 */
bool Overlap::CalculateIntegrals(Orbitals* _orbitalsA, Orbitals* _orbitalsB,
    Orbitals* _orbitalsAB, ub::matrix<double>* _JAB) {

    CTP_LOG(ctp::logDEBUG,*_pLog) << "Calculating electronic couplings" << flush;

    const std::vector<ctp::QMAtom*> atomsA=_orbitalsA->QMAtoms();
    const std::vector<ctp::QMAtom*> atomsB=_orbitalsB->QMAtoms();
    const std::vector<ctp::QMAtom*> atomsAB=_orbitalsAB->QMAtoms();

  for (unsigned i=0;i<atomsAB.size();i++){
        ctp::QMAtom* dimer=atomsAB[i];
        ctp::QMAtom* monomer=NULL;
        if (i<atomsA.size()){
            monomer=atomsA[i];
        }
        else if (i<atomsB.size()+atomsA.size() ){
            monomer=atomsB[i-atomsA.size()];
        }
        else{
            throw runtime_error((format("Number of Atoms in dimer %3i and the two monomers A:%3i B:%3i does not agree") %atomsAB.size() %atomsA.size() %atomsB.size()).str());
        }

        if(monomer->type != dimer->type){
            throw runtime_error("\nERROR: Atom types do not agree in dimer and monomers\n");
        }
        if(std::abs(monomer->x-dimer->x)>0.001 || std::abs(monomer->y-dimer->y)>0.001 || std::abs(monomer->z-dimer->z)>0.001){
            CTP_LOG(ctp::logERROR,*_pLog) << "======WARNING=======\n Coordinates of monomers and dimer atoms do not agree, do you know what you are doing?\n " << flush;
            break;
        }

    }

    // constructing the direct product orbA x orbB
    int _basisA = _orbitalsA->getBasisSetSize();
    int _basisB = _orbitalsB->getBasisSetSize();

    if ( ( _basisA == 0 ) || ( _basisB == 0 ) ) {
        CTP_LOG(ctp::logERROR,*_pLog) << "Basis set size is not stored in monomers" << flush;
        return false;
    }

    int _levelsA = _orbitalsA->getNumberOfLevels();
    int _levelsB = _orbitalsB->getNumberOfLevels();

    //boost::timer t; // start timing
    //double _st = t.elapsed();

    CTP_LOG(ctp::logDEBUG,*_pLog) << "Levels:Basis A[" << _levelsA << ":" << _basisA << "]"
                                     << " B[" << _levelsB << ":" << _basisB << "]" << flush;

    if ( ( _levelsA == 0 ) || (_levelsB == 0) ) {
        CTP_LOG(ctp::logERROR,*_pLog) << "No information about number of occupied/unoccupied levels is stored" << flush;
        return false;
    }

    //       | Orbitals_A          0 |      | Overlap_A |
    //       | 0          Orbitals_B |  X   | Overlap_B |  X  Transpose( Orbitals_AB )
    ub::zero_matrix<double> zeroB( _levelsA, _basisB ) ;
    ub::zero_matrix<double> zeroA( _levelsB, _basisA ) ;
    ub::matrix<double> _psi_AxB ( _levelsA + _levelsB, _basisA + _basisB  );

    CTP_LOG(ctp::logDEBUG,*_pLog) << "Constructing direct product AxB ["
            << _psi_AxB.size1() << "x"
            << _psi_AxB.size2() << "]" << flush;

    ub::project( _psi_AxB, ub::range (0, _levelsA ), ub::range ( _basisA, _basisA +_basisB ) ) = zeroB;
    ub::project( _psi_AxB, ub::range (_levelsA, _levelsA + _levelsB ), ub::range ( 0, _basisA ) ) = zeroA;
    ub::project( _psi_AxB, ub::range (0, _levelsA ), ub::range ( 0, _basisA ) ) = _orbitalsA->MOCoefficients();
    ub::project( _psi_AxB, ub::range (_levelsA, _levelsA + _levelsB ), ub::range ( _basisA, _basisA + _basisB ) ) = _orbitalsB->MOCoefficients();

    // psi_AxB * S_AB * psi_AB

    CTP_LOG(ctp::logDEBUG,*_pLog) << "Projecting dimer onto monomer orbitals" << flush;
    ub::matrix<double> overlap;
    if ( !_orbitalsAB->hasAOOverlap() ) {
            CTP_LOG(ctp::logDEBUG,*_pLog) << "Reading overlap matrix from orbitals" << flush;
           overlap= _orbitalsAB->AOOverlap();
    }else{
        CTP_LOG(ctp::logDEBUG,*_pLog) << "Calculating overlap matrix for basisset: "<< _orbitalsAB->getDFTbasis()<< flush;
        BasisSet _dftbasisset;
        AOBasis _dftbasis;
        _dftbasisset.LoadBasisSet(_orbitalsAB->getDFTbasis());

        _dftbasis.AOBasisFill(&_dftbasisset, _orbitalsAB->QMAtoms());
        AOOverlap _dftAOoverlap;
        _dftAOoverlap.Fill(_dftbasis);
        overlap=_dftAOoverlap.Matrix();
    }

    ub::matrix<double> _psi_AB = ub::prod(overlap, ub::trans( _orbitalsAB->MOCoefficients()) );
    ub::matrix<double> _psi_AxB_dimer_basis = ub::prod( _psi_AxB, _psi_AB );
    _psi_AB.clear();

    //check to see if projection quality is sufficient
    for (unsigned i=0;i<_psi_AxB_dimer_basis.size1();i++){
        double mag=0.0;
        for (unsigned j=0;j<_psi_AxB_dimer_basis.size2();j++){
            mag+=_psi_AxB_dimer_basis(i,j)*_psi_AxB_dimer_basis(i,j);

    }
        if (mag<0.95){
            throw runtime_error("\nERROR: Projection of monomer orbitals on dimer is insufficient, maybe the orbital order is screwed up, otherwise increase dimer basis.\n");
        }
    }

    // J = psi_AxB_dimer_basis * FAB * psi_AxB_dimer_basis^T
    CTP_LOG(ctp::logDEBUG,*_pLog) << "Projecting the Fock matrix onto the dimer basis" << flush;
    ub::diagonal_matrix<double> _fock_AB( _orbitalsAB->getNumberOfLevels(), _orbitalsAB->MOEnergies().data() );
    ub::matrix<double> _temp = ub::prod( _fock_AB, ub::trans( _psi_AxB_dimer_basis ) ) ;
    ub::matrix<double> JAB_dimer = ub::prod( _psi_AxB_dimer_basis, _temp);

    // S = psi_AxB_dimer_basis * psi_AxB_dimer_basis^T
    CTP_LOG(ctp::logDEBUG,*_pLog) << "Constructing Overlap matrix" << flush;
    ub::matrix<double> _S_AxB = ub::prod( _psi_AxB_dimer_basis, ub::trans( _psi_AxB_dimer_basis ));

   CTP_LOG(ctp::logDEBUG,*_pLog) << "Calculating the effective overlap JAB ["
              << JAB_dimer.size1() << "x"
              << JAB_dimer.size2() << "]" << flush;
   double smalleig=linalg_loewdin(JAB_dimer, _S_AxB);
    CTP_LOG(ctp::logDEBUG,*_pLog) << "Smallest eigenvalue of overlap matrix is "<<smalleig<< flush;
    (*_JAB) = JAB_dimer;

    CTP_LOG(ctp::logDEBUG,*_pLog) << "Done with electronic couplings" << flush;

    return true;

}

}}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2013 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2013 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is 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_GEOMETRY_ALGORITHMS_DETAIL_POINT_IS_EQUAL_OR_SPIKE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_POINT_IS_EQUAL_OR_SPIKE_HPP

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/algorithms/detail/convert_point_to_point.hpp>
#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename Point1, typename Point2, typename Point3>
static inline bool point_is_spike_or_equal(Point1 const& last_point, Point2 const& segment_a, Point3 const& segment_b)
{
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Point1>::type
    >::type side_strategy;

    typedef Point1 vector_type;

    int const side = side_strategy::apply(last_point, segment_a, segment_b);
    if (side == 0)
    {
        // Last point is collinear w.r.t previous segment.
        // Check if it is equal
        vector_type diff1;
        conversion::convert_point_to_point(last_point, diff1);
        geometry::subtract_point(diff1, segment_b);
        int const sgn_x1 = math::sign(geometry::get<0>(diff1));
        int const sgn_y1 = math::sign(geometry::get<1>(diff1));
        if (sgn_x1 == 0 && sgn_y1 == 0)
        {
            return true;
        }

        // Check if it moves forward
        vector_type diff2;
        conversion::convert_point_to_point(segment_b, diff2);
        geometry::subtract_point(diff2, segment_a);
        int const sgn_x2 = math::sign(geometry::get<0>(diff2));
        int const sgn_y2 = math::sign(geometry::get<1>(diff2));

        return sgn_x1 != sgn_x2 || sgn_y1 != sgn_y2;
    }
    return false;
}

} // namespace detail
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_POINT_IS_EQUAL_OR_SPIKE_HPP


#include <initializer_list>
#include "../../def_submodule.hpp"

#include "IceRay/type/color.hpp"

#include <boost/python.hpp>

typedef GS_DDMRM::S_IceRay::S_type::S_color::S__internal::GT_scalar                GTs_Scalar;
typedef GS_DDMRM::S_IceRay::S_type::S_color::GT_scalar       GTs_ColorScalar;
typedef GS_DDMRM::S_IceRay::S_type::S_color::S_gray::GT_scalar   GTs_ColorGrayScalar;
typedef GS_DDMRM::S_IceRay::S_type::S_color::GT_char         GTs_ColorChar;

template < typename N_number>
 N_number getGray( GTs_ColorGrayScalar const& P_color, int P_index )
  {
   if( P_index >= 0 && P_index < P_color.size() )
    {
     return P_color[ P_index ];
    }

   PyErr_SetString( PyExc_IndexError, "index out of range" );
   return 0;
  }

template < typename N_number, typename N_component >
 N_number getRGB( ::color::rgb<N_component> const& P_color, int P_index )
  {
   if( P_index >= 0 && P_index < P_color.size() )
    {
     return P_color[ P_index ];
    }

   PyErr_SetString( PyExc_IndexError, "index out of range" );
   return 0;
  }

template < typename N_number >
void setGray( GTs_ColorGrayScalar & P_color, int P_index, N_number P_value )
 {
  if( P_index >= 0 && P_index < P_color.size())
   {
    P_color.set( P_index, P_value );
    return;
   }

   PyErr_SetString(PyExc_IndexError, "index out of range");
   return;
}

template < typename N_number >
void setRGB( GTs_ColorScalar & P_color, int P_index, N_number P_value )
 {
  if( P_index >= 0 && P_index < P_color.size())
   {
    P_color.set( P_index, P_value );
    return;
   }

   PyErr_SetString(PyExc_IndexError, "index out of range");
   return;
}

GTs_ColorScalar &
GFs_fillRGB( GTs_ColorScalar & v, GTs_Scalar x )
{
  v.set( 0, x );
  v.set( 1, x );
  v.set( 2, x );
  return v;
}

GTs_ColorGrayScalar &
GFs_fillGray( GTs_ColorGrayScalar & v, GTs_Scalar x )
{
  v.set( 0, x );
  return v;
}
GTs_ColorGrayScalar &
GFs_load1( GTs_ColorGrayScalar & v, GTs_Scalar x )
 {
  v.set( 0, x );
  return v;
 }

GTs_ColorScalar &
GFs_load3( GTs_ColorScalar & v, GTs_Scalar x, GTs_Scalar y, GTs_Scalar z )
 {
  v.set( 0, x );
  v.set( 1, y );
  v.set( 2, z );
  return v;
 }

GTs_ColorScalar &
GFs_load4( GTs_ColorScalar & v, GTs_Scalar x, GTs_Scalar y, GTs_Scalar z, GTs_Scalar t )
 {
  v.set( 0, x );
  v.set( 1, y );
  v.set( 2, z );
  v.set( 3, z );
  return v;
 }

//template < typename N_number>    mul   ( GTs_ColorScalar const& P_color ){ }
//template < typename N_number>    rmul  ( GTs_ColorScalar const& P_color ){ }
//template < typename N_number>    div   ( GTs_ColorScalar const& P_color ){ }
//template < typename N_number>    eq    ( GTs_ColorScalar const& P_color ){ }
//template < typename N_number>    ne    ( GTs_ColorScalar const& P_color ){ }
//template < typename N_number>    invert( GTs_ColorScalar const& P_color ){ }

// TODO: s * c, c * s, c / s, v+v, v-v
// Clamp color
void expose_graph_type_color()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( library   );
  MAKE_SUBMODULE( color  );

  boost::python::class_<GTs_ColorScalar>( "GraphTypeColorRGB" )
   .def( boost::python::init<>() )
    //.def( boost::python::init<GTs_Scalar>() )
    .def( boost::python::init<GTs_ColorScalar>() )

    .def( "fill",        &GFs_fillRGB, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",        &GFs_load3,   boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "load",        &GFs_load4,   boost::python::return_value_policy<boost::python::return_by_value>() )

    .def("__getitem__"  , &getRGB<GTs_Scalar,GTs_Scalar > )
    .def("__setitem__"  , &setRGB<GTs_Scalar > )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )

    .def( boost::python::self * GTs_Scalar() )
    .def( GTs_Scalar() * boost::python::self  )

    .def( boost::python::self / GTs_Scalar() )

    .def( boost::python::self /= GTs_Scalar() )
    .def( boost::python::self *= GTs_Scalar() )

    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )

    //.def("__eq__"       , &eq    <GTs_Scalar::T_value > )
    //.def("__ne__"       , &ne    <GTs_Scalar::T_value > )
    //.def("__invert__"   , &invert<TF_scalar::T_value > )
  ;

  boost::python::class_<GTs_ColorGrayScalar>( "GraphTypeColorGray" )
    .def( boost::python::init<>() )
   //.def( boost::python::init<GTs_Scalar>() )
    .def( boost::python::init<GTs_ColorGrayScalar>() )

    .def( "load",        &GFs_load1, boost::python::return_value_policy<boost::python::return_by_value>() )
    .def( "fill",        &GFs_fillGray,  boost::python::return_value_policy<boost::python::return_by_value>() )

    .def("__getitem__"  , &getGray<GTs_Scalar > )
    .def("__setitem__"  , &setGray<GTs_Scalar > )

    .def( boost::python::self + boost::python::self )
    .def( boost::python::self - boost::python::self )

    .def( boost::python::self * GTs_Scalar() )
    .def( GTs_Scalar() * boost::python::self  )

    .def( boost::python::self / GTs_Scalar() )

    .def( boost::python::self /= GTs_Scalar() )
    .def( boost::python::self *= GTs_Scalar() )

    .def( boost::python::self += boost::python::self )
    .def( boost::python::self -= boost::python::self )
 /*
    //.def("__eq__"       , &eq    <TF_scalar::T_value > )
    //.def("__ne__"       , &ne    <TF_scalar::T_value > )
    //.def("__invert__"   , &invert<TF_scalar::T_value > )   */
  ;

    //boost::python::def( "GraphTypeColor_RGB2HLS",  &GFs_);
    //boost::python::def( "GraphTypeColor_RGB2HSV",  &GFs_);
    //boost::python::def( "GraphTypeColor_RGB2CMY",  &GFs_);

    //boost::python::def( "GraphTypeColor_HSV2RGB",  &GFs_);
    //boost::python::def( "GraphTypeColor_HSV2HLS",  &GFs_);

    //boost::python::def( "GraphTypeColor_HLS2RGB",  &GFs_);
    //boost::python::def( "GraphTypeColor_HLS2HSV",  &GFs_);

    //boost::python::def( "GraphTypeColor_clamp",  &GFs_clamp);

 }


#include "viewerfrr1.h"
#include "Helpers/mesh_importer.h"
#include <QOpenGLFunctions>
#include <QtOpenGL>
#include <vector>
#include <Eigen>
#include <glm/glm.hpp>
#include <math.h>

#define GL_GLEXT_PROTOTYPES 1

//shader ID
enum {GeometryPass,SSAO,LightSSAO,BlurSSAO,SepSSAO,Blur1,Blur2};

ViewerFRR1::ViewerFRR1(const QGLFormat &glf, QWidget *parent) : HW(glf, parent) , mesh_importer(this)
{
    // init vars
    setFocusPolicy(Qt::StrongFocus);
}
void ViewerFRR1::initializeGL()
{
    // initialize GL function resolution for current context
    initializeGLFunctions();

    // init vertex and fragment shaders
    initShaders();

    glEnable(GL_NORMALIZE);
    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);
    glEnable(GL_DEPTH_TEST);

    // initialize vertex buffer and write positions to vertex shader
    initVertexBuffer();

    // init state variables
    glClearColor(1.0, 1.0, 1.0, 1.0);	// set background color
    glColor3f   (1.0, 1.0, 0.0);		// set foreground color

    if(!timerG.isCreated()){
        timerG.create();
    }

}

void ViewerFRR1::resizeGL(int w, int h)
{

    if (h == 0) h = 1;
    width_ = w;
    height_ = h;

    camera_.SetViewport(0, 0, w, h);
    camera_.SetProjection(kFieldOfView, kZNear, kZFar);

    m_winW = w;
    m_winH = h;
    initVertexBuffer();
    return;
}

void ViewerFRR1::paintGL()
{
     if(timerG.isCreated()) timerG.begin();

    glClearColor(1.0f,1.0f,1.0f,1.0f);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    if (mesh_ == nullptr) return;

    glBindFramebuffer(GL_FRAMEBUFFER, fbo);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    camera_.SetViewport();
    Eigen::Matrix4f projection =camera_.SetProjection();
    Eigen::Matrix4f view = camera_.SetView();
    Eigen::Matrix4f model = camera_.SetModel();
    const Eigen::Affine3f piRotation(Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0.0, 1.0, 0.0)));
    model = piRotation* model ;
    Eigen::Matrix4f t = view * model;
    Eigen::Matrix3f normal;

    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j) normal(i, j) = t(i, j);

    normal = normal.inverse().transpose();

    glUseProgram(m_program[GeometryPass].programId());
    glUniformMatrix4fv(const_uniform[GeometryPass][CONST_UNIFORM_PROJ], 1, GL_FALSE, projection.data());
    glUniformMatrix4fv(const_uniform[GeometryPass][CONST_UNIFORM_MODEL], 1, GL_FALSE, model.data());
    glUniformMatrix4fv(const_uniform[GeometryPass][CONST_UNIFORM_VIEW], 1, GL_FALSE, view.data());
    glUniformMatrix3fv(const_uniform[GeometryPass][CONST_UNIFORM_NORMAL], 1, GL_FALSE, normal.data());

    drawVAO();

    glBindFramebuffer(GL_FRAMEBUFFER, 0);

    if(showBlur || showSepBlur ||showLight){ glBindFramebuffer(GL_FRAMEBUFFER, fboSSAO);}//
    glClear(GL_COLOR_BUFFER_BIT);

    if(!useSepSSAO){
        glUseProgram(m_program[SSAO].programId());
        glUniform1f(glGetUniformLocation(m_program[SSAO].programId(), "height"), height_);
        glUniform1f(glGetUniformLocation(m_program[SSAO].programId(), "width"), width_);
        glUniform1f(glGetUniformLocation(m_program[SSAO].programId(), "radius"), radi);
        glUniform1f(glGetUniformLocation(m_program[SSAO].programId(), "bias"), bias);

        // Send kernel + rotation
        for (unsigned int i = 0; i < 64; ++i){
            std::string str = "samples[" + std::to_string(i) + "]";
            const char *ch=str.c_str();
            glUniform3fv(glGetUniformLocation(m_program[SSAO].programId(),ch),1,&ssaoKernel[i][0]);//,ssaoKernel[i][1],ssaoKernel[i][2]);
        }

        glActiveTexture(GL_TEXTURE0);
        glBindTexture(GL_TEXTURE_2D, texturePosition);
        glActiveTexture(GL_TEXTURE1);
        glBindTexture(GL_TEXTURE_2D, textureNormal);
        glActiveTexture(GL_TEXTURE2);
        glBindTexture(GL_TEXTURE_2D, textureNoise);
        renderQuad();
        glBindFramebuffer(GL_FRAMEBUFFER, 0);

    }else{
        glUseProgram(m_program[SepSSAO].programId());
        glUniform1f(glGetUniformLocation(m_program[SepSSAO].programId(), "height"), height_);
        glUniform1f(glGetUniformLocation(m_program[SepSSAO].programId(), "width"), width_);
        glUniform1f(glGetUniformLocation(m_program[SepSSAO].programId(), "radius"), radi);
        glUniform1f(glGetUniformLocation(m_program[SepSSAO].programId(), "bias"), bias);

        glActiveTexture(GL_TEXTURE0);
        glBindTexture(GL_TEXTURE_2D, texturePosition);
        glActiveTexture(GL_TEXTURE1);
        glBindTexture(GL_TEXTURE_2D, textureNormal);
        glActiveTexture(GL_TEXTURE2);
        glBindTexture(GL_TEXTURE_2D, textureNoise);
        renderQuad();
        glBindFramebuffer(GL_FRAMEBUFFER, 0);
    }

    if(showSSAO || showSepSSAO){
        if(timerG.isCreated()){
             timerG.end();
             resTime = timerG.waitForResult();
             if(resTime==0){return;}

             fps = 1.0/((float)resTime/1000000000.0);

             std::stringstream s_fps;
             s_fps << std::fixed << std::setprecision(1);
             s_fps << fps << " FPS" << std::ends;
             s_fps << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
             fpsString = QString::fromStdString(s_fps.str());
             m_fpsCount->setText(fpsString);
         }
        return;}

    if(showLight && !useSepBlur){ glBindFramebuffer(GL_FRAMEBUFFER, fboBlurSSAO);}//
    if(useSepBlur||showSepBlur)glBindFramebuffer(GL_FRAMEBUFFER, fboBlurSSAO1);
    glClear(GL_COLOR_BUFFER_BIT);

    if(!useSepBlur){

        glUseProgram(m_program[BlurSSAO].programId());
        glUniform1f(glGetUniformLocation(m_program[BlurSSAO].programId(), "height"), height_);
        glUniform1f(glGetUniformLocation(m_program[BlurSSAO].programId(), "width"), width_);
        glActiveTexture(GL_TEXTURE0);
        glBindTexture(GL_TEXTURE_2D, fboColorSSAO);
        renderQuad();
        glBindFramebuffer(GL_FRAMEBUFFER, 0);

    }else{
        glUseProgram(m_program[Blur1].programId());
        glUniform1f(glGetUniformLocation(m_program[Blur1].programId(), "height"), height_);
        glUniform1f(glGetUniformLocation(m_program[Blur1].programId(), "width"), width_);
        glActiveTexture(GL_TEXTURE0);
        glBindTexture(GL_TEXTURE_2D, fboColorSSAO);
        renderQuad();
        glBindFramebuffer(GL_FRAMEBUFFER, 0);

        if(showLight)glBindFramebuffer(GL_FRAMEBUFFER, fboBlurSSAO2);
        glClear(GL_COLOR_BUFFER_BIT);

        glUseProgram(m_program[Blur2].programId());
        glUniform1f(glGetUniformLocation(m_program[Blur2].programId(), "height"), height_);
        glUniform1f(glGetUniformLocation(m_program[Blur2].programId(), "width"), width_);
        glActiveTexture(GL_TEXTURE0);
        glBindTexture(GL_TEXTURE_2D, fboColorBlurSSAO1);
        renderQuad();
        glBindFramebuffer(GL_FRAMEBUFFER, 0);

    }

    if(showBlur || showSepBlur){
        if(timerG.isCreated()){
             timerG.end();
             resTime = timerG.waitForResult();
             if(resTime==0){return;}

             fps = 1.0/((float)resTime/1000000000.0);

             std::stringstream s_fps;
             s_fps << std::fixed << std::setprecision(1);
             s_fps << fps << " FPS" << std::ends;
             s_fps << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
             fpsString = QString::fromStdString(s_fps.str());
             m_fpsCount->setText(fpsString);
         }
        return;}

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glUseProgram(m_program[LightSSAO].programId());

    // send light relevant uniforms

    Eigen::Vector4f lightPos = Eigen::Vector4f(0, 2, 1, 1.0);
    Eigen::Vector3f lightColor = Eigen::Vector3f(0.8,0.8,0.8);
    Eigen::Vector4f viewPos4 = view * lightPos;
    Eigen::Vector3f lightPosView = Eigen::Vector3f(viewPos4[0] , viewPos4[1], viewPos4[2]);// Eigen::Vector3f(view * Eigen::Vector4f(lightPos, 1.0));

    glUniform3f(glGetUniformLocation(m_program[LightSSAO].programId(), "light.Position"), lightPosView[0],lightPosView[1],lightPosView[2]);
    glUniform3f(glGetUniformLocation(m_program[LightSSAO].programId(), "light.Color"), lightColor[0],lightColor[1],lightColor[2]);
    glUniform1f(glGetUniformLocation(m_program[LightSSAO].programId(), "height"), height_);
    glUniform1f(glGetUniformLocation(m_program[LightSSAO].programId(), "width"), width_);

    // Update attenuation parameters
    const float linear    = 0.09;
    const float quadratic = 0.032;

    glUniform1f(glGetUniformLocation(m_program[LightSSAO].programId(), "light.Linear"), linear);
    glUniform1f(glGetUniformLocation(m_program[LightSSAO].programId(), "light.Quadratic"), quadratic);

    glActiveTexture(GL_TEXTURE0);
    glBindTexture(GL_TEXTURE_2D, texturePosition);
    glActiveTexture(GL_TEXTURE1);
    glBindTexture(GL_TEXTURE_2D, textureNormal);
    glActiveTexture(GL_TEXTURE2);
    glBindTexture(GL_TEXTURE_2D, textureAlbedo);
    glActiveTexture(GL_TEXTURE3); // add extra SSAO texture to lighting pass
    glBindTexture(GL_TEXTURE_2D, fboColorBlurSSAO);
    renderQuad();

    if(timerG.isCreated()){
         timerG.end();
         resTime = timerG.waitForResult();
         if(resTime==0){return;}

         fps = 1.0/((float)resTime/1000000000.0);

         std::stringstream s_fps;
         s_fps << std::fixed << std::setprecision(1);
         s_fps << fps << " FPS" << std::ends;
         s_fps << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
         fpsString = QString::fromStdString(s_fps.str());
         m_fpsCount->setText(fpsString);
     }

}

QGroupBox* ViewerFRR1::controlPanel()
{
    // init group box
    QGroupBox *groupBox = HW::controlPanel();
    groupBox->setStyleSheet(GroupBoxStyle);
    mesh_importer.AddImportExportPLY(groupBox);
    radiusAndBias(groupBox);
    showPasses(groupBox);
    usePassesSSAO(groupBox);
    usePassesBlur(groupBox);
    showFramerate(groupBox);

    return(groupBox);
}

void ViewerFRR1::reset()
{
    mesh_ = nullptr;	// recompute geometry
    initVertexBuffer();
    // draw
    updateGL();
}

void ViewerFRR1::initShaders()
{
    // init uniforms hash table based on uniform variable names and location IDs
    UniformMap uniforms;
    // compile shader, bind attribute vars, link shader, and initialize uniform var table
    initShader(GeometryPass, QString(":FRR/Shaders/geometry_ssao.vert"), QString(":FRR/Shaders/geometry_ssao.frag"), uniforms);
    initShader(SSAO, QString(":FRR/Shaders/ssao.vert"), QString(":FRR/Shaders/ssao.frag"), uniforms);
    initShader(LightSSAO, QString(":FRR/Shaders/ssao.vert"), QString(":FRR/Shaders/light_ssao.frag"), uniforms);
    initShader(BlurSSAO, QString(":FRR/Shaders/ssao.vert"), QString(":FRR/Shaders/blur_ssao.frag"), uniforms);
    initShader(SepSSAO, QString(":FRR/Shaders/ssao.vert"), QString(":FRR/Shaders/ssao_sep.frag"), uniforms);
    initShader(Blur1, QString(":FRR/Shaders/ssao.vert"), QString(":FRR/Shaders/blur1.frag"), uniforms);
    initShader(Blur2, QString(":FRR/Shaders/ssao.vert"), QString(":FRR/Shaders/blur2.frag"), uniforms);

    glUseProgram(m_program[LightSSAO].programId());
    unsigned int texLoc = glGetUniformLocation(m_program[LightSSAO].programId(), "ssaoInput");
    glUniform1i(texLoc, 3);
    texLoc = glGetUniformLocation(m_program[LightSSAO].programId(), "gPosition");
    glUniform1i(texLoc, 0);
    texLoc = glGetUniformLocation(m_program[LightSSAO].programId(), "gNormal");
    glUniform1i(texLoc, 1);
    texLoc = glGetUniformLocation(m_program[LightSSAO].programId(), "gAlbedo");
    glUniform1i(texLoc, 2);

    glUseProgram(m_program[SSAO].programId());
    texLoc = glGetUniformLocation(m_program[SSAO].programId(), "gPosition");
    glUniform1i(texLoc, 0);
    texLoc = glGetUniformLocation(m_program[SSAO].programId(), "gNormal");
    glUniform1i(texLoc, 1);
    texLoc = glGetUniformLocation(m_program[SSAO].programId(), "texNoise");
    glUniform1i(texLoc, 2);

    glUseProgram(m_program[SepSSAO].programId());
    texLoc = glGetUniformLocation(m_program[SepSSAO].programId(), "gPosition");
    glUniform1i(texLoc, 0);
    texLoc = glGetUniformLocation(m_program[SepSSAO].programId(), "gNormal");
    glUniform1i(texLoc, 1);
    texLoc = glGetUniformLocation(m_program[SepSSAO].programId(), "texNoise");
    glUniform1i(texLoc, 2);

    glUseProgram(m_program[Blur1].programId());
    texLoc = glGetUniformLocation(m_program[Blur1].programId(), "ssaoInput");
    glUniform1i(texLoc, 0);

    glUseProgram(m_program[Blur2].programId());
    texLoc = glGetUniformLocation(m_program[Blur2].programId(), "blur1stPass");
    glUniform1i(texLoc, 0);

}

void ViewerFRR1::initVertexBuffer()
{

    if (mesh_ == nullptr) return;
    if(width_==0||height_==0){return;}
    initVAO();
    //Create the FrameBuffer
    glGenFramebuffers(1, &fbo);
    glBindFramebuffer(GL_FRAMEBUFFER, fbo);

    // Position buffer
    glGenTextures(1, &texturePosition);
    glBindTexture(GL_TEXTURE_2D, texturePosition);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB16F, width_, height_, 0, GL_RGB, GL_FLOAT, NULL);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, texturePosition, 0);

    // Normal buffer
    glGenTextures(1, &textureNormal);
    glBindTexture(GL_TEXTURE_2D, textureNormal);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB16F, width_, height_, 0, GL_RGB, GL_FLOAT, NULL);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT1, GL_TEXTURE_2D, textureNormal, 0);

    // color + specular color buffer
    glGenTextures(1, &textureAlbedo);
    glBindTexture(GL_TEXTURE_2D, textureAlbedo);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT2, GL_TEXTURE_2D, textureAlbedo, 0);

    // tell OpenGL which color attachments we'll use (of this framebuffer) for rendering
    GLuint attachments[3] = { GL_COLOR_ATTACHMENT0, GL_COLOR_ATTACHMENT1, GL_COLOR_ATTACHMENT2 };
    glDrawBuffers = (_glDrawBuffers) QGLWidget::context()->getProcAddress("glDrawBuffers");
    glDrawBuffers(3,attachments);

    // create and attach depth buffer (renderbuffer)
    unsigned int rboDepth;
    glGenRenderbuffers(1, &rboDepth);
    glBindRenderbuffer(GL_RENDERBUFFER, rboDepth);
    glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, width_, height_);
    glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboDepth);

    // finally check if framebuffer is complete
    if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
        std::cout << "Framebuffer not complete!" << glCheckFramebufferStatus(GL_FRAMEBUFFER) << std::endl;
    else
        std::cout << "Framebuffer Complete!" << std::endl;

    glBindFramebuffer(GL_FRAMEBUFFER, 0);

    // Buffer for the ssao
    glGenFramebuffers(1, &fboSSAO);
    glGenFramebuffers(1, &fboBlurSSAO);
    glGenFramebuffers(1, &fboBlurSSAO2);
    glBindFramebuffer(GL_FRAMEBUFFER, fboSSAO);

    // SSAO color buffer
    glGenTextures(1, &fboColorSSAO);
    glBindTexture(GL_TEXTURE_2D, fboColorSSAO);
    glBindTexture(GL_TEXTURE_2D, fboColorSSAO);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, width_, height_, 0, GL_RGB, GL_FLOAT, NULL);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, fboColorSSAO, 0);

    if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
        std::cout << "SSAO buffer object NOT complete" << glCheckFramebufferStatus(GL_FRAMEBUFFER) << std::endl;
    else
        std::cout << "SSAO buffer object complete!" << std::endl;

    glBindFramebuffer(GL_FRAMEBUFFER, fboBlurSSAO);
    glGenTextures(1, &fboColorBlurSSAO);
    glBindTexture(GL_TEXTURE_2D, fboColorBlurSSAO);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, width_, height_, 0, GL_RGB, GL_FLOAT, NULL);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, fboColorBlurSSAO, 0);
    glBindFramebuffer(GL_FRAMEBUFFER, 0);

    glGenFramebuffers(1, &fboBlurSSAO1);
    glGenFramebuffers(1, &fboBlurSSAO2);

    glBindFramebuffer(GL_FRAMEBUFFER, fboBlurSSAO1);
    glGenTextures(1, &fboColorBlurSSAO1);
    glBindTexture(GL_TEXTURE_2D, fboColorBlurSSAO1);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, width_, height_, 0, GL_RGB, GL_FLOAT, NULL);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, fboColorBlurSSAO1, 0);
    glBindFramebuffer(GL_FRAMEBUFFER, 0);

    //second blur pass
    glBindFramebuffer(GL_FRAMEBUFFER, fboBlurSSAO2);
    glGenTextures(1, &fboColorBlurSSAO2);
    glBindTexture(GL_TEXTURE_2D, fboColorBlurSSAO2);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, width_, height_, 0, GL_RGB, GL_FLOAT, NULL);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, fboColorBlurSSAO2, 0);

    if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
        std::cout << "SSAO Blur buffer object NOT complete" << glCheckFramebufferStatus(GL_FRAMEBUFFER) << std::endl;
    else
        std::cout << "SSAO Blur buffer object complete!" << std::endl;

    std::cout << width_ << "  " << height_ << std::endl;

    glBindFramebuffer(GL_FRAMEBUFFER, 0);

    //// KERNEL
    std::uniform_real_distribution<float> randomFloats(0.0, 1.0); // random floats between 0.0 - 1.0
    std::default_random_engine generator;
    for (unsigned int i = 0; i < 64; ++i)
    {
        Eigen::Vector3f sample(
                    randomFloats(generator) * 2.0 - 1.0,
                    randomFloats(generator) * 2.0 - 1.0,
                    randomFloats(generator)
                    );
        sample.normalize();
        sample *= randomFloats(generator);

        float scale = float(i) / 64.0;
        scale = lerp(0.1f, 1.0f, scale * scale);
        sample *= scale;
        ssaoKernel.push_back(sample);
    }

    //// NOISE
    for (unsigned int i = 0; i < 16; i++)
    {
        Eigen::Vector3f noise(randomFloats(generator) * 2.0 - 1.0, randomFloats(generator) * 2.0 - 1.0, 0.0f); // rotate around z-axis (in tangent space)
        ssaoNoise.push_back(noise);
    }

    glGenTextures(1, &textureNoise);
    glBindTexture(GL_TEXTURE_2D, textureNoise);
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB32F, 4, 4, 0, GL_RGB, GL_FLOAT, &textureNoise);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);

    std::cout << "Complete" << std::endl;

    update();
}

float ViewerFRR1::lerp(float a, float b, float f)
{
    return a + f * (b - a);
}
void ViewerFRR1::importModel(){
    QString filename;

    filename = QFileDialog::getOpenFileName(this, tr("Load model"), "./",
                                            tr("PLY Files ( *.ply )"));
    if (!filename.isNull()) {
        if(!mesh_importer.LoadModel(filename))
            QMessageBox::warning(this, tr("Error"),
                                 tr("The file could not be opened"));
    }

}
void ViewerFRR1::initVAO(){
    init = true;
    // set flag for creating buffers (1st time only)
    static bool flag = 1;

    if(flag) {
        glGenBuffers(1, &facesBufferVBO_ID);
        flag = 0;
    }

    if (mesh_ == nullptr) return;

    std::vector<GLfloat> data;
    data.clear();
    for(int i =0;i<(int)mesh_->vertices_.size()/3;i++){

        data.push_back(mesh_->vertices_[3*i]);
        data.push_back(mesh_->vertices_[3*i+1]);
        data.push_back(mesh_->vertices_[3*i+2]);
        data.push_back(mesh_->normals_[3*i]);
        data.push_back(mesh_->normals_[3*i+1]);
        data.push_back(mesh_->normals_[3*i+2]);
    }

    glGenVertexArrays = (_glGenVertexArrays) QGLWidget::context()->getProcAddress("glGenVertexArrays");
    glBindVertexArray = (_glBindVertexArray) QGLWidget::context()->getProcAddress("glBindVertexArray");

    //       /* Allocate and assign a Vertex Array Object to our handle */
    glGenVertexArrays(1, &VAO_ID);

    //        /* Bind our Vertex Array Object as the current used object */
    glBindVertexArray(VAO_ID);
    glGenBuffers(1, &VBO_ID);

    //        /* Bind our first VBO as being the active buffer and storing vertex attributes (coordinates) */
    glBindBuffer(GL_ARRAY_BUFFER, VBO_ID);
    glBufferData(GL_ARRAY_BUFFER,2*mesh_->vertices_.size() * sizeof(GLfloat), &data[0], GL_STATIC_DRAW);

    //        /* Specify that our coordinate data is going into attribute index 0, and contains two floats per vertex */
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float), 0);

    //        /* Enable attribute index 0 as being used */
    glEnableVertexAttribArray(0);

    //        /* Specify that our normal data is going into attribute index 1, and contains three floats per vertex */
    glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6*sizeof(float),(void*)(3*sizeof(float)));

    //        /* Enable attribute index 1 as being used */
    glEnableVertexAttribArray(1);

    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID);
    glBufferData(GL_ELEMENT_ARRAY_BUFFER, mesh_->faces_.size() * sizeof(int), &mesh_->faces_[0], GL_STATIC_DRAW);

}
void ViewerFRR1::drawVAO(){

    // draw triangles
    if (mesh_ == nullptr) return;
    if(!init) initVAO();
    // VAO
    glBindVertexArray = (_glBindVertexArray) QGLWidget::context()->getProcAddress("glBindVertexArray");

    glBindVertexArray(VAO_ID);
    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, facesBufferVBO_ID);
    glDrawElements(GL_TRIANGLES,mesh_->faces_.size(), GL_UNSIGNED_INT, 0);

    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0);
    glBindVertexArray(0);
}

void ViewerFRR1::renderQuad(){
    if (quadVAO == 0)
    {
        float quadVertices[] = {
            -1.0f,  1.0f, 0.0f, 0.0f, 1.0f,
            -1.0f, -1.0f, 0.0f, 0.0f, 0.0f,
            1.0f,  1.0f, 0.0f, 1.0f, 1.0f,
            1.0f, -1.0f, 0.0f, 1.0f, 0.0f,
        };

        glGenVertexArrays(1, &quadVAO);
        glGenBuffers(1, &quadVBO);
        glBindVertexArray(quadVAO);
        glBindBuffer(GL_ARRAY_BUFFER, quadVBO);
        glBufferData(GL_ARRAY_BUFFER, sizeof(quadVertices), &quadVertices, GL_STATIC_DRAW);
        glEnableVertexAttribArray(0);
        glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 5 * sizeof(float), (void*)0);
        glEnableVertexAttribArray(1);
        glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 5 * sizeof(float), (void*)(3 * sizeof(float)));
    }
    glBindVertexArray(quadVAO);
    glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
    glBindVertexArray(0);

}

void ViewerFRR1::showPasses(QGroupBox *groupBox){
    showSSAObox = new QRadioButton("SSAO");
    showSepSSAObox = new QRadioButton("SSAO separable");
    showBlurbox = new QRadioButton("Blur");
    showSepBlurbox = new QRadioButton("Blur separable");
    showLightbox = new QRadioButton("Light");

    showLightbox->setChecked(true);

    QGridLayout *layout2 = new QGridLayout;
    QLabel *passesL= new QLabel(tr("Choose what pass to show:"));
    QWidget *w1 = new QWidget();
    QFrame* line = new QFrame();
    line->setFrameShape(QFrame::HLine);

    connect(showSSAObox, SIGNAL(clicked()),this, SLOT(changePass()));
    connect(showSepSSAObox, SIGNAL(clicked()),this, SLOT(changePass()));
    connect(showBlurbox, SIGNAL(clicked()),this, SLOT(changePass()));
    connect(showSepBlurbox, SIGNAL(clicked()),this, SLOT(changePass()));
    connect(showLightbox, SIGNAL(clicked()),this, SLOT(changePass()));

    auto layout = dynamic_cast<QGridLayout*>(groupBox->layout());

    int row = layout2->rowCount() + 1;
    int headRow = row;
    row++;
    layout2->addWidget(passesL,row,0);
    row++;
    layout2->addWidget(showSSAObox,row, 0);
    layout2->addWidget(showSepSSAObox,row, 1);
    row++;
    layout2->addWidget(showBlurbox,row, 0);
    layout2->addWidget(showSepBlurbox,row, 1);
    row++;
    layout2->addWidget(showLightbox,row, 0);
    w1->setLayout(layout2);
    layout->addWidget(w1,layout->rowCount() + 1,0);
    layout->addWidget(line,headRow, 0, 1 ,layout->columnCount());

    groupBox->setLayout(layout);

}
void ViewerFRR1::changePass(){
    if(showSSAObox->isChecked()){
        useSepSSAO=false;
        useSSAObox->setChecked(true);
        useSepSSAObox->setChecked(false);
        showSSAO=true;
        showBlur=false;
        showSepSSAO=false;
        showSepBlur=false;
        showLight =false;
    }
    else if(showSepSSAObox->isChecked()){
        useSSAObox->setChecked(false);
        useSepSSAObox->setChecked(true);
        useSepSSAO=true;
        showSSAO=false;
        showBlur=false;
        showSepSSAO=true;
        showSepBlur=false;
        showLight =false;
    }
    else if(showBlurbox->isChecked()){
        useBlurbox->setChecked(true);
        useSepBlurbox->setChecked(false);
        useSepBlur=false;
        showSSAO=false;
        showBlur=true;
        showSepSSAO=false;
        showSepBlur=false;
        showLight =false;
    }
    else if(showSepBlurbox->isChecked()){
        useBlurbox->setChecked(false);
        useSepBlurbox->setChecked(true);
        useSepBlur=true;
        showSSAO=false;
        showBlur=false;
        showSepSSAO=false;
        showSepBlur=true;
        showLight =false;
    }
    else if(showLightbox->isChecked()){
        showSSAO=false;
        showBlur=false;
        showSepSSAO=false;
        showSepBlur=false;
        showLight =true;
    }

    update();
}

void ViewerFRR1::usePassesSSAO(QGroupBox *groupBox){
    useSSAObox = new QRadioButton("SSAO");
    useSepSSAObox = new QRadioButton("SSAO separable");
    useSSAObox->setChecked(true);

    QGridLayout *layout2 = new QGridLayout;
    QLabel *useL= new QLabel(tr("Choose what SSAO pass to use:"));
    QWidget *w1 = new QWidget();
    QFrame* line = new QFrame();
    line->setFrameShape(QFrame::HLine);

    connect(useSSAObox, SIGNAL(clicked()),this, SLOT(changeUseSSAO()));
    connect(useSepSSAObox, SIGNAL(clicked()),this, SLOT(changeUseSSAO()));

    auto layout = dynamic_cast<QGridLayout*>(groupBox->layout());

    int row = layout2->rowCount() + 1;
    int headRow = row;
    row++;
    layout2->addWidget(useL,row,0);
    row++;
    layout2->addWidget(useSSAObox,row, 0);
    layout2->addWidget(useSepSSAObox,row, 1);
    layout2->addWidget(line,headRow, 0, 1 ,layout->columnCount());
    w1->setLayout(layout2);
    layout->addWidget(w1,layout->rowCount() + 1,0);
    groupBox->setLayout(layout);

}
void ViewerFRR1::changeUseSSAO(){
    useSepSSAO=useSepSSAObox->isChecked();
    update();
}

void ViewerFRR1::usePassesBlur(QGroupBox *groupBox){

    useBlurbox = new QRadioButton("Blur");
    useSepBlurbox = new QRadioButton("Blur separable");
    useBlurbox->setChecked(true);

    QGridLayout *layout2 = new QGridLayout;

    QLabel *useL= new QLabel(tr("Choose what blur pass to use:"));
    QWidget *w1 = new QWidget();
    QFrame* line = new QFrame();
    line->setFrameShape(QFrame::HLine);

    connect(useSSAObox, SIGNAL(clicked()),this, SLOT(changeUseBlur()));
    connect(useSepSSAObox, SIGNAL(clicked()),this, SLOT(changeUseBlur()));

    auto layout = dynamic_cast<QGridLayout*>(groupBox->layout());

    int row = layout2->rowCount() + 1;
    int headRow = row;
    row++;
    layout2->addWidget(useL,row,0);
    row++;
    layout2->addWidget(useBlurbox,row, 0);
    layout2->addWidget(useSepBlurbox,row, 1);
    layout2->addWidget(line,headRow, 0, 1 ,layout->columnCount());

    w1->setLayout(layout2);
    layout->addWidget(w1,layout->rowCount() + 1,0);
    groupBox->setLayout(layout);

}
void ViewerFRR1::changeUseBlur(){
    useSepBlur=useSepBlurbox->isChecked();
    //initVertexBuffer();
    update();
}
void ViewerFRR1::radiusAndBias(QGroupBox *groupBox){

    QLabel *radiusL= new QLabel(tr("Radius:"));
    QLabel *biasL= new QLabel(tr("Bias:"));

    QDoubleSpinBox *radiusS = new QDoubleSpinBox;
    radiusS->setRange(0.0, 800.0);
    radiusS->setSingleStep(1);
    radiusS->setValue((double)radi);
    QDoubleSpinBox *biasS= new QDoubleSpinBox;
    biasS->setRange(0.0,0.5);
    biasS->setSingleStep(0.0001);
    biasS->setValue((double)bias);
    biasS->setDecimals(4);
    QFrame* line = new QFrame();
    line->setFrameShape(QFrame::HLine);
    QLabel *title = new QLabel("SSAO:");

    connect(radiusS, SIGNAL(valueChanged(double)),this, SLOT(setRadi(double)));
    connect(biasS, SIGNAL(valueChanged(double)),this, SLOT(setBias(double)));
    auto layout = dynamic_cast<QGridLayout*>(groupBox->layout());

    int row = layout->rowCount() + 1;
    int headRow = row;
    row++;
    layout->addWidget(title,row,0);
    row++;
    layout->addWidget(radiusL,row, 0);
    layout->addWidget(radiusS,row, 1);
    row++;
    layout->addWidget(biasL,row, 0);
    layout->addWidget(biasS,row, 1);
    layout->addWidget(line,headRow, 0, 1 ,layout->columnCount());

    groupBox->setLayout(layout);

}
void ViewerFRR1::setBias(double b){
    bias=(float)b;
    update();
}
void ViewerFRR1::setRadi(double r){
    radi=(float)r;
    update();
}

void ViewerFRR1::showFramerate(QGroupBox *groupBox){
    m_fpsCount = new QLabel("0");
    QFrame* line = new QFrame();

    line->setFrameShape(QFrame::HLine);
    QLabel *title = new QLabel(" FPS:");

    auto layout = dynamic_cast<QGridLayout*>(groupBox->layout());

    int row = layout->rowCount() + 1;
    int headRow = row;
    row++;
    layout->addWidget(title,row,0);
 //   row++;
    layout->addWidget(m_fpsCount,row, 1);

    layout->addWidget(line,headRow, 0, 1 ,layout->columnCount());

    groupBox->setLayout(layout);
}


/*    Copyright (c) 2010-2019, Delft University of Technology
*    All rigths reserved
*
*    This file is part of the Tudat. Redistribution and use in source and
*    binary forms, with or without modification, are permitted exclusively
*    under the terms of the Modified BSD license. You should have received
*    a copy of the license with this file. If not, please or visit:
*    http://tudat.tudelft.nl/LICENSE.
*/

#include <iostream>
#include <fstream>

#include <boost/filesystem.hpp>

#include "pagmo/algorithms/de1220.hpp"
#include "pagmo/algorithms/simulated_annealing.hpp"
#include "pagmo/algorithms/sade.hpp"
#include "pagmo/algorithms/cmaes.hpp"

#include "Problems/earthMarsTransfer.h"
#include "Problems/applicationOutput.h"
#include "Problems/getAlgorithm.h"
#include "Problems/saveOptimizationResults.h"

//! Execute  main
int main( )
{
    using namespace tudat_pagmo_applications;

    //Set seed for reproducible results
    pagmo::random_device::set_seed( 123 );

    // We have two decision variables each with a lower and upper bound, create a vector of vectors that will contain these.
    std::vector< std::vector< double > > bounds( 2, std::vector< double >( 2, 0.0 ) );

    // Define bounds: Search between 2020 and 2025 for flight duration between 200 and 1000 days.
    bounds[ 0 ][ 0 ] = 2458849.5;
    bounds[ 1 ][ 0 ] = 2460676.5;
    bounds[ 0 ][ 1 ] = 200;
    bounds[ 1 ][ 1 ] = 1000;

    // Create object to compute the problem fitness
    problem prob{EarthMarsTransfer( bounds )};

    // Perform grid saerch
    createGridSearch( prob, bounds, { 1000, 1000 }, "porkchopEarthMars" );

    // Perform optimization with 8 different optimizers
    for( int j = 0; j < 8; j++ )
    {
        // Retrieve algorothm
        int algorithmIndex = j;
        algorithm algo{getAlgorithm( algorithmIndex )};

        // Create an island with 1024 individuals
        island isl{algo, prob, 1024};

        // Evolve for 100 generations
        for( int i = 0 ; i < 100; i++ )
        {
            isl.evolve( );
            while( isl.status( ) != pagmo::evolve_status::idle &&
                   isl.status( ) != pagmo::evolve_status::idle_error )
            {
                isl.wait( );
            }
            isl.wait_check( ); // Raises errors

            // Write current iteration results to file
            printPopulationToFile( isl.get_population( ).get_x( ), "earthMarsLambert_" + std::to_string( j ) + "_" + std::to_string( i ) , false );
            printPopulationToFile( isl.get_population( ).get_f( ), "earthMarsLambert_" + std::to_string( j ) + "_" + std::to_string( i ) , true );

            std::cout<<i<<" "<<algorithmIndex<<std::endl;
        }
    }

    return 0;

}