#include "qlearner/qlearner.hpp"
#include "qlearner/stats.h"
#include "qlearner/action.h"
#include <iostream>
#include <fstream>
#include <thread>
#include <mutex>
#include <atomic>
#include <chrono>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <limits.h>

#include "config.h"
#include "net/fc_layer.hpp"
#include "net/relu_layer.hpp"
#include "net/tanh_layer.hpp"
#include "net/solver.hpp"
#include "net/rmsprop.hpp"
#include "net/network.hpp"

#include "dt.h"

#include "pole.h"

#include "uct-agent.h"

#include <random>

using namespace net;
using namespace qlearn;

const float BAT_SIZE = 0.05;

typedef boost::tuples::tuple<double, double, double, double> polestate;

Vector dconv(float val, float min, float max, int steps, Vector result)
{
	float p = (val - min) / (max - min);
	int rp = std::min(steps-1, std::max(0, int(steps * p)));
	result = Matrix::Zero(steps, 1);
	result[rp] = 1;
	return std::move(result);
}

struct PongGame
{
	float ballx;
	float bally;
	float bvx;
	float bvy;
	float posy;

	PongGame()
	{

	}

	void reset()
	{
		ballx = 0.6;
		bally = (rand() % 101) / 100.f;
		bvx = 1;
		bvy = (rand() % 101 - 50) / 20.f;
		posy = (rand() % 101) / 100.f;
	}

	float step( int ac )
	{
		ballx += 0.01*bvx;
		bally += 0.01*bvy;

		if( ac == 1 )
			posy += 0.025;
		else if(ac == 2)
			posy -= 0.025;

		if(bally > 1)
		{
			bally = 2 - bally;
			bvy *= -1;
		}

		if(bally < 0)
		{
			bally = -bally;
			bvy *= -1;
		}

		if( ballx > 1.0 )
		{
			if( std::abs(bally - posy) < BAT_SIZE )
				return 1;
			else
				return -1;
		}

		return 0;
	}

	const Vector& data() const
	{
		vec.resize (5);
		vec[0] = ballx;
		vec[1] = bally;
		vec[2] = posy;
		vec[3] = bvx;
		vec[4] = bvy;
		/*vec.resize(30);
		auto d1 = dconv(bally, 0, 1, 15, std::move(dccache1));
		auto d2 = dconv(posy, 0, 1, 15, std::move(dccache2));
		vec << d1, d2;
		dccache1 = std::move(d1);
		dccache2 = std::move(d2);*/
		return vec;
	}

private:
	mutable Vector vec;
	mutable Vector dccache1;
	mutable Vector dccache2;
};

Vector vec;

// Encode game state using vector
const Vector& data(boost::tuples::tuple<double, double, double, double> cpst)
{
	vec.resize (4);
	vec[0] = cpst.get<0>();
	vec[1] = cpst.get<1>();
	vec[2] = cpst.get<2>();
	vec[3] = cpst.get<3>();
	return vec;
}

void display(boost::tuples::tuple<double, double, double, double> cpst)
{
	std::cout << cpst.get<0>() << "," << cpst.get<1>() << ","
		<< cpst.get<2>() << "," << cpst.get<3>() << "\n";
}

void display(boost::tuples::tuple<double, double, double, double> cpst, int ac)
{
	std::cout << cpst.get<0>() << "," << cpst.get<1>() << ","
		<< cpst.get<2>() << "," << cpst.get<3>() << " : " << ac << "\n";
}

void render(std::fstream& out, boost::tuples::tuple<double, double, double, double> cpst, int ac)
{
	out << cpst.get<0>() << "," << cpst.get<1>() << ","
		<< cpst.get<2>() << "," << cpst.get<3>() << ",";

	if (ac == 0)
		out << "Left\n";
	else if (ac == 2)
		out << "Right\n";
	else
		out << "Stay\n";
}

void build_image(const qlearn::QLearner& l);

std::mutex mTargetNet;
std::atomic<bool> evaluate(false);
std::atomic<bool> run(true);

void learn_thread( Network& target_net, ComputationGraph& graph )
{
	Config config(4, 3, 2000000);
	config.epsilon_steps(2000000).update_interval(10000).batch_size(32).init_memory_size(10000).init_epsilon_time(100000)
		.discount_factor(0.98);

	Network network;
	network << FcLayer(Matrix::Random(10, 4).array() / 5);
	network << ReLULayer(Matrix::Zero(10, 1));
	network << FcLayer(Matrix::Random(10, 10).array() / 5);
	network << ReLULayer(Matrix::Zero(10, 1));
	network << FcLayer(Matrix::Random(3, 10).array() / 5);
	//network << TanhLayer(Matrix::Zero(3, 1));

	qlearn::QLearner learner( config, std::move(network) );

	auto prop = std::unique_ptr<RMSProp>(new RMSProp(0.9, 0.0005, 0.001));
	RMSProp* rmsprop = prop.get();
	Solver solver( std::move(prop) );

	std::fstream rewf("reward.txt", std::fstream::out);

	Pole game;
	game.reset ();
	game.perturbation ();

	int ac = 1;
	int games = 0;
	auto last_time = std::chrono::high_resolution_clock::now();
//	bool run = true;

	learner.setCallback( [&](const QLearner& learner, const Stats& stats )
	{
		std::cout << games << ": " << learner.getCurrentEpsilon() << "\n";
		std::cout << stats.getSmoothReward() << " (" <<  stats.getSmoothQVal() << ", " << stats.getSmoothMSE() << ")\n";
		rewf << stats.getSmoothReward() << "\t" <<  stats.getSmoothQVal() << "\t" << stats.getSmoothMSE() << " " << learner.getCurrentEpsilon()  << "\n";
		rewf.flush();
//		std::cout << learner.getNumberLearningSteps() << "\n";
		build_image(learner);
		std::cout << std::chrono::duration_cast<std::chrono::milliseconds>(
					std::chrono::high_resolution_clock::now() - last_time).count() << " ms\n";
		last_time = std::chrono::high_resolution_clock::now();
//		std::cout << Eigen::internal::malloc_counter() << "\n";
		std::cout << " - - - - - - - - - - \n";
		std::lock_guard<std::mutex> lck(mTargetNet);
		target_net = learner.network().clone();
		graph = ComputationGraph(target_net);
		evaluate = true;
	} );

	while(run)
	{
		/*if(games < 1000)
		{
			rmsprop->setRate(0.001);
		} else if ( games < 5000 )
		{
			rmsprop->setRate(0.005);
		} else
		{
			rmsprop->setRate(0.0001);
		}*/

		game.step(ac-1);
		float r = 0;
		bool failed = game.fail();
		if (failed) r = -1;

		ac = learner.learn_step( data (game.getState()), r, failed, solver );
		if( failed )
		{
			game.reset();
			game.perturbation ();
			games++;
		}
	}
}

extern int status;
void agentPlay (std::string filename);
void networkToJson (std::string filename);
void abstraction_check (std::string filename);
void cegis_train();
int main(int argc, char** argv)
{
	// Network network;
	// network << FcLayer(Matrix::Random(10, 4).array() / 5);
	// network << ReLULayer(Matrix::Zero(10, 1));
	// network << FcLayer(Matrix::Random(10, 10).array() / 5);
	// network << ReLULayer(Matrix::Zero(10, 1));
	// network << FcLayer(Matrix::Random(3, 10).array() / 5);

	// ComputationGraph graph(network);

	// Vector vec;
	// vec.resize(4);
	// vec[0] = -0.469213;
	// vec[1] = -0.0971917;
	// vec[2] = -0.00305836;
	// vec[3] = 0.0654995;

	// auto prop = std::unique_ptr<RMSProp>(new RMSProp(0.9, 0.0005, 0.001));
	// RMSProp* rmsprop = prop.get();
	// Solver solver( std::move(prop) );
	// int error;
	// do {
	// 	error = 0;
	// 	const auto& result = graph.forward(vec);
	// 	int row, col;
	// 	result.maxCoeff(&row,&col);
	// 	error += (row == 2? 0 : 1);
	// 	std::cout << "Its original behavior on this is: " << row  << "\n";
	// 	for (int k = 0; k < result.size(); k ++)
	// 		std::cout << result[k] << " ";
	// 	std::cout << "\n";
	// 	Vector errorCache = Vector::Zero( result.size() );

	// 	//float delta = result[i->second] - target_value;
	// 	//errorCache[i->second] = delta;
	// 	for (int k = 0; k < result.size(); k++) {
	// 		if (k == 2)
	// 			errorCache[k] = result[k] - 1.0;
	// 		else
	// 			errorCache[k] = result[k] - (-1.0);
	// 	}

	// 	graph.backpropagate(errorCache, solver);
	// 	network.update(solver);
	// } while (error > 0);

	if (argc > 1) {
		std::string param(argv[1]);

		if (param.compare ("uct") == 0) {
			UCTPoleSimulator* sim = new UCTPoleSimulator ();
		    UCTPoleSimulator* sim2 = new UCTPoleSimulator ();
		    UCT::UCTPlanner uct (sim2, -1, 1000, 1, 0.95);
		    int numGames = 1;

		    for (int i = 0; i < numGames; ++i) {
		        int steps = 0;
		        double r = 0;
		        sim->getState()->print(); cout << endl;
		        while (! sim->isTerminal()) {
		            steps++; //std::cout << "step : " << steps << "\n";
		            uct.setRootNode(sim->getState(), sim->getActions(), r, sim->isTerminal());
		            uct.plan();
		            UCT::SimAction* action = uct.getAction();

					uct.testTreeStructure();

		            r = sim->act(action);
		            sim->getState()->print(); cout << endl;
		        }
		        sim->reset();
		        cout << "Game:" << i << "  steps: " << steps << "  r: " << r << endl;
			}
		}
		else if (param.compare ("cegis") == 0) {
			cegis_train();
		}
		else {
			//Network agent;
			//agent.load (agentfile);
			//ComputationGraph graph(agent);

			//vec.resize (4);
			//vec[0] = -0.189315;
			//vec[1] = -0.00291353;
			//vec[2] = 0.00289438;
			//vec[3] = 0.00875231;

			//auto ac = getAction(graph, vec);
			//std::cout << "decision : " << ac.id-1 << "\n";

			//agentPlay (param);
			//networkToJson(agentfile);

			// -- doing abstraction refinement --
			abstraction_check (param);
		}
		return 0;
	}

	Network network;
	ComputationGraph graph(network);
	std::thread learner( learn_thread, std::ref(network), std::ref(graph));
	learner.detach();

	int games = 0;

	std::fstream evl("test.txt", std::fstream::out);

	int step = 0;
	while(true)
	{
		if(evaluate)
		{
			Network copy = network.clone();
			ComputationGraph graph(copy);
			float reward = 0;
			int ts = 0;
			for(int g = 0; g < 200; ++g)
			{
				Pole game;
				game.reset();
				game.perturbation();
				for(int s = 0; s < 200; ++s)
				{
					auto ac = getAction(graph, data(game.getState()));
					game.step(ac.id-1);
					float currReward = 0;
					bool failed = game.fail();
					if (failed) currReward = -1;
					reward += currReward;
					ts ++;
					if( failed ) break;
				}
			}
			std::cout << (ts / 200.0) << "\n";
			if (ts / 200.0 == 200.0) {
				copy.save ("agent" + std::to_string(step) + ".network");
			}
			evl << reward << "\n";
			evl.flush();
			evaluate = false;
			step ++;
			sleep (10);
		}
	}
	run = false;
	return 0;
}

void build_image(const QLearner& l)
{

}

//===================  Test and Verification of learned neural model ===================
// Collect samples for each class.
void agentSample (ComputationGraph& graph, int acts, std::vector<std::pair<polestate,int>>& keyset) {
	Pole game;
	game.reset();
	game.perturbation();
	int s = 0;
	int th = 0;
	int c = 0;
	while (1) {
		polestate st = game.getState();
		auto ac = getAction(graph, data(st));
		if (ac.id == th) {
			keyset.push_back(std::make_pair (st, ac.id));
			th++;
			c++;
		}
		game.step(ac.id-1);
		if (c == acts) break;
		bool failed = game.fail();
		s++;
		if( failed ) break;
	}
}

// test a learn model in file.
void agentPlay (std::string filename) {
	Network agent;
	agent.load (filename);
	ComputationGraph graph(agent);
	float reward = 0;
	int ts = 0;

	std::fstream evl("c5.data", std::fstream::out);

	for(int g = 0; g < 100; ++g)
	{
		Pole game;
		game.reset();
		game.perturbation();
		display(game.getState());
		int s = 0;
		while(true)
		{
			auto ac = getAction(graph, data(game.getState()));
			//std::cout << s << ": "; display (game.getState(), ac.id-1);
			game.step(ac.id-1);
			render (evl, game.getState(), ac.id);
			evl.flush ();
			float currReward = 0;
			bool failed = game.fail();
			if (failed) currReward = -1;
			reward += currReward;
			ts ++;
			s ++;
			if( failed ) {
				std::cout << "Maintained in " << s << " steps\n";
				break;
			}
		}
	}
	evl.close();
	std::cout << (ts / 100.0) << "\n";
}

bool agentPlay (Pole& game, ComputationGraph& graph, int bound) {
	int s = 0;
	while (s < bound) {
		auto ac = getAction(graph, data(game.getState()));
		game.step(ac.id-1);
		bool failed = game.fail();
		s++;
		if( failed ) break;
	}
	//std::cout << "agent played " << s << " step\n";
	return (s == bound);
}

// export a learned model
void networkToJson (std::string filename) {
	Network agent;
	agent.load (filename);
	agent.saveJson (filename + ".json");
}

// UCT advisor
int uct_advise (polestate st, bool terminal, ComputationGraph& graph) {
    UCTPoleSimulator* sim2 = new UCTPoleSimulator (&graph);
    UCT::UCTPlanner uct (sim2, -1, 1000, 1, 0.95);

    UCTPoleState* current = new UCTPoleState (st);
    uct.setRootNode (current, sim2->getActions(), 0, terminal);
    uct.plan();
    UCT::SimAction* action = uct.getAction();
    const UCTPoleAction* act = dynamic_cast<const UCTPoleAction*> (action);
    return (act->id)+1;
}

// Train NN with key points
bool key_gen (Pole& game, ComputationGraph& graph, int bound, std::vector<std::pair<polestate,int>>& keyset) {
	if (bound == 0)
		return true;

	// game should be in the very initial state.
	game.reset();
	game.perturbation();

	std::vector<std::pair<polestate,int>> path;
	int s = 0;
	while(s < bound)
	{
		//boost::tuples::tuple<double, double, double, double>
		polestate st = game.getState();
		auto ac = getAction(graph, data(game.getState()));
		path.push_back (std::make_pair(st, ac.id));
		game.step(ac.id-1);
		display (st, ac.id-1);
		bool failed = game.fail();
		s ++;
		if( failed ) break;
	}
	std::cout << "how well is the trained network: " << s << " steps\n";
	if (s == bound) {
		// neural network is good enough so there is no need to improve it.
		return true;
	}
	else {
		for (std::vector<std::pair<polestate,int>>::reverse_iterator i = path.rbegin(); i != path.rend(); ++i) {
			// Check how mc thinks about the best move.
			//std::cout << "from the " << s << " step in the counterexample\n";
			int ac = uct_advise(i->first, game.fail(), graph);
			if (ac == i->second) {
				// The mc and neural net agrees with each other.
			} else {
				std::vector<std::pair<polestate,int>> local_keyset;
				polestate st = i->first;
				game.setState (st);
				int i = 1;
				bool res = false;
				while (i <= bound - s) {
					local_keyset.push_back (std::make_pair (st, ac));
					game.step(ac-1);
					if (game.fail()) break;
					st = game.getState();
					//std::cout << "agent needs to play " << bound - s - i << " steps from "; display (st);
					res = agentPlay (game, graph, bound - s - i);
					game.setState(st);
					if (res) break;
					ac = uct_advise(st, game.fail(), graph);
					i++;
				}

				if (res) {
					std::cout << "we can fix the the counterexample path\n";
					keyset.insert(keyset.end(), local_keyset.begin(), local_keyset.end());
					return true;
				}
			}
			s--;
		}
		std::cout << "we cannot fix the counterexample path\n";
		return false;
	}
}

// CEGIS based training for controller synthesis.
void cegis_train() {
	Network network;
	network << FcLayer(Matrix::Random(10, 4).array() / 5);
	network << ReLULayer(Matrix::Zero(10, 1));
	network << FcLayer(Matrix::Random(10, 10).array() / 5);
	network << ReLULayer(Matrix::Zero(10, 1));
	network << FcLayer(Matrix::Random(3, 10).array() / 5);
	network << TanhLayer(Matrix::Zero(3, 1));

	ComputationGraph graph(network);

	Pole game;

	int bound = 200;

	std::vector<std::pair<polestate,int>> keyset;

	int g = 0;
	do {
		int size = keyset.size();
		bool fixable = key_gen (game, graph, bound, keyset);
		if (!fixable) {
			std::cout << "Should go back to reinforcement learning?\n";
			return;
		}
		if (keyset.size() == size) {
			g++;
		} else {
			g = 0;
			// Supervised learning to train the controller.
			auto prop = std::unique_ptr<RMSProp>(new RMSProp(0.9, 0.005, 0.001));
			RMSProp* rmsprop = prop.get();
			Solver solver( std::move(prop) );
			// First learn on the new keypoints
			int error;
			int currSize = keyset.size() - size;
			std::cout << "---------------------------------------------------------------\n";
			int iteri = 0;
			do {
				error = 0;
				int index = 0;
				for (std::vector<std::pair<polestate,int>>::reverse_iterator i = keyset.rbegin();
						i != keyset.rend() && index < currSize; ++i) {
					//std::cout << "Enforcing the neural network to behave as: \n";
					//display (i->first, i->second-1);

					//float target_value = 1.0;

					const auto& result = graph.forward(data(i->first));
					int row, col;
					result.maxCoeff(&row,&col);
					error += (row == i->second? 0 : 1);
					//std::cout << "Its original behavior on this is: " << row - 1 << "\n";
					//for (int k = 0; k < result.size(); k ++)
					//	std::cout << result[k] << " ";
					//std::cout << "\n";
					Vector errorCache = Vector::Zero( result.size() );

					//float delta = result[i->second] - target_value;
					//errorCache[i->second] = delta;
					for (int k = 0; k < result.size(); k++) {
						if (k == i->second)
							errorCache[k] = result[k] - 1.0;
						else
							errorCache[k] = result[k] - (-1.0);
					}

					graph.backpropagate(errorCache, solver);

					index ++;
				}
				network.update( solver );
				iteri ++;
				if (iteri % 10000 == 0)
					std::cout << "temporaral fix training error number: " << error << "\n";
			} while (error > 0 && iteri < 100000);
			std::cout << "fix training with error number: " << error << "\n";
			std::cout << "---------------------------------------------------------------\n";
			// Second learn on the whole keypoints
			iteri = 0;
			do {
				error = 0;
				for (std::vector<std::pair<polestate,int>>::iterator i = keyset.begin();
						i != keyset.end(); ++i) {

					//float target_value = 1.0;

					const auto& result = graph.forward(data(i->first));
					int row, col;
					result.maxCoeff(&row,&col);
					error += (row == i->second? 0 : 1);

					Vector errorCache = Vector::Zero( result.size() );

					//float delta = result[i->second] - target_value;
					//errorCache[i->second] = delta;
					for (int k = 0; k < result.size(); k++) {
						if (k == i->second)
							errorCache[k] = result[k] - 1.0;
						else
							errorCache[k] = result[k] - (-1.0);
					}

					graph.backpropagate(errorCache, solver);
				}
				network.update( solver );
				iteri++;
				//std::cout << "error number: " << error << "\n";
			} while (error > 0 && iteri < 10000);
			std::cout << "batch training error number: " << error << "\n";
		}
	} while (g < 5);
	network.save ("supervised_agent.network");
	std::cout << "Trained Successfully!\n";
}

// Verify an abstraction
bool ce_gen (Pole& game, ComputationGraph& graph, DT& dt, int bound, std::vector<std::pair<polestate,int>>& keyset, bool rec_flag) {
	if (bound == 0)
		return true;

	std::vector<std::pair<polestate,int>> path;
	int s = 0;
	while(s < bound)
	{
		//boost::tuples::tuple<double, double, double, double>
		polestate st = game.getState();
		double*  dtst = new double[4];
		dtst[0] = st.get<0>();dtst[1] = st.get<1>();dtst[2] = st.get<2>();dtst[3] = st.get<3>();
		int ac = dt.predict (dtst, 4);
		delete [] dtst;
		path.push_back (std::make_pair(st, ac));
		game.step(ac-1);

		bool failed = game.fail();
		s ++;
		if( failed ) break;
	}
	if (!rec_flag)
		std::cout << "how well is the abstraction: " << s << " steps\n";
	if (s == bound) {
		// abstraction is fine so there is no counterexample
		return true;
	}
	else {
		for (std::vector<std::pair<polestate,int>>::reverse_iterator i = path.rbegin(); i != path.rend(); ++i) {
			game.setState (i->first);
			// Check how neural net works.
			auto ac = getAction(graph, data(i->first));
			if (ac.id == i->second) {
				// The abstraction and neural net agrees with each other.
			} else {
				// Check if neural net's decision is good.
				bool res = agentPlay (game, graph, bound - s);
				game.setState(i->first);
				if (res) {
					keyset.push_back (std::make_pair (i->first, ac.id));
					game.step(ac.id-1);
					return ce_gen (game, graph, dt, bound - s - 1, keyset, true);
				}
			}
			s--;
		}
		return false;
	}
}

// abstract-refine checking of a neural model
void abstraction_check (std::string filename) {
	Network agent;
	agent.load (filename);
	std::string modelname = filename + ".json";
	agent.saveJson (modelname);
	ComputationGraph graph(agent);
	int g = 0;

	// Load abstraction from decision tree
	DT dt(modelname);
	std::vector<std::pair<polestate,int>> keyset;
	// Use keyset to refine the abstraction
	std::string datafile = filename+".data";
	std::remove(datafile.c_str());

	// Fixme. Should ask reluplex to generate at least one sample for each class.
	//std::fstream out(datafile, std::fstream::out);
	//out << -1.70586e-01 << "," << 5.38879e-02 << ","
	//	<< -6.72141e-03 << "," << -3.10080e-02  << ",";
	//out << "Left\n";
	//out << -3.28355e-01 << "," << -1.09655e-01 << ","
	//	<< -3.96630e-03 << "," << 3.00918e-02 << ",";
	//out << "Stay\n";
	//out << -4.21625e-01 << "," << -2.76742e-01 << ","
	//	<< -1.65369e-02 << "," << 1.37284e-01 << ",";
	//out << "Right\n";
	//out.flush ();
	//out.close ();

	agentSample (graph, 3, keyset);

	// Iterative buidling an abstraction of a neuron controller.
	do {
		// Execute the abstraction
		Pole game;
		game.reset();
		game.perturbation();

		int size = keyset.size();
		std::cout << "before ce-gen keyset.size() == " << keyset.size() << "\n";
		bool res = ce_gen (game, graph, dt, 200, keyset, false);
		std::cout << "after  ce-gen keyset.size() == " << keyset.size() << "\n";
		if (!res) {
			// We found a counterexample to the neural net.
			std::cout << "A real counterexample is found!\n";
			// Should terminate reporting the counterexample.
		} else {
			if (keyset.size() == size) {
				g++;
			} else {
				g = 0;
				std::fstream evl(datafile, std::fstream::out);

				for (std::vector<std::pair<polestate,int>>::iterator i = keyset.begin();
						i != keyset.end(); ++i) {
					render (evl, i->first, i->second);
					evl.flush ();
				}
				evl.close();
				if (keyset.size() > 200) {
					std::cout << "keyset.size() == " << keyset.size() << "\n";
					dt.learn(datafile);
				}
			}
		}
	} while (g < 10); // Verification converges after consecutive success in several rounds.
	std::cout << "Verifed!\n";

	int ts = 0;
	for(int g = 0; g < 100; ++g)
	{
		Pole game;
		game.reset();
		game.perturbation();
		display(game.getState());
		int s = 0;
		while(true)
		{
			polestate st = game.getState();
			double*  dtst = new double[4];
			dtst[0] = st.get<0>();dtst[1] = st.get<1>();dtst[2] = st.get<2>();dtst[3] = st.get<3>();
			int ac = dt.predict (dtst, 4);
			delete [] dtst;
			game.step(ac-1);

			bool failed = game.fail();
			s ++;
			ts ++;
			if( failed ) {
				std::cout << "Maintained in " << s << " steps\n";
				break;
			}
		}
	}
	std::cout << (ts / 100.0) << "\n";
}


#include "imls_icp.h"
#include <csm/csm_all.h>
#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "sensor_msgs/PointCloud.h"
#include "geometry_msgs/Point32.h"
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"
#include "champion_nav_msgs/ChampionNavLaserScan.h"
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>

//pcl::visualization::CloudViewer g_cloudViewer("cloud_viewer");
//此处bag包的地址需要自行修改
std::string bagfile = "/home/shenlan/imlsMatcherProject/src/bag/imls_icp.bag";

class imlsDebug
{
public:
    imlsDebug()
    {
        m_imlsPathPub = m_node.advertise<nav_msgs::Path>("imls_path_pub_",1,true);
        m_imlsPath.header.stamp = ros::Time::now();
        m_imlsPath.header.frame_id = "odom";
        m_odomPathPub = m_node.advertise<nav_msgs::Path>("odom_path_pub_",1,true);
        m_odomPath.header.stamp = ros::Time::now();
        m_odomPath.header.frame_id = "odom";

        m_isFirstFrame = true;

        rosbag::Bag bag;
        bag.open(bagfile, rosbag::bagmode::Read);

        std::vector<std::string> topics;
        topics.push_back(std::string("/sick_scan"));
        topics.push_back(std::string("/odom"));
        rosbag::View view(bag, rosbag::TopicQuery(topics));
        //按顺序读取bag内激光的消息和里程计的消息
        BOOST_FOREACH(rosbag::MessageInstance const m, view)
        {
            champion_nav_msgs::ChampionNavLaserScanConstPtr scan = m.instantiate<champion_nav_msgs::ChampionNavLaserScan>();
            if(scan != NULL)
                championLaserScanCallback(scan);

            nav_msgs::OdometryConstPtr odom = m.instantiate<nav_msgs::Odometry>();
            if(odom != NULL)
                odomCallback(odom);

            ros::spinOnce();
            if(!ros::ok())
                break;
        }
        // m_laserscanSub = m_nh.subscribe("sick_scan",5,&imlsDebug::championLaserScanCallback,this);
    }

    //将激光消息转换为激光坐标系下的二维点云
    void ConvertChampionLaserScanToEigenPointCloud(const champion_nav_msgs::ChampionNavLaserScanConstPtr& msg,
                                                   std::vector<Eigen::Vector2d>& eigen_pts)
    {
        eigen_pts.clear();
        for(int i = 0; i < msg->ranges.size(); ++i)
        {
            if(msg->ranges[i] < msg->range_min || msg->ranges[i] > msg->range_max)
                continue;

            double lx = msg->ranges[i] * std::cos(msg->angles[i]);
            double ly = msg->ranges[i] * std::sin(msg->angles[i]);

            if(std::isnan(lx) || std::isinf(ly) ||
               std::isnan(ly) || std::isinf(ly))
                continue;

            eigen_pts.push_back(Eigen::Vector2d(lx,ly));
        }
    }

    void championLaserScanCallback(const champion_nav_msgs::ChampionNavLaserScanConstPtr& msg)
    {
        if(m_isFirstFrame == true)
        {
            std::cout <<"First Frame"<<std::endl;
            m_isFirstFrame = false;
            m_prevLaserPose = Eigen::Vector3d(0, 0, 0);
            pubPath(m_prevLaserPose, m_imlsPath, m_imlsPathPub);
            ConvertChampionLaserScanToEigenPointCloud(msg, m_prevPointCloud);
            return ;
        }

        std::vector<Eigen::Vector2d> nowPts;
        ConvertChampionLaserScanToEigenPointCloud(msg, nowPts);

        //调用imls进行icp匹配，并输出结果．
        m_imlsMatcher.setSourcePointCloud(nowPts);
        m_imlsMatcher.setTargetPointCloud(m_prevPointCloud);

        Eigen::Matrix3d rPose,rCovariance;
        if(m_imlsMatcher.Match(rPose,rCovariance))
        {
            std::cout <<"IMLS Match Successful:"<<rPose(0,2)<<","<<rPose(1,2)<<","<<atan2(rPose(1,0),rPose(0,0))*57.295<<std::endl;
            Eigen::Matrix3d lastPose;
            lastPose << cos(m_prevLaserPose(2)), -sin(m_prevLaserPose(2)), m_prevLaserPose(0),
                        sin(m_prevLaserPose(2)),  cos(m_prevLaserPose(2)), m_prevLaserPose(1),
                        0, 0, 1;
            Eigen::Matrix3d nowPose = lastPose * rPose;
            m_prevLaserPose << nowPose(0, 2), nowPose(1, 2), atan2(nowPose(1,0), nowPose(0,0));
            pubPath(m_prevLaserPose, m_imlsPath, m_imlsPathPub);
        }
        else
        {
            std::cout <<"IMLS Match Failed!!!!"<<std::endl;
        }

        m_prevPointCloud = nowPts;
    }

    void odomCallback(const nav_msgs::OdometryConstPtr& msg)
    {
        if(m_isFirstFrame == true)
            return;

        pubPath(msg, m_odomPath, m_odomPathPub);
    }

    //发布路径消息
    void pubPath(Eigen::Vector3d& pose, nav_msgs::Path &path, ros::Publisher &mcu_path_pub_)
    {
        ros::Time current_time = ros::Time::now();
        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped.pose.position.x = pose(0);
        this_pose_stamped.pose.position.y = pose(1);

        geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(pose(2));
        this_pose_stamped.pose.orientation.x = goal_quat.x;
        this_pose_stamped.pose.orientation.y = goal_quat.y;
        this_pose_stamped.pose.orientation.z = goal_quat.z;
        this_pose_stamped.pose.orientation.w = goal_quat.w;

        this_pose_stamped.header.stamp = current_time;
        this_pose_stamped.header.frame_id = "odom";
        path.poses.push_back(this_pose_stamped);
        mcu_path_pub_.publish(path);
    }

    void pubPath(const nav_msgs::OdometryConstPtr& msg, nav_msgs::Path &path, ros::Publisher &mcu_path_pub_)
    {
        ros::Time current_time = ros::Time::now();
        geometry_msgs::PoseStamped this_pose_stamped;
        this_pose_stamped.pose.position.x = msg->pose.pose.position.x;
        this_pose_stamped.pose.position.y = msg->pose.pose.position.y;

        this_pose_stamped.pose.orientation.x = msg->pose.pose.orientation.x;
        this_pose_stamped.pose.orientation.y = msg->pose.pose.orientation.y;
        this_pose_stamped.pose.orientation.z = msg->pose.pose.orientation.z;
        this_pose_stamped.pose.orientation.w = msg->pose.pose.orientation.w;

        this_pose_stamped.header.stamp = current_time;
        this_pose_stamped.header.frame_id = "odom";
        path.poses.push_back(this_pose_stamped);
        mcu_path_pub_.publish(path);
    }

    bool m_isFirstFrame;
    ros::NodeHandle m_nh;
    IMLSICPMatcher m_imlsMatcher;
    Eigen::Vector3d m_prevLaserPose;
    std::vector<Eigen::Vector2d> m_prevPointCloud;
    nav_msgs::Path m_imlsPath;
    nav_msgs::Path m_odomPath;

    tf::TransformListener m_tfListener;
    ros::NodeHandle m_node;

    ros::Subscriber m_laserscanSub;
    ros::Publisher m_imlsPathPub;
    ros::Publisher m_odomPathPub;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "imls_debug");

    imlsDebug imls_debug;

    ros::spin();

    return (0);
}


#include "MarchingCubes.h"
#include "MCLookupTables.h"
#include <string.h>
#include <iostream>
#include "DensityFunctions.h"
using namespace std;

//boost
#include <boost/thread/thread.hpp>
#include <boost/asio.hpp>

using namespace boost;
using namespace asio;

#define M_1_UV_SCALE 0.2f //bigger num = larger texture appearance

extern Camera cam; //for centering terrain around eye

struct Voxel {
	float3 pos[8];
	float density[8];
};

//------------------------------------------Look Ups
static int3 RELATIVE_CORNER_POS[8] = {
    int3(0, 0, 0), //0
    int3(1, 0, 0), //1
    int3(1, 0, 1), //2
    int3(0, 0, 1), //3
    int3(0, 1, 0), //4
    int3(1, 1, 0), //5
    int3(1, 1, 1), //6
    int3(0, 1, 1)  //7
};

//0 means x-edges, 1 means y-edges, 2 means z-edges
//case 0: return 0; case 1: return 2; case 2: return 0;
//case 3: return 2; case 4: return 0; case 5: return 2;
//case 6: return 0; case 7: return 2; case 8: return 1;
//case 9: return 1; case 10:return 1; case 11:return 1;
static int EDGE_TYPE[12] = {0, 2, 0, 2, 0, 2, 0, 2, 1, 1, 1, 1};

static int3 EDGE_OFFSETS[12] = {
    int3(0, 0, 0), //0 //x-axis
    int3(1, 0, 0), //1 //y-axis
    int3(0, 0, 1), //2 //x-axis
    int3(0, 0, 0), //3 //y-axis
    int3(0, 1, 0), //4 //x-axis
    int3(1, 1, 0), //5 //y-axis
    int3(0, 1, 1), //6 //x-axis
    int3(0, 1, 0), //7 //y-axis
    int3(0, 0, 0), //8 //z-axis
    int3(1, 0, 0), //9 //z-axis
    int3(1, 0, 1), //10//z-axis
    int3(0, 0, 1)  //11//z-axis
};

//------------------------------------------Utilities
static inline
float DensityFunction(const float3& pos) {
    //return Plane(pos);
    return Terrain(pos);
}

//Linear interpolation
static inline
float3 Lerp(const float3& vert0, const float3& vert1, float val0, float val1) {
    return vert0 + (vert1 - vert0) * ((-val0) / (val1 - val0));
    //OLD//return vert0 + (-val0) * (vert1 - vert0) / (val1 - val0);
}

//TODO [23:12] <LordCrc> say i is the index "behind", ie x-1 and j is the index in front, ie x+1, but i and j have been clamped using min/max
//[23:12] <LordCrc> then you should divide by (j-i)

//Our density value function - creates the field we are polygonizing
static inline
float3 GetVertexNormal(const int3& vc, int edge_idx, const float*** densities, const int3& num_voxels) {
    //determine grid endpoints where the normals need to be calculated at
    //gp0 = grid position 0
    int3 gp0 = vc, gp1 = vc;
    switch(edge_idx) {
        case 0: gp0 += RELATIVE_CORNER_POS[0]; gp1 += RELATIVE_CORNER_POS[1]; break;
        case 1: gp0 += RELATIVE_CORNER_POS[1]; gp1 += RELATIVE_CORNER_POS[2]; break;
        case 2: gp0 += RELATIVE_CORNER_POS[2]; gp1 += RELATIVE_CORNER_POS[3]; break;
        case 3: gp0 += RELATIVE_CORNER_POS[3]; gp1 += RELATIVE_CORNER_POS[0]; break;
        case 4: gp0 += RELATIVE_CORNER_POS[4]; gp1 += RELATIVE_CORNER_POS[5]; break;
        case 5: gp0 += RELATIVE_CORNER_POS[5]; gp1 += RELATIVE_CORNER_POS[6]; break;
        case 6: gp0 += RELATIVE_CORNER_POS[6]; gp1 += RELATIVE_CORNER_POS[7]; break;
        case 7: gp0 += RELATIVE_CORNER_POS[7]; gp1 += RELATIVE_CORNER_POS[4]; break;
        case 8: gp0 += RELATIVE_CORNER_POS[0]; gp1 += RELATIVE_CORNER_POS[4]; break;
        case 9: gp0 += RELATIVE_CORNER_POS[1]; gp1 += RELATIVE_CORNER_POS[5]; break;
        case 10:gp0 += RELATIVE_CORNER_POS[2]; gp1 += RELATIVE_CORNER_POS[6]; break;
        case 11:gp0 += RELATIVE_CORNER_POS[3]; gp1 += RELATIVE_CORNER_POS[7]; break;
    }
    //determine the normals at the two gridpoints
    //gn0 = grid normal 0; gn1 = grid normal 1; (df/dx, df/dy, df/dz)
    int min_gp0x = max(gp0.x - 1, 0); int max_gp0x = min(gp0.x + 1, num_voxels.x);
    int min_gp0y = max(gp0.y - 1, 0); int max_gp0y = min(gp0.y + 1, num_voxels.y);
    int min_gp0z = max(gp0.z - 1, 0); int max_gp0z = min(gp0.z + 1, num_voxels.z);
    float3 gn0(densities[gp0.z][gp0.y][max_gp0x] - densities[gp0.z][gp0.y][min_gp0x],
               densities[gp0.z][max_gp0y][gp0.x] - densities[gp0.z][min_gp0y][gp0.x],
               densities[max_gp0z][gp0.y][gp0.x] - densities[min_gp0z][gp0.y][gp0.x]);
    gn0 = normalize(gn0);
    if(gn0.y < 0.f) gn0 *= -1.f; //flip if needed

    int min_gp1x = max(gp1.x - 1, 0); int max_gp1x = min(gp1.x + 1, num_voxels.x);
    int min_gp1y = max(gp1.y - 1, 0); int max_gp1y = min(gp1.y + 1, num_voxels.y);
    int min_gp1z = max(gp1.z - 1, 0); int max_gp1z = min(gp1.z + 1, num_voxels.z);
    float3 gn1(densities[gp0.z][gp0.y][max_gp1x] - densities[gp0.z][gp0.y][min_gp1x],
               densities[gp0.z][max_gp1y][gp0.x] - densities[gp0.z][min_gp1y][gp0.x],
               densities[max_gp1z][gp0.y][gp0.x] - densities[min_gp1z][gp0.y][gp0.x]);
    gn1 = normalize(gn1);
    if(gn1.y < 0.f) gn1 *= -1.f; //flip if needed

    return normalize(Lerp(gn0, gn1, densities[gp0.z][gp0.y][gp0.x], densities[gp1.z][gp1.y][gp1.x]));
}

//------------------------------------------Marching Cubes Algorithm
//TriangulateVoxelData(...) determines where to placed triangles (maximum 5)
//depending on sign and value of the scalar field at passed voxel's corners.
//vc = absolute voxel position (integers)
static void TriangulateVoxelData(const Voxel& v, const int3& vc, const float*** densities,
    int*** xedge_cache, int*** yedge_cache, int*** zedge_cache, vector<Triangle>* tris,
    vector<float3>* verts, vector<float3>* normals, vector<float2>* uvs, const int3& num_voxels) {
    //***determine case number (a.k.a voxel index)***
    //determine corners that are inside the terrain
    int voxel_index = 0;
    if(v.density[0] > 0.f) voxel_index |= 1;
    if(v.density[1] > 0.f) voxel_index |= 2;
    if(v.density[2] > 0.f) voxel_index |= 4;
    if(v.density[3] > 0.f) voxel_index |= 8;
    if(v.density[4] > 0.f) voxel_index |= 16;
    if(v.density[5] > 0.f) voxel_index |= 32;
    if(v.density[6] > 0.f) voxel_index |= 64;
    if(v.density[7] > 0.f) voxel_index |= 128;

    if(EDGE_TABLE[voxel_index] == 0) {
        return; //voxel completely in or out of surface
    }
    if(EDGE_TABLE[voxel_index] == 255) {
        return; //voxel completely in or out of surface
    }

    //determine vertices
    //there are 12 vertices because they are found on the edge of a voxel and there are 12 edges
    float3 lerp_verts[12];

    //bit operators...works on each bit of binary number
    if(EDGE_TABLE[voxel_index] & 1)
        lerp_verts[0] = Lerp(v.pos[0], v.pos[1], v.density[0], v.density[1]);
    if(EDGE_TABLE[voxel_index] & 2)
        lerp_verts[1] = Lerp(v.pos[1], v.pos[2], v.density[1], v.density[2]);
    if(EDGE_TABLE[voxel_index] & 4)
        lerp_verts[2] = Lerp(v.pos[2], v.pos[3], v.density[2], v.density[3]);
    if(EDGE_TABLE[voxel_index] & 8)
        lerp_verts[3] = Lerp(v.pos[3], v.pos[0], v.density[3], v.density[0]);
    if(EDGE_TABLE[voxel_index] & 16)
        lerp_verts[4] = Lerp(v.pos[4], v.pos[5], v.density[4], v.density[5]);
    if(EDGE_TABLE[voxel_index] & 32)
        lerp_verts[5] = Lerp(v.pos[5], v.pos[6], v.density[5], v.density[6]);
    if(EDGE_TABLE[voxel_index] & 64)
        lerp_verts[6] = Lerp(v.pos[6], v.pos[7], v.density[6], v.density[7]);
    if(EDGE_TABLE[voxel_index] & 128)
        lerp_verts[7] = Lerp(v.pos[7], v.pos[4], v.density[7], v.density[4]);
    if(EDGE_TABLE[voxel_index] & 256)
        lerp_verts[8] = Lerp(v.pos[0], v.pos[4], v.density[0], v.density[4]);
    if(EDGE_TABLE[voxel_index] & 512)
        lerp_verts[9] = Lerp(v.pos[1], v.pos[5], v.density[1], v.density[5]);
    if(EDGE_TABLE[voxel_index] & 1024)
        lerp_verts[10]= Lerp(v.pos[2], v.pos[6], v.density[2], v.density[6]);
    if(EDGE_TABLE[voxel_index] & 2048)
        lerp_verts[11]= Lerp(v.pos[3], v.pos[7], v.density[3], v.density[7]);

    //***construct triangles***
    for(int i = 0; TRIANGLE_TABLE[voxel_index][i] != -1; i += 3) {
        //tris
        Triangle tri;
        float3 first_vert;
        for (int j = 0; j < 3; j++) { //for each vert
            int edge_idx = TRIANGLE_TABLE[voxel_index][i + j];
            int edge_type = EDGE_TYPE[edge_idx]; //x, y, or z axis we are on
            int3 e = vc + EDGE_OFFSETS[edge_idx];

            if(edge_type == 0) {
                if(xedge_cache[e.z][e.y][e.x] == -1) {
                    xedge_cache[e.z][e.y][e.x] = verts->size(); //update cache
                    verts->push_back(lerp_verts[edge_idx]);
                    normals->push_back(GetVertexNormal(vc, edge_idx, densities, num_voxels));
                    uvs->push_back(float2(0, 0)); //filled later. allocating for now
                }
                tri.vi[j] = xedge_cache[e.z][e.y][e.x];
            } else if(edge_type == 1) {
                if(yedge_cache[e.z][e.y][e.x] == -1) {
                    yedge_cache[e.z][e.y][e.x] = verts->size(); //update cache
                    verts->push_back(lerp_verts[edge_idx]);
                    normals->push_back(GetVertexNormal(vc, edge_idx, densities, num_voxels));
                    uvs->push_back(float2(0, 0)); //filled later

                }
                tri.vi[j] = yedge_cache[e.z][e.y][e.x];
            } else { //edge_type == 2
                if(zedge_cache[e.z][e.y][e.x] == -1) {
                    zedge_cache[e.z][e.y][e.x] = verts->size(); //update cache
                    verts->push_back(lerp_verts[edge_idx]);
                    normals->push_back(GetVertexNormal(vc, edge_idx, densities, num_voxels));
                    uvs->push_back(float2(0, 0)); //filled later                }
                }
                tri.vi[j] = zedge_cache[e.z][e.y][e.x];
            }
            uvs->at(tri.vi[j]) = float2(verts->at(tri.vi[j]).x * M_1_UV_SCALE,
                                        verts->at(tri.vi[j]).z * M_1_UV_SCALE);
        }
        tris->push_back(tri);
    }
}

//these functions will never access the same vertex. synchronization is not needed
static inline
void ScheduleAcrossX(const int const_x, const float3& voxel_size, int num_verty,
    int num_vertz, float*** vert_densities, const float3& offset, io_service* thread_pool) {
    //
    for(int z = 0; z < num_vertz; z++) {
        for(int y = 0; y < num_verty; y++) {
            float3 corner(const_x * voxel_size.x, y * voxel_size.y, z * voxel_size.z);
            corner += offset;
            vert_densities[z][y][const_x] = DensityFunction(corner);
        }
    }
}

static inline
void ScheduleAcrossY(const int const_y, const float3& voxel_size, int num_vertx,
    int num_vertz, float*** vert_densities, const float3& offset, io_service* thread_pool) {
    //
    for(int z = 0; z < num_vertz; z++) {
        for(int x = 0; x < num_vertx; x++) {
            float3 corner(x * voxel_size.x, const_y * voxel_size.y, z * voxel_size.z);
            corner += offset;
            vert_densities[z][const_y][x] = DensityFunction(corner);
        }
    }
}

static inline
void ScheduleAcrossZ(const int const_z, const float3& voxel_size, int num_vertx,
    int num_verty, float*** vert_densities,  const float3& offset, io_service* thread_pool) {
    //
    for(int y = 0; y < num_verty; y++) {
        for(int x = 0; x < num_vertx; x++) {
            float3 corner(x * voxel_size.x, y * voxel_size.y, const_z * voxel_size.z);
            corner += offset;
            vert_densities[const_z][y][x] = DensityFunction(corner);
        }
    }
}

void GenerateTerrain(Mesh* mesh, const float3& voxel_size, const int3& num_voxels) {
    cout<<"About to setup thread pool"<<endl;
    io_service thread_pool;
    //work object informs thread_pool when work starts and finishes
    //so its run() will not exit while work is being done...keeps it alive
    io_service::work work(thread_pool);
    thread_group threads; //collection of threads that will chip away at tasks in queue
    //need to subtract one because a seperate thread will be calling this. otherwise it would have
    //to wait for a free spot to begin/other way around with these threads needing to wait
    for(int i = 0; i < max(1, (int)thread::hardware_concurrency() - 1); i++) { //add threads to group
        threads.create_thread(bind(&io_service::run, &thread_pool));
    }

    //offset for centering terrain around camera eye
    int num_vertx = num_voxels.x + 1;
    int num_verty = num_voxels.y + 1;
    int num_vertz = num_voxels.z + 1;
    float3 offset(-num_vertx, -num_verty, -num_vertz);
    offset *= voxel_size;
    offset *= 0.5f;
    offset += cam.eye;
    offset.y = 0; //dont offset in the y direction (so when player is fex +200y still see terrain)

    //allocate memory for each vertex density - put into dynamic 3D array
    float*** vert_densities;
    vert_densities = new float**[num_vertz];
    //allocate memory for storing vert densities
    for(int z = 0; z < num_vertz; z++) {
        vert_densities[z] = new float*[num_verty];
        for(int y = 0; y < num_verty; y++) {
            vert_densities[z][y] = new float[num_vertx];
        }
    }

    //allocate memory for edge/vert indices (see below comments)
    int*** xedge_cache = new int**[num_vertz]; //vertex indices for vertex on each x-axis
    int*** yedge_cache = new int**[num_vertz]; //vertex indices for vertex on each y-axis
    int*** zedge_cache = new int**[num_vertz]; //vertex indices for vertex on each z-axis
    //allocate mem
    for(int z = 0; z < num_vertz; z++) {
        xedge_cache[z] = new int*[num_verty];
        yedge_cache[z] = new int*[num_verty];
        zedge_cache[z] = new int*[num_verty];
        for(int y = 0; y < num_verty; y++) {
            xedge_cache[z][y] = new int[num_vertx];
            yedge_cache[z][y] = new int[num_vertx];
            zedge_cache[z][y] = new int[num_vertx];
        }
    }
    //set to no index (-1)
    for(int z = 0; z < num_vertz; z++) {
        for(int y = 0; y < num_verty; y++) {
            for(int x = 0; x < num_vertx; x++) {
                xedge_cache[z][y][x] = -1;
                yedge_cache[z][y][x] = -1;
                zedge_cache[z][y][x] = -1;
            }
        }
    }

    cout<<"Enqueueing tasks to thread pool"<<endl;
    //divide work to each thread
    int max_axis = Max(num_vertx, num_verty, num_vertz); //work divided along this axis
    if(max_axis == 0) { //x
        for(int x = 0; x < num_vertx; x++) {
            thread_pool.post(bind(ScheduleAcrossX, x, voxel_size, num_verty, num_vertz, vert_densities, offset, &thread_pool));
        }
    } else if(max_axis == 1) { //y
        for(int y = 0; y < num_verty; y++) {
            thread_pool.post(bind(ScheduleAcrossY, y, voxel_size, num_vertx, num_vertz, vert_densities, offset, &thread_pool));
        }
    } else { //z
        for(int z = 0; z < num_vertz; z++) {
            thread_pool.post(bind(ScheduleAcrossZ, z, voxel_size, num_vertx, num_verty, vert_densities, offset, &thread_pool));
        }
    }

    cout<<"About to wait for thread pool to finish"<<endl;
    //wait till all threads have been completed. then proceed
    thread_pool.stop();
    threads.join_all();

    cout<<"Polygonizing scalar field now"<<endl;
    //polygonize each voxel
    for(int z = 0; z < num_voxels.z; z++) {
        for(int y = 0; y < num_voxels.y; y++) {
            for(int x = 0; x < num_voxels.x; x++) {
            	//for each voxel corner
            	Voxel vox;
            	for(int i = 0; i < 8; i++) {
	                vox.pos[i] = voxel_size * (float3(x, y, z) + RELATIVE_CORNER_POS[i]) + offset;
	                //vox.density[i] = DensityFunction(vox.pos[i]);
	                int3 idx = int3(x, y, z) + RELATIVE_CORNER_POS[i];
	                vox.density[i] = vert_densities[idx.z][idx.y][idx.x];
            	}

            	//polygonize passed voxel with density values at each vertex
            	TriangulateVoxelData(vox, int3(x, y, z), (const float***)vert_densities, xedge_cache,
                    yedge_cache, zedge_cache, &mesh->triangles, &mesh->vertices, &mesh->normals, &mesh->uvs, num_voxels);
            }
        }
    }

    cout<<"Cleaning up vert density array"<<endl;
    //clean up 3D vert density array
    for(int z = 0; z < num_vertz; z++) {
        for(int y = 0; y < num_verty; y++) {
            delete[] vert_densities[z][y]; //delete x dyn mem
            delete[] xedge_cache[z][y]; //delete x dyn mem
            delete[] yedge_cache[z][y]; //delete x dyn mem
            delete[] zedge_cache[z][y]; //delete x dyn mem
        }
        delete[] vert_densities[z]; //delete y dyn mem
        delete[] xedge_cache[z]; //delete y dyn mem
        delete[] yedge_cache[z]; //delete y dyn mem
        delete[] zedge_cache[z]; //delete y dyn mem
    }
    delete[] vert_densities;
    delete[] xedge_cache;
    delete[] yedge_cache;
    delete[] zedge_cache;

    vert_densities = NULL;
    xedge_cache = NULL;
    yedge_cache = NULL;
    zedge_cache = NULL;

    cout<<"MC Terrain:"<<endl;
    cout<<"\tNum Tris: "<<mesh->triangles.size()<<endl;
    cout<<"\tNum Verts: "<<mesh->vertices.size()<<endl;
}


/* stat_aggregator.cc
   Jeremy Banres, 3 August 2011
   Copyright (c) 2011 Datacratic.  All rights reserved.

*/

#include "soa/service/stat_aggregator.h"
#include "ace/INET_Addr.h"
#include "jml/arch/exception.h"
#include "jml/arch/format.h"
#include <iostream>
#include "jml/arch/cmp_xchg.h"
#include "jml/arch/atomic_ops.h"
#include "jml/utils/floating_point.h"
#include "jml/utils/smart_ptr_utils.h"
#include "jml/utils/exc_check.h"
#include <boost/tuple/tuple.hpp>
#include <algorithm>

using namespace std;
using namespace ML;

namespace Datacratic {

/*****************************************************************************/
/* COUNTER AGGREGATOR                                                        */
/*****************************************************************************/

CounterAggregator::
CounterAggregator()
    : start(Date::now()), total(0.0),
      totalsBuffer() // Keep 10sec of data.
{
}

CounterAggregator::
~CounterAggregator()
{
}

void
CounterAggregator::
record(float value)
{
    double oldval = total;

    while (!ML::cmp_xchg(total, oldval, oldval + value));
}

std::pair<double, Date>
CounterAggregator::
reset()
{
    double oldval = total;

    while (!ML::cmp_xchg(total, oldval, 0.0));

    Date oldStart = start;
    start = Date::now();

    return make_pair(oldval, oldStart);
}

std::vector<StatReading>
CounterAggregator::
read(const std::string & prefix)
{
    double current;
    Date oldStart;

    boost::tie(current, oldStart) = reset();

    if (totalsBuffer.size() >= 10)
        totalsBuffer.pop_front();
    totalsBuffer.push_back(current);

    // Grab the average of the last x seconds to make sure that sparse values
    // show up in at least one of the x second carbon window. (x = 10 for now).
    double value = accumulate(totalsBuffer.begin(), totalsBuffer.end(), 0.0);
    value /= totalsBuffer.size();

    return vector<StatReading>(1, StatReading(prefix, value, start));
}

/*****************************************************************************/
/* GAUGE AGGREGATOR                                                          */
/*****************************************************************************/

GaugeAggregator::
GaugeAggregator(Verbosity verbosity, std::vector<int> extra)
    : verbosity(verbosity), values(new ML::distribution<float>())
    , extra(std::move(extra))
{
    if (verbosity == Outcome)
        ExcCheck(this->extra.size() > 0, "Can not construct with empty percentiles");

    values->reserve(100);
}

GaugeAggregator::
~GaugeAggregator()
{
    delete values;
}

void
GaugeAggregator::
record(float value)
{
    ML::distribution<float> * current = values;
    while ((current = values) == 0 || !cmp_xchg(values, current,
                                     (ML::distribution<float>*)0));

    current->push_back(value);

    memory_barrier();

    values = current;
}

std::pair<ML::distribution<float> *, Date>
GaugeAggregator::
reset()
{
    ML::distribution<float> * current = values;
    ML::distribution<float> * new_current = new ML::distribution<float>();

    // TODO: reserve memory for new_current

    while ((current = values) == 0 || !cmp_xchg(values, current, new_current));

    // Date oldStart = start;
    start = Date::now();

    return make_pair(current, start);
}

std::vector<StatReading>
GaugeAggregator::
read(const std::string & prefix)
{
    ML::distribution<float> * values;
    Date oldStart;

    boost::tie(values, oldStart) = reset();

    std::auto_ptr<ML::distribution<float> > vptr(values);

    if (values->empty())
        return vector<StatReading>();

    vector<StatReading> result;

    auto addMetric = [&] (const char * name, double value)
        {
            result.push_back(StatReading(prefix + "." + name,
                                         value, start));
        };

    auto percentile = [&] (float outOf100) -> double
        {
            int element
                = std::max(0,
                           std::min<int>(values->size() - 1,
                                         outOf100 / 100.0 * values->size()));
            return (*values)[element];
        };

    std::sort(values->begin(), values->end(),
              ML::safe_less<float>());

    if (verbosity == StableLevel)
        result.push_back(StatReading(prefix, values->mean(), start));

    else {
        addMetric("mean", values->mean());
        addMetric("upper", values->back());
        addMetric("lower", values->front());

        if (verbosity == Outcome) {
            addMetric("count", values->size());
            for (int pct: extra) {
                addMetric(ML::format("upper_%d", pct).c_str(), percentile(pct));
            }
        }
    }

    return result;
}

} // namespace Datacratic


#ifndef POSE_ESTIMATOR_HPP
#define POSE_ESTIMATOR_HPP

#include <memory>
#include <boost/optional.hpp>

#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/registration.h>

#include <time.h>

namespace kkl {
  namespace alg {
template<typename T, class System> class UnscentedKalmanFilterX;
  }
}

namespace hdl_localization {

class PoseSystem;
class OdomSystem;

/**
 * @brief scan matching-based pose estimator
 */
class PoseEstimator {
public:
  using PointT = pcl::PointXYZI;

  /**
   * @brief constructor
   * @param registration        registration method
   * @param stamp               timestamp
   * @param pos                 initial position
   * @param quat                initial orientation
   * @param cool_time_duration  during "cool time", prediction is not performed
   */
  PoseEstimator(pcl::Registration<PointT, PointT>::Ptr& registration, const ros::Time& stamp, const Eigen::Vector3f& pos, const Eigen::Quaternionf& quat, double cool_time_duration = 1.0);
  ~PoseEstimator();

  /**
   * @brief predict
   * @param stamp    timestamp
   */
  void predict(const ros::Time& stamp);

  /**
   * @brief predict
   * @param stamp    timestamp
   * @param acc      acceleration
   * @param gyro     angular velocity
   */
  void predict(const ros::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro);

  /**
   * @brief update the state of the odomety-based pose estimation
   */
  void predict_odom(const Eigen::Matrix4f& odom_delta);

  /**
   * @brief correct
   * @param cloud   input cloud
   * @return cloud aligned to the globalmap
   */
  pcl::PointCloud<PointT>::Ptr correct(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud);

  /* getters */
  ros::Time last_correction_time() const;

  Eigen::Vector3f pos() const;
  Eigen::Vector3f vel() const;
  Eigen::Quaternionf quat() const;
  Eigen::Matrix4f matrix() const;

  Eigen::Vector3f odom_pos() const;
  Eigen::Quaternionf odom_quat() const;
  Eigen::Matrix4f odom_matrix() const;

  const boost::optional<Eigen::Matrix4f>& wo_prediction_error() const;
  const boost::optional<Eigen::Matrix4f>& imu_prediction_error() const;
  const boost::optional<Eigen::Matrix4f>& odom_prediction_error() const;

private:
  ros::Time init_stamp;             // when the estimator was initialized
  ros::Time prev_stamp;             // when the estimator was updated last time
  ros::Time last_correction_stamp;  // when the estimator performed the correction step
  double cool_time_duration;        //从init时刻开始算起，过了这段冷却时间后，estimator才开始用imu数据或者常量模型做predict

  Eigen::MatrixXf process_noise;
  std::unique_ptr<kkl::alg::UnscentedKalmanFilterX<float, PoseSystem>> ukf;
  std::unique_ptr<kkl::alg::UnscentedKalmanFilterX<float, OdomSystem>> odom_ukf;

  Eigen::Matrix4f last_observation;
  boost::optional<Eigen::Matrix4f> wo_pred_error;
  boost::optional<Eigen::Matrix4f> imu_pred_error;
  boost::optional<Eigen::Matrix4f> odom_pred_error;

  pcl::Registration<PointT, PointT>::Ptr registration;
  };

}  // namespace hdl_localization

#endif  // POSE_ESTIMATOR_HPP


/**
 * @file gust.cpp
 *
 * Computes wind gusts
 */

#include <boost/lexical_cast.hpp>

#include "forecast_time.h"
#include "gust.h"
#include "level.h"
#include "logger.h"
#include "numerical_functions.h"
#include "plugin_factory.h"
#include <boost/thread.hpp>

#include "hitool.h"
#include "neons.h"
#include "radon.h"
#include <NFmiLocation.h>
#include <NFmiMetTime.h>

using namespace std;
using namespace himan;
using namespace himan::plugin;

struct deltaT
{
	vector<double> deltaT_100;
	vector<double> deltaT_200;
	vector<double> deltaT_300;
	vector<double> deltaT_400;
	vector<double> deltaT_500;
	vector<double> deltaT_600;
	vector<double> deltaT_700;

	deltaT() {}
};

struct deltaTot
{
	vector<double> deltaTot_100;
	vector<double> deltaTot_200;
	vector<double> deltaTot_300;
	vector<double> deltaTot_400;
	vector<double> deltaTot_500;
	vector<double> deltaTot_600;
	vector<double> deltaTot_700;

	deltaTot() {}
};

struct intT
{
	vector<double> intT_100;
	vector<double> intT_200;
	vector<double> intT_300;
	vector<double> intT_400;
	vector<double> intT_500;
	vector<double> intT_600;
	vector<double> intT_700;

	intT() {}
};

struct intTot
{
	vector<double> intTot_100;
	vector<double> intTot_200;
	vector<double> intTot_300;
	vector<double> intTot_400;
	vector<double> intTot_500;
	vector<double> intTot_600;
	vector<double> intTot_700;

	intTot() {}
};

void DeltaT(shared_ptr<const plugin_configuration> conf, info_t T_lowestLevel, const forecast_time& ftime,
            const forecast_type& ftype, size_t gridSize, deltaT& dT);
void DeltaTot(deltaTot& dTot, info_t T_lowestLevel, size_t gridSize);
void IntT(intT& iT, const deltaT& dT, size_t gridSize);
void IntTot(intTot& iTot, const deltaTot& dTot, size_t gridSize);
void LowAndMiddleClouds(vector<double>& lowAndMiddleClouds, info_t lowClouds, info_t middleClouds, info_t highClouds,
                        info_t totalClouds);

gust::gust() { itsLogger = logger("gust"); }
void gust::Process(std::shared_ptr<const plugin_configuration> conf)
{
	Init(conf);

	param theRequestedParam("FFG2-MS", 417, 0, 2, 22);

	theRequestedParam.Unit(kMs);

	SetParams({theRequestedParam});

	Start();
}

/*
 * Calculate()

 * This function does the actual calculation.
 */

void gust::Calculate(shared_ptr<info> myTargetInfo, unsigned short threadIndex)
{
	NFmiMetTime theTime(boost::lexical_cast<short>(myTargetInfo->Time().ValidDateTime().String("%Y")),
	                    boost::lexical_cast<short>(myTargetInfo->Time().ValidDateTime().String("%m")),
	                    boost::lexical_cast<short>(myTargetInfo->Time().ValidDateTime().String("%d")),
	                    boost::lexical_cast<short>(myTargetInfo->Time().ValidDateTime().String("%H")),
	                    boost::lexical_cast<short>(myTargetInfo->Time().ValidDateTime().String("%M")));

	auto myThreadedLogger = logger("gust_pluginThread #" + boost::lexical_cast<string>(threadIndex));

	/*
	 * Required source parameters
	 *
	 */
	const size_t gridSize = myTargetInfo->Grid()->Size();

	const param BLHParam("MIXHGT-M");                                     // boundary layer height
	const param WSParam("FF-MS");                                         // wind speed
	const param GustParam("FFG-MS");                                      // wind gust
	const param TParam("T-K");                                            // temperature
	const param TopoParam("Z-M2S2");                                      // geopotential height
	const params LowCloudParam = {param("NL-PRCNT"), param("NL-0TO1")};   // low cloud cover
	const params MidCloudParam = {param("NM-PRCNT"), param("NM-0TO1")};   // middle cloud cover
	const params HighCloudParam = {param("NH-PRCNT"), param("NH-0TO1")};  // high cloud cover
	const params TotalCloudParam = {param("N-PRCNT"), param("N-0TO1")};   // total cloud cover

	level H0, H10;

	producer prod = itsConfiguration->SourceProducer(0);

	HPDatabaseType dbtype = itsConfiguration->DatabaseType();

	long lowestHybridLevelNumber = kHPMissingInt;

	if (dbtype == kNeons || dbtype == kNeonsAndRadon)
	{
		auto n = GET_PLUGIN(neons);

		lowestHybridLevelNumber = boost::lexical_cast<long>(n->ProducerMetaData(prod.Id(), "last hybrid level number"));
	}

	if ((dbtype == kRadon || dbtype == kNeonsAndRadon) && lowestHybridLevelNumber == kHPMissingInt)
	{
		auto r = GET_PLUGIN(radon);

		lowestHybridLevelNumber =
		    boost::lexical_cast<long>(r->RadonDB().GetProducerMetaData(prod.Id(), "last hybrid level number"));
	}

	assert(lowestHybridLevelNumber != kHPMissingInt);

	level lowestHybridLevel(kHybrid, lowestHybridLevelNumber);

	if (myTargetInfo->Producer().Id() == 240 || myTargetInfo->Producer().Id() == 243)
	{
		H0 = level(kGround, 0);
		H10 = H0;
	}
	else
	{
		H0 = level(kHeight, 0);
		H10 = level(kHeight, 10);
	}

	info_t GustInfo, T_LowestLevelInfo, BLHInfo, TopoInfo, LCloudInfo, MCloudInfo, HCloudInfo, TCloudInfo;

	// Current time and level

	forecast_time forecastTime = myTargetInfo->Time();
	level forecastLevel = myTargetInfo->Level();
	forecast_type forecastType = myTargetInfo->ForecastType();

	GustInfo = Fetch(forecastTime, H10, GustParam, forecastType, false);
	T_LowestLevelInfo = Fetch(forecastTime, lowestHybridLevel, TParam, forecastType, false);
	BLHInfo = Fetch(forecastTime, H0, BLHParam, forecastType, false);
	TopoInfo = Fetch(forecastTime, H0, TopoParam, forecastType, false);
	LCloudInfo = Fetch(forecastTime, H0, LowCloudParam, forecastType, false);
	MCloudInfo = Fetch(forecastTime, H0, MidCloudParam, forecastType, false);
	HCloudInfo = Fetch(forecastTime, H0, HighCloudParam, forecastType, false);
	TCloudInfo = Fetch(forecastTime, H0, TotalCloudParam, forecastType, false);

	if (!GustInfo || !T_LowestLevelInfo || !LCloudInfo || !MCloudInfo || !HCloudInfo || !TCloudInfo)
	{
		itsLogger.Error("Unable to find all source data");
		return;
	}

	deltaT dT;
	vector<double> lowAndMiddleClouds(gridSize, kFloatMissing);

	boost::thread t(&DeltaT, itsConfiguration, T_LowestLevelInfo, forecastTime, forecastType, gridSize, boost::ref(dT));
	boost::thread t2(&LowAndMiddleClouds, boost::ref(lowAndMiddleClouds), LCloudInfo, MCloudInfo, HCloudInfo,
	                 TCloudInfo);

	// calc boundary layer height
	vector<double> z_boundaryl(gridSize, kFloatMissing);
	vector<double> z_one_third_boundaryl(gridSize, kFloatMissing);
	vector<double> z_two_third_boundaryl(gridSize, kFloatMissing);
	vector<double> z_zero(gridSize, 0);
	for (size_t i = 0; i < gridSize; ++i)
	{
		if (BLHInfo->Data()[i] == kFloatMissing)
		{
			continue;
		}

		z_boundaryl[i] = BLHInfo->Data()[i];

		if (BLHInfo->Data()[i] >= 200)
		{
			z_boundaryl[i] = 0.5 * BLHInfo->Data()[i] + 100;

			if (BLHInfo->Data()[i] > 1200)
			{
				z_boundaryl[i] = 700;
			}
		}

		z_one_third_boundaryl[i] = z_boundaryl[i] / 3;
		z_two_third_boundaryl[i] = 2 * z_one_third_boundaryl[i];
	}

	// maybe need adjusting
	auto h = GET_PLUGIN(hitool);

	h->Configuration(itsConfiguration);
	h->Time(forecastTime);
	h->ForecastType(forecastType);

	vector<double> maxEstimate;
	try
	{
		maxEstimate = h->VerticalAverage(WSParam, z_two_third_boundaryl, z_boundaryl);
	}

	catch (const HPExceptionType& e)
	{
		if (e != kFileDataNotFound)
		{
			throw runtime_error("Caught exception " + boost::lexical_cast<string>(e));
		}
		else
		{
			myThreadedLogger.Error("hitool was unable to find data");
			t.join();
			t2.join();
			return;
		}
	}

	vector<double> BLtop_ws;
	try
	{
		BLtop_ws = h->VerticalAverage(WSParam, z_one_third_boundaryl, z_boundaryl);
	}

	catch (const HPExceptionType& e)
	{
		if (e != kFileDataNotFound)
		{
			throw runtime_error("Caught exception " + boost::lexical_cast<string>(e));
		}
		else
		{
			myThreadedLogger.Error("hitool was unable to find data");
			t.join();
			t2.join();
			return;
		}
	}

	vector<double> meanWind;
	try
	{
		meanWind = h->VerticalAverage(WSParam, z_zero, z_boundaryl);
	}

	catch (const HPExceptionType& e)
	{
		if (e != kFileDataNotFound)
		{
			throw runtime_error("Caught exception " + boost::lexical_cast<string>(e));
		}
		else
		{
			myThreadedLogger.Error("hitool was unable to find data");
			t.join();
			t2.join();
			return;
		}
	}

	vector<double> t_diff;
	try
	{
		t_diff = h->VerticalMaximum(TParam, 0, 200);
		for (size_t i = 0; i < gridSize; ++i)
		{
			t_diff[i] = t_diff[i] - T_LowestLevelInfo->Data()[i];
		}
	}

	catch (const HPExceptionType& e)
	{
		if (e != kFileDataNotFound)
		{
			throw runtime_error("Caught exception " + boost::lexical_cast<string>(e));
		}
		else
		{
			myThreadedLogger.Error("hitool was unable to find data");
			t.join();
			t2.join();
			return;
		}
	}

	vector<double> BLbottom_ws;
	try
	{
		BLbottom_ws = h->VerticalAverage(WSParam, 0, 200);
	}

	catch (const HPExceptionType& e)
	{
		if (e != kFileDataNotFound)
		{
			throw runtime_error("Caught exception " + boost::lexical_cast<string>(e));
		}
		else
		{
			myThreadedLogger.Error("hitool was unable to find data");
			t.join();
			t2.join();
			return;
		}
	}

	t.join();
	t2.join();

	deltaTot dTot;
	intT iT;
	intTot iTot;

	DeltaTot(dTot, T_LowestLevelInfo, gridSize);
	IntT(iT, dT, gridSize);
	IntTot(iTot, dTot, gridSize);

	string deviceType = "CPU";

	LOCKSTEP(myTargetInfo, GustInfo, T_LowestLevelInfo, TopoInfo, BLHInfo, LCloudInfo, MCloudInfo)
	{
		size_t i = myTargetInfo->LocationIndex();

		double topo = TopoInfo->Value();
		double esto = kFloatMissing;
		double esto_tot = kFloatMissing;
		double gust = kFloatMissing;
		double turb_lisa = 0;
		double turb_kerroin = 0;
		double pilvikerroin = 0;
		double cloudCover = (LCloudInfo->Value() + MCloudInfo->Value()) * 100;
		topo *= himan::constants::kIg;

		NFmiLocation theLocation(myTargetInfo->LatLon().X(), myTargetInfo->LatLon().Y());
		double elevationAngle = theLocation.ElevationAngle(theTime);

		/* Calculations go here */

		if (z_boundaryl[i] >= 200)
		{
			esto = iT.intT_100[i] + iT.intT_200[i];
			esto_tot = iTot.intTot_100[i] + iTot.intTot_200[i];
		}

		if (z_boundaryl[i] >= 300)
		{
			esto = iT.intT_100[i] + iT.intT_200[i] + iT.intT_300[i];
			esto_tot = iTot.intTot_100[i] + iTot.intTot_200[i] + iTot.intTot_300[i];
		}

		if (z_boundaryl[i] >= 400)
		{
			esto = iT.intT_100[i] + iT.intT_200[i] + iT.intT_300[i] + iT.intT_400[i];
			esto_tot = iTot.intTot_100[i] + iTot.intTot_200[i] + iTot.intTot_300[i] + iTot.intTot_400[i];
		}

		if (z_boundaryl[i] >= 500)
		{
			esto = iT.intT_100[i] + iT.intT_200[i] + iT.intT_300[i] + iT.intT_400[i] + iT.intT_500[i];
			esto_tot =
			    iTot.intTot_100[i] + iTot.intTot_200[i] + iTot.intTot_300[i] + iTot.intTot_400[i] + iTot.intTot_500[i];
		}

		if (z_boundaryl[i] >= 600)
		{
			esto = iT.intT_100[i] + iT.intT_200[i] + iT.intT_300[i] + iT.intT_400[i] + iT.intT_500[i] + iT.intT_600[i];
			esto_tot = iTot.intTot_100[i] + iTot.intTot_200[i] + iTot.intTot_300[i] + iTot.intTot_400[i] +
			           iTot.intTot_500[i] + iTot.intTot_600[i];
		}

		if (z_boundaryl[i] == 700)
		{
			esto = iT.intT_100[i] + iT.intT_200[i] + iT.intT_300[i] + iT.intT_400[i] + iT.intT_500[i] + iT.intT_600[i] +
			       iT.intT_700[i];
			esto_tot = iTot.intTot_100[i] + iTot.intTot_200[i] + iTot.intTot_300[i] + iTot.intTot_400[i] +
			           iTot.intTot_500[i] + iTot.intTot_600[i] + iTot.intTot_700[i];
		}

		if (z_boundaryl[i] >= 200 && esto >= 0 && esto <= esto_tot)
		{
			gust = ((BLbottom_ws[i] - BLtop_ws[i]) / esto_tot) * esto + BLtop_ws[i];
		}

		if (z_boundaryl[i] >= 200 && esto <= 0 && esto >= -200)
		{
			gust = ((BLtop_ws[i] - maxEstimate[i]) / 200) * esto + BLtop_ws[i];
		}

		if (z_boundaryl[i] >= 200 && esto < -200)
		{
			gust = maxEstimate[i];
		}

		if (z_boundaryl[i] >= 200 && esto > esto_tot)
		{
			gust = BLbottom_ws[i];
		}

		if (z_boundaryl[i] < 200)
		{
			gust = meanWind[i];
		}

		if (gust == kFloatMissing || gust < 1)
		{
			gust = 1;
		}

		if (topo > 15 && topo < 400 && t_diff[i] > 0 && t_diff[i] <= 4 && BLbottom_ws[i] < 7 && elevationAngle < 15)
		{
			gust = ((1 - gust) / 4) * t_diff[i] + gust;
		}

		if (topo > 15 && topo < 400 && t_diff[i] > 4 && BLbottom_ws[i] < 7)
		{
			gust = 1;
		}

		if (cloudCover >= 30 && cloudCover <= 70)
		{
			pilvikerroin = -0.025 * cloudCover + 1.75;
		}

		if (cloudCover < 30)
		{
			pilvikerroin = 1;
		}

		if (elevationAngle >= 20 && topo > 10 && z_boundaryl[i] >= 200 && esto < 0)
		{
			turb_lisa = 0.133333333 * elevationAngle - 2.666666667;

			if (elevationAngle > 50)
			{
				turb_lisa = 4;
			}

			if (gust > 4 && gust <= 14)
			{
				turb_kerroin = -0.1 * gust + 1.4;
			}

			if (gust <= 4)
			{
				turb_kerroin = 1;
			}
		}

		gust = gust + turb_lisa * turb_kerroin * pilvikerroin;

		if (topo > 400 || T_LowestLevelInfo->Value() == kFloatMissing || BLHInfo->Value() == kFloatMissing)
		{
			gust = GustInfo->Value() * 0.95;
		}

		myTargetInfo->Value(gust);
	}

	himan::matrix<double> filter_kernel(3, 3, 1, kFloatMissing);
	filter_kernel.Fill(1.0 / 9.0);
	himan::matrix<double> gust_filtered = numerical_functions::Filter2D(myTargetInfo->Data(), filter_kernel);

	myTargetInfo->Grid()->Data(gust_filtered);

	myThreadedLogger.Info("[" + deviceType + "] Missing values: " +
						  boost::lexical_cast<string>(myTargetInfo->Data().MissingCount()) + "/" +
						  boost::lexical_cast<string>(myTargetInfo->Data().Size()));
}

void DeltaT(shared_ptr<const plugin_configuration> conf, info_t T_lowestLevel, const forecast_time& ftime,
            const forecast_type& ftype, size_t gridSize, deltaT& dT)
{
	auto h = GET_PLUGIN(hitool);

	h->Configuration(conf);
	h->Time(ftime);
	h->ForecastType(ftype);

	const param TParam("T-K");

	try
	{
		// Potential temperature differences
		dT.deltaT_100 = h->VerticalValue(TParam, 100);
		dT.deltaT_200 = h->VerticalValue(TParam, 200);
		dT.deltaT_300 = h->VerticalValue(TParam, 300);
		dT.deltaT_400 = h->VerticalValue(TParam, 400);
		dT.deltaT_500 = h->VerticalValue(TParam, 500);
		dT.deltaT_600 = h->VerticalValue(TParam, 600);
		dT.deltaT_700 = h->VerticalValue(TParam, 700);

		for (size_t i = 0; i < gridSize; ++i)
		{
			if (T_lowestLevel->Data()[i] == himan::kFloatMissing)
			{
				dT.deltaT_100[i] = himan::kFloatMissing;
				dT.deltaT_200[i] = himan::kFloatMissing;
				dT.deltaT_300[i] = himan::kFloatMissing;
				dT.deltaT_400[i] = himan::kFloatMissing;
				dT.deltaT_500[i] = himan::kFloatMissing;
				dT.deltaT_600[i] = himan::kFloatMissing;
				dT.deltaT_700[i] = himan::kFloatMissing;
				continue;
			}
			if (dT.deltaT_100[i] != himan::kFloatMissing)
				dT.deltaT_100[i] -= T_lowestLevel->Data()[i] + 9.8 * (0.010 - (100.0 / 1000.0));
			if (dT.deltaT_200[i] != himan::kFloatMissing)
				dT.deltaT_200[i] -= T_lowestLevel->Data()[i] + 9.8 * (0.010 - (200.0 / 1000.0));
			if (dT.deltaT_300[i] != himan::kFloatMissing)
				dT.deltaT_300[i] -= T_lowestLevel->Data()[i] + 9.8 * (0.010 - (300.0 / 1000.0));
			if (dT.deltaT_400[i] != himan::kFloatMissing)
				dT.deltaT_400[i] -= T_lowestLevel->Data()[i] + 9.8 * (0.010 - (400.0 / 1000.0));
			if (dT.deltaT_500[i] != himan::kFloatMissing)
				dT.deltaT_500[i] -= T_lowestLevel->Data()[i] + 9.8 * (0.010 - (500.0 / 1000.0));
			if (dT.deltaT_600[i] != himan::kFloatMissing)
				dT.deltaT_600[i] -= T_lowestLevel->Data()[i] + 9.8 * (0.010 - (600.0 / 1000.0));
			if (dT.deltaT_700[i] != himan::kFloatMissing)
				dT.deltaT_700[i] -= T_lowestLevel->Data()[i] + 9.8 * (0.010 - (700.0 / 1000.0));
		}
	}
	catch (const HPExceptionType& e)
	{
		if (e != kFileDataNotFound)
		{
			throw runtime_error("DeltaT() caught exception " + boost::lexical_cast<string>(e));
		}
	}
}

void DeltaTot(deltaTot& dTot, info_t T_lowestLevel, size_t gridSize)
{
	dTot.deltaTot_100 = vector<double>(gridSize, 0);
	dTot.deltaTot_200 = vector<double>(gridSize, 0);
	dTot.deltaTot_300 = vector<double>(gridSize, 0);
	dTot.deltaTot_400 = vector<double>(gridSize, 0);
	dTot.deltaTot_500 = vector<double>(gridSize, 0);
	dTot.deltaTot_600 = vector<double>(gridSize, 0);
	dTot.deltaTot_700 = vector<double>(gridSize, 0);

	for (size_t i = 0; i < gridSize; ++i)
	{
		dTot.deltaTot_100[i] =
		    (T_lowestLevel->Data()[i] + 6 * (0.010 - (100.0 / 1000.0))) -
		    (T_lowestLevel->Data()[i] + 9.8 * (0.010 - (100.0 / 1000.0)));  // => -3.8*(0.010-(100/1000)) ???
		dTot.deltaTot_200[i] =
		    (T_lowestLevel->Data()[i] + 6 * (0.010 - (200.0 / 1000.0))) -
		    (T_lowestLevel->Data()[i] + 9.8 * (0.010 - (200.0 / 1000.0)));  // => -3.8*(0.010-(200/1000))
		dTot.deltaTot_300[i] = (T_lowestLevel->Data()[i] + 6 * (0.010 - (300.0 / 1000.0))) -
		                       (T_lowestLevel->Data()[i] + 9.8 * (0.010 - (300.0 / 1000.0)));  // ... etc.
		dTot.deltaTot_400[i] = (T_lowestLevel->Data()[i] + 6 * (0.010 - (400.0 / 1000.0))) -
		                       (T_lowestLevel->Data()[i] + 9.8 * (0.010 - (400.0 / 1000.0)));
		dTot.deltaTot_500[i] = (T_lowestLevel->Data()[i] + 6 * (0.010 - (500.0 / 1000.0))) -
		                       (T_lowestLevel->Data()[i] + 9.8 * (0.010 - (500.0 / 1000.0)));
		dTot.deltaTot_600[i] = (T_lowestLevel->Data()[i] + 6 * (0.010 - (600.0 / 1000.0))) -
		                       (T_lowestLevel->Data()[i] + 9.8 * (0.010 - (600.0 / 1000.0)));
		dTot.deltaTot_700[i] = (T_lowestLevel->Data()[i] + 6 * (0.010 - (700.0 / 1000.0))) -
		                       (T_lowestLevel->Data()[i] + 9.8 * (0.010 - (700.0 / 1000.0)));
	}
}

void IntT(intT& iT, const deltaT& dT, size_t gridSize)
{
	iT.intT_100 = vector<double>(gridSize, himan::kFloatMissing);
	iT.intT_200 = vector<double>(gridSize, himan::kFloatMissing);
	iT.intT_300 = vector<double>(gridSize, himan::kFloatMissing);
	iT.intT_400 = vector<double>(gridSize, himan::kFloatMissing);
	iT.intT_500 = vector<double>(gridSize, himan::kFloatMissing);
	iT.intT_600 = vector<double>(gridSize, himan::kFloatMissing);
	iT.intT_700 = vector<double>(gridSize, himan::kFloatMissing);

	for (size_t i = 0; i < gridSize; ++i)
	{
		if (dT.deltaT_100[i] != himan::kFloatMissing)
			iT.intT_100[i] = 0.5 * dT.deltaT_100[i] * 100;  // why 0.5 * here? Would make sense in case of
		                                                    // (dT.deltaT_100[i] + dT.deltaT_0[i]) if this is
		                                                    // integration using trapezoidal rule
		if (dT.deltaT_100[i] != himan::kFloatMissing && dT.deltaT_200[i] != himan::kFloatMissing)
			iT.intT_200[i] = 0.5 * (dT.deltaT_200[i] + dT.deltaT_100[i]) * 100;
		if (dT.deltaT_200[i] != himan::kFloatMissing && dT.deltaT_300[i] != himan::kFloatMissing)
			iT.intT_300[i] = 0.5 * (dT.deltaT_300[i] + dT.deltaT_200[i]) * 100;
		if (dT.deltaT_300[i] != himan::kFloatMissing && dT.deltaT_400[i] != himan::kFloatMissing)
			iT.intT_400[i] = 0.5 * (dT.deltaT_400[i] + dT.deltaT_300[i]) * 100;
		if (dT.deltaT_400[i] != himan::kFloatMissing && dT.deltaT_500[i] != himan::kFloatMissing)
			iT.intT_500[i] = 0.5 * (dT.deltaT_500[i] + dT.deltaT_400[i]) * 100;
		if (dT.deltaT_500[i] != himan::kFloatMissing && dT.deltaT_600[i] != himan::kFloatMissing)
			iT.intT_600[i] = 0.5 * (dT.deltaT_600[i] + dT.deltaT_500[i]) * 100;
		if (dT.deltaT_600[i] != himan::kFloatMissing && dT.deltaT_700[i] != himan::kFloatMissing)
			iT.intT_700[i] = 0.5 * (dT.deltaT_700[i] + dT.deltaT_600[i]) * 100;
	}
}

void IntTot(intTot& iTot, const deltaTot& dTot, size_t gridSize)
{
	iTot.intTot_100 = vector<double>(gridSize, 0);
	iTot.intTot_200 = vector<double>(gridSize, 0);
	iTot.intTot_300 = vector<double>(gridSize, 0);
	iTot.intTot_400 = vector<double>(gridSize, 0);
	iTot.intTot_500 = vector<double>(gridSize, 0);
	iTot.intTot_600 = vector<double>(gridSize, 0);
	iTot.intTot_700 = vector<double>(gridSize, 0);

	for (size_t i = 0; i < gridSize; ++i)
	{
		iTot.intTot_100[i] = 0.5 * dTot.deltaTot_100[i] * 100;  // 0.5 * ???
		iTot.intTot_200[i] = 0.5 * (dTot.deltaTot_200[i] + dTot.deltaTot_100[i]) * 100;
		iTot.intTot_300[i] = 0.5 * (dTot.deltaTot_300[i] + dTot.deltaTot_200[i]) * 100;
		iTot.intTot_400[i] = 0.5 * (dTot.deltaTot_400[i] + dTot.deltaTot_300[i]) * 100;
		iTot.intTot_500[i] = 0.5 * (dTot.deltaTot_500[i] + dTot.deltaTot_400[i]) * 100;
		iTot.intTot_600[i] = 0.5 * (dTot.deltaTot_600[i] + dTot.deltaTot_500[i]) * 100;
		iTot.intTot_700[i] = 0.5 * (dTot.deltaTot_700[i] + dTot.deltaTot_600[i]) * 100;
	}
}

void LowAndMiddleClouds(vector<double>& lowAndMiddleClouds, info_t lowClouds, info_t middleClouds, info_t highClouds,
                        info_t totalClouds)
{
	for (size_t i = 0; i < lowAndMiddleClouds.size(); ++i)
	{
		if (highClouds->Data()[i] == himan::kFloatMissing || highClouds->Data()[i] == 0.0)
		{
			if (totalClouds->Data()[i] == himan::kFloatMissing)
				lowAndMiddleClouds[i] = himan::kFloatMissing;
			else
				lowAndMiddleClouds[i] = totalClouds->Data()[i] * 100;
		}
		else
		{
			if (lowClouds->Data()[i] != himan::kFloatMissing)
			{
				if (middleClouds->Data()[i] != himan::kFloatMissing)
					lowAndMiddleClouds[i] = max(lowClouds->Data()[i], middleClouds->Data()[i]) * 100;
				else
					lowAndMiddleClouds[i] = lowClouds->Data()[i] * 100;
			}
			else
			{
				if (middleClouds->Data()[i] == himan::kFloatMissing)
					lowAndMiddleClouds[i] = himan::kFloatMissing;
				else
					lowAndMiddleClouds[i] = middleClouds->Data()[i] * 100;
			}
		}
	}
}


#include <lwr_controllers/inverse_dynamics_controller.h>

#include <pluginlib/class_list_macros.h>
#include <urdf/model.h>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>

#include <Eigen/Dense>

namespace lwr_controllers
{

  InverseDynamicsController::InverseDynamicsController()
    : robot_description_("")
      ,root_name_("")
      ,tip_name_("")
      ,joint_names_()
      ,id_solver_(NULL)
      ,ext_wrenches_()
      ,joint_states_()
      ,torques_()
  {}

  InverseDynamicsController::~InverseDynamicsController()
  {
    ext_wrench_sub_.shutdown();
  }

  bool InverseDynamicsController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
  {
    KinematicChainControllerBase<hardware_interface::EffortJointInterface>::init(robot, n);

    //Create inverse dynamics solver
    id_solver_.reset( new KDL::ChainIdSolver_RNE( kdl_chain_, gravity_) );

    // Resize working vectors
    joint_states_.resize(n_dof_);
    torques_.resize(n_dof_);
    ext_wrenches_.resize(kdl_chain_.getNrOfSegments());

    // Zero out torque data
    torques_.data.setZero();

    // Subscribe to external wrench topic
    ext_wrench_sub_ = nh_.subscribe<geometry_msgs::WrenchStamped>(
        "ext_wrench", 1,
        &InverseDynamicsController::ext_wrench_cb, this);

    return true;
  }

  void InverseDynamicsController::update(const ros::Time& time, const ros::Duration& period)
  {
    // Read the positions/velocities
    for(unsigned int j=0; j<n_dof_; j++) {
      joint_states_.q(j) = joint_handles_[j].getPosition();
      joint_states_.qdot(j) = joint_handles_[j].getVelocity();
      // JointStateInterface has no acceleration support
      // joint_states_.qdotdot(j) = joint_handles_[j].getAcceleration();
      joint_states_.qdotdot(j) = 0.0;
    }

    // Compute inverse dynamics
    // This computes the torques on each joint of the arm as a function of
    // the arm's joint-space position, velocities, accelerations, external
    // forces/torques and gravity.
    if(id_solver_->CartToJnt(
            joint_states_.q,
            joint_states_.qdot,
            joint_states_.qdotdot,
            ext_wrenches_,
            torques_) != 0)
    {
      ROS_ERROR("Could not compute joint torques! Setting all torques to zero!");
      KDL::SetToZero(torques_);
    }

    // Set the commands
    for(unsigned int j=0; j<n_dof_; j++) {
      	joint_handles_[j].setCommand(torques_(j));
    	//joint_handles_[j].setCommand(0);
    }
  }

  void InverseDynamicsController::ext_wrench_cb(
      const geometry_msgs::WrenchStampedConstPtr &wrench_msg)
  {
    // TODO: Transform wrench into appropriate frame (probably local frame for each link)

    // Convert to KDL
    KDL::Wrench wrench(
        KDL::Vector(wrench_msg->wrench.force.x,
                    wrench_msg->wrench.force.y,
                    wrench_msg->wrench.force.z),
        KDL::Vector(wrench_msg->wrench.torque.x,
                    wrench_msg->wrench.torque.y,
                    wrench_msg->wrench.torque.z));

    // Set the wrench on the tip link
    ext_wrenches_.back() = wrench;
  }

}

PLUGINLIB_EXPORT_CLASS(lwr_controllers::InverseDynamicsController, controller_interface::ControllerBase)