#include <stdlib.h>
#include <cmath>
#include <vector>
#include <queue>
#include <chrono>
#include <cstdlib>
#include <cstdio>
#include <Eigen/Geometry>

#include "RenderClient.h"
#include "RenderCommands.h"
#include "UnityOde.h"
#include "UnityOde_internal.h"

#include "mathutils.h"
#include "SCAController.h"
#include "FileUtils.h"

using namespace std::chrono;
using namespace AaltoGames;
using namespace Eigen;
#include "Biped.h"
#include "BipedLegsOnly.h"
#include "Quadruped.h"
#include "Monoped.h"

#define MONOPED 1
#define LEGS 2
#define HUMANOID 3
#define QUADRUPED 4

#define CHARACTER HUMANOID

#if CHARACTER == MONOPED
//Monoped-specific parameters
typedef Monoped TestRig;
static const float controlAccSd = deg2rad*10.0f;
static const float angleSd = deg2rad*20.0f;  //cost function
static const int nTrajectories = 256;
static const int nRealtimeTrajectories = 64;
int non_groung_contact_bones[] = { 1, 0 };
int feet_bones[] = { 2 };
#endif

#if CHARACTER == LEGS
//Legs only biped specific parameters
typedef BipedLegsOnly TestRig;
static const float controlAccSd = deg2rad*10.0f;
static const float angleSd = deg2rad*20.0f;  //cost function
static const int nTrajectories = 64;
static const int nRealtimeTrajectories = 16;
int non_groung_contact_bones[] = { (int)TestRig::Bones::bLeftThigh,(int)TestRig::Bones::bRightThigh,(int)TestRig::Bones::bLeftShin,(int)TestRig::Bones::bRightShin };
int feet_bones[] = { (int)TestRig::Bones::bLeftFoot,(int)TestRig::Bones::bRightFoot };
#endif

#if CHARACTER == HUMANOID
//Full humanoid biped specific parameters
typedef Biped TestRig;
static const float controlAccSd = deg2rad*10.0f;
static const float angleSd = deg2rad*20.0f;  //cost function
static const int nTrajectories = 64;
static const int nRealtimeTrajectories = 16;
int non_groung_contact_bones[] = { (int)TestRig::Bones::bPelvis,(int)TestRig::Bones::bLeftThigh,(int)TestRig::Bones::bLeftShin,(int)TestRig::Bones::bRightThigh,(int)TestRig::Bones::bRightShin,(int)TestRig::Bones::bSpine,(int)TestRig::Bones::bHead,(int)TestRig::Bones::bLeftUpperArm,(int)TestRig::Bones::bLeftForeArm,(int)TestRig::Bones::bRightUpperArm,(int)TestRig::Bones::bRightForeArm };
int feet_bones[] = { (int)TestRig::Bones::bLeftFoot,(int)TestRig::Bones::bRightFoot };
#endif

#if CHARACTER == QUADRUPED
//Quadruped-specific parameters
typedef Quadruped TestRig;
static const float controlAccSd = deg2rad*10.0f;
static const float angleSd = deg2rad*20.0f;  //cost function
static const int nTrajectories = 64;
static const int nRealtimeTrajectories = 16;
int non_groung_contact_bones[] = { (int)TestRig::Bones::bPelvis,(int)TestRig::Bones::bLeftThigh,(int)TestRig::Bones::bRightThigh,(int)TestRig::Bones::bLeftUpperArm,(int)TestRig::Bones::bRightUpperArm };
int feet_bones[] = { (int)TestRig::Bones::bLeftForeArm,(int)TestRig::Bones::bRightForeArm,(int)TestRig::Bones::bLeftShin,(int)TestRig::Bones::bRightShin };
#endif

//Common parameters
static int num_motor_angles = 0;
static const float maxDistanceFromOrigin = 5.0f;
static const float rad2deg = 1.0f / deg2rad;
static const bool rigTestMode = false;
static const float poseSpringConstant = 10.0f;
static TestRig character;
static VectorXf controlMin, controlMax, controlRange, controlMean, controlSd, controlDiffSd;
static SCAControl *flc;

static const float planningHorizonSeconds = 1.2f;
static int nTimeSteps = (int)(planningHorizonSeconds / timeStep);
static int nPhysicsPerStep = 1;
static const int fps = (int)(0.5f + 1.0f / timeStep);
static const bool useThreads = true;
static int resetSaveSlot = nTrajectories + 1;
static int masterContext;
static const float kmh2ms = 1.0f / 3.6f;
static float targetSpeed = 1.0f;
static const float ikSd = 0.05f;
static const bool scoreAngles = true;
static const float velSd = 0.05f;
static const bool useContactVelCost = false;
static const float contactVelSd = 0.2f;
static float comDiffSd = 0.025f;
static const float controlSdRelToRange = 8.0f;

static const bool useFastStep = false;
static const float angleSamplingSd = deg2rad*25.0f;  //sampling
static const float resampleThreshold = 2.0f;
static const float mutationSd = 0.1f;
static const float poseTorqueRelSd = 0.5f;
static const float poseTorqueK = maxControlTorque / (1.0f*PI); //we exert maximum torque towards default pose if the angle displacement is 90 degrees
static const float friction = 0.5f;
static int frameIdx = 0;
static int randomSeed = 2;
static const bool multiTask = false;
static bool enableRealTimeMode = true;
static bool realtimeMode = false;
static int stateDim;
static const bool useErrorLearning = false;
static const bool test_real_time_mode = false;

//video capturing
static bool captureVideo = true;
static const int startRealtimeModeAt = 9000;
static int autoExitAt = 5000;
static const int nInitialCapturedFrames = autoExitAt;
static const int nFramesToCaptureEveryMinute = 15 * fps;

//launched spheres
static const bool useSpheres = false;
static const int sphereInterval = 5000;
static int lastSphereLaunchTime = 0;

//acceleration
static const bool useAcceleration = false;
static const float acceleration = 1.0f / 10.0f; //1 m/s in 10 seconds
static const float acceleratedMaxSpeed = 100.0f; //practically inf (meters per second)

static bool use_external_prior = true;
//random impulses
static bool useRandomImpulses = false;
static const float randomImpulseMagnitude = 100.0f;
static const int randomImpulseInterval = 200;

//targeted walking
static bool useWalkTargets = false;
static Vector3f walkTarget = Vector3f(-1000.0f, 0, 0);
static int walk_time = 200;

static enum TestType
{
	STRAIGHT_WALK = 0, GET_UP
};

static const bool read_settings = false;
static TestType test = TestType::STRAIGHT_WALK;

//get up test
static int start_get_up_test_at = 2000;
const std::string fallen_filename = "fallen_position.txt";

//recovery mode
static const bool enableRecoveryMode = false;
static const bool includeRecoveryModeInState = true; //Has to be included as long as the mode directly affects fmax and spring kp
static float recoveryModePoseSdMult = 5.0f;
static float recoveryModeAccSdMult = 1.0f;
static float recoveryModeFmax = defaultFmax*2.0f;
static bool inRecoveryMode = false;
static const float recoveryModeAngleLimit = 30.0f;
static const float recoveryModeSpringKp = springKp*5.0f;

//misc
static int groundPlane = -1;

std::deque<Eigen::VectorXf> control_sequence;
std::deque<Eigen::VectorXf> machine_learning_control_sequence;

static bool run_on_neural_network = false;

static std::string started_at_time_string = "";
static std::vector<std::vector<float>> costs;
static std::vector<std::string> comments;
static bool no_settings_exit = false;

static Eigen::VectorXf minControls;
static Eigen::VectorXf maxControls;

static void write_vector_to_file(const std::string& filename, const std::vector<std::vector<float> >& data, const std::vector<std::string> comments = std::vector<std::string>()) {

	std::ofstream myfile;
	myfile.open(filename);
	myfile.clear();

	for (const std::string& comment_line : comments) {
		myfile << "//" << comment_line << std::endl;
	}

	for (const std::vector<float>& measurement : data) {
		for (unsigned i = 0; i < measurement.size(); i++) {
			myfile << measurement[i];
			if (i < (int)measurement.size() - 1) {
				myfile << ",";
			}
		}
		myfile << std::endl;
	}

	myfile.close();

}

static void write_deque_of_eigen_to_file(const std::string& filename, const std::deque<Eigen::VectorXf >& data, const std::vector<std::string> comments = std::vector<std::string>()) {

	std::ofstream myfile;
	myfile.open(filename);
	myfile.clear();

	for (const std::string& comment_line : comments) {
		myfile << "//" << comment_line << std::endl;
	}

	for (const Eigen::VectorXf& measurement : data) {
		for (unsigned i = 0; i < measurement.size(); i++) {
			myfile << measurement[i];
			if (i < (int)measurement.size() - 1) {
				myfile << ",";
			}
		}
		myfile << std::endl;
	}

	myfile.close();

}

//timing
high_resolution_clock::time_point t1;
void startPerfCount()
{
	t1 = high_resolution_clock::now();
}
int getDurationMs()
{
	high_resolution_clock::time_point t2 = high_resolution_clock::now();

	std::chrono::duration<double> time_span = duration_cast<std::chrono::duration<double>>(t2 - t1);
	return (int)(time_span.count()*1000.0);
}

class SphereData
{
public:
	int body, geom, spawnFrame;
};
static std::vector<SphereData> spheres;

class SimulationContext
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;  //needed as we have fixed size eigen matrices as members
	VectorXf stateFeatures;
	VectorXf control;
	VectorXf priorMean, priorSd;
	VectorXf angleRates;
	Vector3f initialPosition, resultPosition;
	float stateCost;
	float controlCost;
	int trajectoryIdx;
};

static SimulationContext contexts[nTrajectories + 2];

Vector3f get_target_dir(Vector3f& com) {

	Vector3f dir = walkTarget - com;
	dir.normalize();
	return dir;
}

//state vector seen by the controller, including both character's physical state and task information
int computeStateVector(float *out_state)
{

	Vector3f com;
	character.computeCOM(com);

	Vector3f target_dir = get_target_dir(com);
	const Vector3f initialDir(-1, 0, 0);
	Quaternionf targetRotation = Quaternionf::FromTwoVectors(initialDir, target_dir);
	Quaternionf stateRotation = targetRotation.inverse();

	const bool use_motor_angles = false;

	int nState = 0;
	if (!use_motor_angles) {
		nState = character.computeStateVector(out_state, stateRotation);

		//task variables scaled larger so that they dominate the state distances
		out_state[nState++] = targetSpeed*10.0f;

	}

	if (use_motor_angles) {

		//since ground contacts are highly significant, add foot bone tip y pos and vel with greater weight
		for (int i = 0; i <= 1; i++)
		{
			Vector3f footPos = character.getFootPos((OdeRig::BodySides)i);
			out_state[nState] = footPos.z();
			nState++;
		}

		Vector3f tmp = com;
		out_state[nState] = tmp.z();
		nState++;

		character.computeMeanVel(tmp);
		out_state[nState] = tmp.z();
		nState++;

		Quaternionf q(odeBodyGetQuaternion(character.bones[0]->body));
		Quaternionf root_rotation = stateRotation*q;

		out_state[nState] = root_rotation.x();
		nState++;
		out_state[nState] = root_rotation.y();
		nState++;
		out_state[nState] = root_rotation.z();
		nState++;
		out_state[nState] = root_rotation.w();
		nState++;

		if (num_motor_angles == 0) {
			for (auto joint_ptr : character.joints) {
				num_motor_angles += joint_ptr->nMotorDof;
			}
		}

		float motor_angles[100];
		character.getCurrentMotorAngles(motor_angles);

		for (int motor_angle = 0; motor_angle < num_motor_angles; motor_angle++) {
			out_state[nState] = motor_angles[motor_angle];
			nState++;
		}

		character.getCurrentAngleRates(motor_angles);

		for (int motor_angle = 0; motor_angle < num_motor_angles; motor_angle++) {
			out_state[nState] = motor_angles[motor_angle];
			nState++;
		}

		//task variables scaled larger so that they dominate the state distances
		out_state[nState++] = target_dir.norm()*10.0f;

	}

	if (enableRecoveryMode && includeRecoveryModeInState  && flc->sampling_mode_ == SCAControl::SamplingMode::PRODUCT_OF_GAUSSIANS)
	{
		out_state[nState++] = 10.0f*(inRecoveryMode ? 1.0f : 0);  //needed as state cost computed differently, and recovery mode affects the state,action -> next state mapping (through spring constants and fmax)
	}

	if (useSpheres)
	{

		bool pushZeros = true;
		if (spheres.size() > 0)
		{
			//add the relative position of the last launched sphere
			SphereData &sd = spheres.back();
			Vector3f spherePos(odeBodyGetPosition(sd.body));
			Vector3f sphereVel(odeBodyGetLinearVel(sd.body));
			Vector3f pos(odeBodyGetPosition(character.bones[0]->body));
			Vector3f relPos = stateRotation*(spherePos - pos);
			Vector3f relVel = stateRotation*sphereVel;
			//if sphere flying towards us, add it to the state
			if (relPos.norm() < 5.0f && relVel.dot(relPos.normalized()) < -1.0f)
			{
				character.pushStateVector3f(nState, out_state, relPos);
				character.pushStateVector3f(nState, out_state, relVel);
				pushZeros = false;
			}
		}
		if (pushZeros)
		{
			character.pushStateVector3f(nState, out_state, Vector3f::Zero());
			character.pushStateVector3f(nState, out_state, Vector3f::Zero());
		}
	}
	return nState;
}

std::chrono::time_point<std::chrono::system_clock> start, end;

Eigen::VectorXf init_motor_angles;

static int character_root_bone = -1;

static int orig_ml_trajectories;
static int orig_noisy_ml_trajectories;
static int orig_nearest_neighbor_trajectories;

void print_sampling_mode(void) {
	switch (flc->sampling_mode_)
	{
	case AaltoGames::SCAControl::PRODUCT_OF_GAUSSIANS:
		std::cout << "Sampling scheme: Product of distributions" << std::endl;
		break;
	case AaltoGames::SCAControl::MINIMIZING_INFORMATION_LOSS:
		std::cout << "Sampling scheme: Maximum entropy" << std::endl;
		break;
	default:
		std::cout << "Error in sampling scheme." << std::endl;
		break;
	}
}

void print_nonlinearity(void) {

	switch (flc->network_type_)
	{
	case AaltoGames::SCAControl::BSELU:
		std::cout << "BSELU" << std::endl;
		break;
	case AaltoGames::SCAControl::ELU:
		std::cout << "ELU" << std::endl;
		break;
	default:
		std::cout << "Error in network type." << std::endl;
		break;
	}

}

void EXPORT_API rcInit()
{

#if CHARACTER == HUMANOID
	character_root_bone = (int)TestRig::Bones::bSpine;
#endif

#if CHARACTER == MONOPED
	comDiffSd *= 100000.0f;
#endif

	flc = new SCAControl();

	if (read_settings) {

		std::ifstream infile("settings.txt");

		std::string comment = "//";

		std::deque<std::string> settings;

		for (std::string line; getline(infile, line); )
		{

			if (line.find(comment) != std::string::npos) {
				continue;
			}

			settings.push_back(line);

		}

		infile.close();

		if (settings.size() < 1) {
			autoExitAt = 0;
			return;
		}

		for (std::string setting : settings) {

			std::stringstream stream(setting);
			std::string key = "";
			std::string value = "";
			std::getline(stream, key, ':');
			std::getline(stream, value, '\n');

			if ("MACHINE_LEARNING" == key) {
				std::stringstream value_stream(value);
				value_stream >> flc->use_machine_learning_;
			}

			if ("NEAREST_NEIGHBOR" == key) {
				std::stringstream value_stream(value);
				value_stream >> flc->use_forests_;
			}

			if ("SAMPLING_MODE" == key) {

				if (value.compare("kl") == 0) {
					flc->sampling_mode_ = SCAControl::SamplingMode::MINIMIZING_INFORMATION_LOSS;
				}

				if (value.compare("l1") == 0) {
					flc->sampling_mode_ = SCAControl::SamplingMode::PRODUCT_OF_GAUSSIANS;
				}
			}

			if ("NETWORK_TYPE" == key) {
				if (value.compare("bselu") == 0) {
					flc->network_type_ = SCAControl::BSELU;
				}

				if (value.compare("elu") == 0) {
					flc->network_type_ = SCAControl::ELU;
				}
			}

			if ("TEST_TYPE" == key) {

				test = TestType::STRAIGHT_WALK;

				int comparison = value.compare("get_up");

				if (comparison == 0) {
					test = TestType::GET_UP;
					std::cout << "Get up test chosen." << std::endl;
				}

			}

			if ("LAYERS" == key) {
				std::stringstream value_stream(value);
				value_stream >> flc->amount_network_layers_;
			}

			if ("DROPOUT" == key) {
				std::stringstream value_stream(value);
				value_stream >> flc->drop_out_stdev_;
			}

			if ("STOCHASTICITY_DIM" == key) {
				std::stringstream value_stream(value);
				value_stream >> flc->input_noise_dim_;
			}

			if ("ZERO_SAMPLES" == key) {
				std::stringstream value_stream(value);
				value_stream >> flc->zero_samples_;
			}

		}

		std::ofstream outfile("settings.txt", std::ofstream::out);
		outfile.clear();
		outfile.close();

		captureVideo = false;
		useWalkTargets = false;
		flc->force_old_best_valid_ = true;

		print_sampling_mode();
		print_nonlinearity();

	};

	if (test == TestType::STRAIGHT_WALK) {
		std::cout << "Straight walk test." << std::endl;
	}
	if (test == TestType::GET_UP) {
		std::cout << "Get up test." << std::endl;
	}

	if (test_real_time_mode) {
		enableRealTimeMode = true;
	}

	started_at_time_string = get_time_string();
	unsigned seed = time(nullptr);
	srand(seed);

	costs.clear();
	comments.clear();

	initOde(nTrajectories + 2);
	setCurrentOdeContext(ALLTHREADS);
	allocateODEDataForThread();
	odeRandSetSeed(randomSeed);
	// create world
	odeWorldSetGravity(0, 0, -9.81f);
	odeWorldSetCFM(1e-5);
	odeSetFrictionCoefficient(friction);

	odeWorldSetContactMaxCorrectingVel(5);
	odeWorldSetContactSurfaceLayer(0.01f);
	groundPlane = odeCreatePlane(0, 0, 0, 1, 0);
	odeGeomSetCategoryBits(groundPlane, 0xf0000000);
	odeGeomSetCollideBits(groundPlane, 0xffffffff);
	character.init(false, rigTestMode);
	int nLegs = character.numberOfLegs();

	//controller init (motor target velocities are the controlled variables, one per joint)
	float tmp[1024];
	stateDim = computeStateVector(tmp);
	controlMin = Eigen::VectorXf::Zero(character.controlDim);
	controlMax = Eigen::VectorXf::Zero(character.controlDim);
	controlMean = Eigen::VectorXf::Zero(character.controlDim);
	controlSd = Eigen::VectorXf::Zero(character.controlDim);
	controlDiffSd = Eigen::VectorXf::Zero(character.controlDim);

	int controlVarIdx = 0;
	for (size_t i = 0; i < character.joints.size(); i++)
	{
		OdeRig::Joint *j = character.joints[i];
		for (int dofIdx = 0; dofIdx < j->nMotorDof; dofIdx++)
		{

			controlMin[controlVarIdx] = j->angleMin[dofIdx];
			controlMax[controlVarIdx] = j->angleMax[dofIdx];
			controlSd[controlVarIdx] = angleSamplingSd;

			controlMean[controlVarIdx] = 0;
			controlDiffSd[controlVarIdx] = controlSd[controlVarIdx];
			controlVarIdx++;
		}
	}

	controlRange = controlMax - controlMin;

	flc->amount_data_in_tree_ = 2 * 60 * 30; //2 minutes

	flc->init(nTrajectories, nTimeSteps / nPhysicsPerStep, stateDim, character.controlDim, controlMin.data(), controlMax.data(), controlMean.data(), controlSd.data(), controlDiffSd.data(), mutationSd, false);
	flc->no_prior_trajectory_portion_ = 0.25f;
	flc->learning_budget_ = 2000;
	if (useWalkTargets) {
		flc->learning_budget_ = 10000;
	}

	minControls = flc->control_min_;
	maxControls = flc->control_max_;

	for (int i = 0; i < nTrajectories + 1; i++)	//+1 because of master context
	{
		contexts[i].stateFeatures.resize(stateDim);
		contexts[i].control.resize(character.controlDim);
		contexts[i].trajectoryIdx = i;
		contexts[i].priorMean.resize(character.controlDim);
		contexts[i].priorSd.resize(character.controlDim);
		contexts[i].angleRates.resize(character.controlDim);
	}

	masterContext = nTrajectories;
	setCurrentOdeContext(masterContext);
	stepOde(timeStep, true); //allow time for joint limits to set
	stepOde(timeStep, true); //allow time for joint limits to set
	stepOde(timeStep, true); //allow time for joint limits to set
	character.saveInitialPose();
	saveOdeState(resetSaveSlot, masterContext);
	saveOdeState(masterContext, masterContext);

	float tmp_angles[1000];
	int amount_motor_angles = character.getCurrentMotorAngles(tmp_angles);

	init_motor_angles.resize(amount_motor_angles);
	for (int i = 0; i < amount_motor_angles; ++i) {
		init_motor_angles[i] = tmp_angles[i];
	}

	orig_ml_trajectories = flc->machine_learning_samples_;
	orig_noisy_ml_trajectories = flc->noisy_machine_learning_samples_;
	orig_nearest_neighbor_trajectories = flc->nn_trajectories_;

	start = std::chrono::system_clock::now();

}

void EXPORT_API rcUninit()
{
	delete flc;
	uninitOde();
}

void EXPORT_API rcGetClientData(RenderClientData &data)
{
	data.physicsTimeStep = timeStep*(float)nPhysicsPerStep;
	data.maxAllowedTimeStep = data.physicsTimeStep; //render every step
	data.defaultMouseControlsEnabled = true;
}

static int dofIdx = 0;
static float rigTestControl[32] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 };
static bool paused = false;
static bool applyImpulse = false;

void throwSphere()
{
	//launch a ball
	setCurrentOdeContext(ALLTHREADS);
	float r = 0.33f;
	SphereData sd;
	sd.geom = odeCreateSphere(r);
	sd.body = odeBodyCreate();
	sd.spawnFrame = frameIdx;
	odeGeomSetBody(sd.geom, sd.body);
	dMass mass;
	odeMassSetSphereTotal(sd.body, 50.0f, r);
	setCurrentOdeContext(masterContext);
	restoreOdeState(masterContext);
	Vector3f characterPos(odeBodyGetPosition(character.bones[0]->body));
	Vector3f throwDir(randomf() - 0.5f, randomf() - 0.5f, 0);
	throwDir.normalize();
	Vector3f vel;
	character.computeMeanVel(vel);
	Vector3f spawnPos = characterPos + vel - 5.0f*throwDir;  //Add vel, as the ball will hit in 1 second
	spawnPos.z() = 2.0f;
	odeBodySetPosition(sd.body, spawnPos.x(), spawnPos.y(), spawnPos.z());
	Vector3f spawnVel = throwDir*5.0f;
	spawnVel.z() = 3.5f;
	odeBodySetLinearVel(sd.body, spawnVel.x(), spawnVel.y(), spawnVel.z());
	saveOdeState(masterContext, masterContext);
	spheres.push_back(sd);
	lastSphereLaunchTime = frameIdx;
}

static void write_vectors_to_file(const std::string& filename, const std::deque<Eigen::VectorXf>& data, const std::vector<std::string> comments = std::vector<std::string>()) {

	std::ofstream myfile;
	myfile.open(filename);

	for (const std::string& comment_line : comments) {
		myfile << "//" << comment_line << std::endl;
	}

	for (const Eigen::VectorXf& datum : data) {
		int size = datum.size();
		for (int i = 0; i < size; i++) {
			myfile << datum[i];
			if (i < size - 1) {
				myfile << ",";
			}
		}
		myfile << std::endl;
	}

	myfile.close();

}

static bool switchTargets = false; //if true, toggles target switching at next rcUpdate()
void EXPORT_API rcOnKeyDown(int key)
{
	if (key == 'p')
		paused = !paused;

	if (key == '2')
	{
		dofIdx = (dofIdx + 1) % character.nTotalDofs;
		printf("Adjusting dof %d\n", dofIdx);
	}

	if (key == '1')
	{
		dofIdx = (dofIdx - 1 + character.nTotalDofs) % character.nTotalDofs;
		printf("Adjusting dof %d\n", dofIdx);
	}

	if (key == '3')
	{
		rigTestControl[dofIdx] = -maxControlSpeed;
	}

	if (key == '4')
	{
		rigTestControl[dofIdx] = 0;
	}

	if (key == '5')
	{
		rigTestControl[dofIdx] = maxControlSpeed;
	}

	if (key == '6') {
		saveOdeStateToFile(fallen_filename.data(), masterContext, 0, nullptr);
	}

	if (key == '7') {
		loadOdeStateFromFile(fallen_filename.data(), masterContext, 0, nullptr);
	}

	if (key == 'r')
	{
		saveOdeState(masterContext, resetSaveSlot);
		if (useAcceleration)
			targetSpeed *= 0;
	}

	if (key == 't')
	{
		realtimeMode = !realtimeMode;
	}

	if (key == ' ')
	{
		throwSphere();
	}

	if (key == 'i')
	{
		applyImpulse = true;
	}

	if (key == 'w')
	{
		switchTargets = true;
	}

	if (key == 'e') {
		autoExitAt = frameIdx;
	}

	if (key == 'n') {
		run_on_neural_network = true;
	}

	if (key == 'm') {
		run_on_neural_network = false;
	}

	if (key == 'o') {
		write_vectors_to_file("controls.txt", control_sequence);
		write_vectors_to_file("ml_controls.txt", machine_learning_control_sequence);
		std::system("python density_plotter.py");
	}

	if (key == 'l') {
		write_vectors_to_file("controls.txt", control_sequence);
		std::system("python sequence_plotter.py");
	}
}

void EXPORT_API rcOnKeyUp(int key)
{
}

float computeStateCost(const TestRig &character, bool debugOutput = false)
{
	float result = 0;
	Vector3f com;
	character.computeCOM(com);

	Vector3f target_dir = get_target_dir(com);
	const Vector3f initialDir(-1, 0, 0);
	Quaternionf targetRotation = Quaternionf::FromTwoVectors(initialDir, target_dir);

	//bone angle diff from initial
	for (size_t i = 0; i < character.bones.size(); i++)
	{
		OdeRig::Bone *b = character.bones[i];
		Quaternionf q = ode2eigenq(odeBodyGetQuaternion(b->body));
		float weight = 1.0f;
		float sdMult = 1.0f;
		result += weight*squared(q.angularDistance(targetRotation*b->initialRotation) / (angleSd));
	}

	//com over feet
	if (scoreAngles)
	{
		Vector3f meanFeet = Vector3f::Zero();
		int num_feet = 0;

		float foot_max_height = -1000.0f;

		for (int i : feet_bones)
		{

			const Eigen::Vector3f footPos(odeBodyGetPosition(character.bones[i]->body));

			foot_max_height = std::max(foot_max_height, footPos.z());

			meanFeet += footPos;
			++num_feet;
		}

		meanFeet /= (float)num_feet;
		meanFeet.z() = foot_max_height;

		Vector3f comDiff = com - meanFeet;
		comDiff.z() = std::min(comDiff.z(), 0.0f); //only penalize vertical difference if a foot higher than com (prevents the cheat where character lies down and holds legs up over com)
		result += comDiff.squaredNorm() / squared(comDiffSd);
	}

	//COM vel difference
	Vector3f vel;
	character.computeMeanVel(vel);
	Vector3f targetVel = targetSpeed*target_dir;
	Vector3f velDiff = vel - targetVel;
	velDiff.z() = std::min(velDiff.z(), 0.0f);  //don't penalize upwards deviation
	result += velDiff.squaredNorm() / squared(velSd);

	return result;
}

bool fallen()
{
	for (int bone : non_groung_contact_bones) {

		int bone_geom_id = character.bones[bone]->geoms[0];

		Vector3f pos, normal, vel;
		pos.setZero();
		normal.setZero();
		vel.setZero();
		bool contact = odeGetGeomContact(bone_geom_id, groundPlane, pos.data(), normal.data(), vel.data());

		if (contact) {
			return true;
		}

	}

	return false;
}

bool recovered()
{
	return !fallen();
}

void applyControl(const float *control)
{

	float currentAngles[256];
	float control2[256];
	memcpy(control2, control, sizeof(float)*character.controlDim);
	character.getCurrentMotorAngles(currentAngles);
	for (int i = 0; i < character.nTotalDofs; i++)
	{
		control2[i] = (control[i] - currentAngles[i]) *  poseSpringConstant;
	}
	character.applyControl(control2);

	character.setFmaxForAllMotors(defaultFmax);
	character.setMotorSpringConstants(springKp, springDamping);

	if (enableRecoveryMode)
	{
		bool allow_changing_spring_constants = true;
		bool allow_changing_fmax = true;

		if (flc->sampling_mode_ == SCAControl::SamplingMode::MINIMIZING_INFORMATION_LOSS) {
			allow_changing_spring_constants = false;
			allow_changing_fmax = false;
		}

		if (inRecoveryMode) {
			if (allow_changing_fmax) {
				character.setFmaxForAllMotors(recoveryModeFmax);
			}
			if (allow_changing_spring_constants) {
				character.setMotorSpringConstants(recoveryModeSpringKp, springDamping);
			}
		}
	}

}

static float previousCost = 10000.0f;

void EXPORT_API rcOnMouse(float, float, float, float, float, float, int, int, int) {

}

int total_simulations = 0;
int cumulative_controller_update_time = 0;

void EXPORT_API rcUpdate()
{

	if (flc->sampling_mode_ == SCAControl::SamplingMode::MINIMIZING_INFORMATION_LOSS) {
		use_external_prior = false;
	}

	if (test == TestType::GET_UP) {
		if (frameIdx == start_get_up_test_at) {
			loadOdeStateFromFile(fallen_filename.data(), masterContext, 0, nullptr);
		}
	}

	//accelerated target velocity
	if (useAcceleration)
	{
		if (frameIdx == 0)
			targetSpeed *= 0;
		targetSpeed += acceleration*timeStep;
	}

	//walk targets
	if (useWalkTargets)
	{

		//pick next target location and velocity
		Vector3f com;
		character.computeCOM(com);
		com.z() = 0;

		auto random_walk_target = []() {
			Eigen::Vector3f target;

			target.setRandom();

			float dist_from_origin = 5.0f;

			target.z() = 0;
			target.normalize();
			target *= dist_from_origin;

			return target;

		};

		if (frameIdx % walk_time == 0 || frameIdx == 0 || switchTargets) {
			walkTarget = random_walk_target();
			switchTargets = false;
		}

		float distToTarget = (walkTarget - com).norm();
		while (distToTarget < 1.5f)
		{
			walkTarget = random_walk_target();
			distToTarget = (walkTarget - com).norm();
		}

	}

	//visualize target as a "pole"
	dMatrix3 R;
	dRSetIdentity(R);
	rcSetColor(0.5f, 1, 0.5f);
	rcDrawCapsule(walkTarget.data(), R, 20.0f, 0.02f);
	rcSetColor(1, 1, 1);

	//multitasking with random velocity
	static float timeOnTask = 0;
	timeOnTask += timeStep;
	if (multiTask && (timeOnTask > 5.0f))
	{
		float r = (float)randInt(-1, 1);
		targetSpeed = r;
		printf("New target vel %f\n", targetSpeed);
		timeOnTask = 0;
	}

	//setup current character state
	setCurrentOdeContext(masterContext);
	restoreOdeState(masterContext);
	VectorXf startState(stateDim);
	computeStateVector(&startState[0]);

	int currentTrajectories = nTrajectories;
	int learning = true;

	if (realtimeMode) {
		currentTrajectories = nRealtimeTrajectories;
		learning = false;

		flc->machine_learning_samples_ = 2;
		flc->noisy_machine_learning_samples_ = 2;
		flc->nn_trajectories_ = 2;

	}
	else {

		flc->machine_learning_samples_ = orig_ml_trajectories;
		flc->noisy_machine_learning_samples_ = orig_noisy_ml_trajectories;
		flc->nn_trajectories_ = orig_nearest_neighbor_trajectories;

	}

	flc->setParams(previousCost*resampleThreshold, learning, currentTrajectories);

	flc->control_diff_std_ = controlSd;
	flc->static_prior_.mean[0] = init_motor_angles;

	end = std::chrono::system_clock::now();

	std::chrono::duration<double> duration = end - start;
	int seconds = (int)duration.count() % 60;
	int minutes = (int)duration.count() / 60;

	VectorXf scaledControlSd = controlSd;
	controlDiffSd = controlSd;

	if (inRecoveryMode)
	{
		scaledControlSd *= recoveryModePoseSdMult;
		controlDiffSd *= recoveryModePoseSdMult;
	}
	else {
		flc->control_min_ = minControls;
		flc->control_max_ = maxControls;
	}

	flc->setSamplingParams(scaledControlSd.data(), controlDiffSd.data(), mutationSd);

	startPerfCount();
	int masterContext = nTrajectories;

	bool willFall = false;
	if (!rigTestMode)
	{

		if (!run_on_neural_network) {

			float total_computation_seconds = (float)cumulative_controller_update_time / 1000.0f;

			rcPrintString("Number of trajectories: %d %s", currentTrajectories, realtimeMode ? "(Realtime-mode)" : "");
			rcPrintString("Planning horizon: %.1f seconds", planningHorizonSeconds);
			rcPrintString("Simulation time %02d:%02d:%2d (%d frames), total computing time %.2f s", frameIdx / (fps * 60), (frameIdx / fps) % 60, frameIdx % fps, frameIdx, total_computation_seconds);
			rcPrintString("Pruning threshold %.1f", previousCost*resampleThreshold);

			flc->startIteration(true, &startState[0]);

			for (int step = 0; step < nTimeSteps / nPhysicsPerStep; step++)
			{
				int nUsedTrajectories = flc->getNumTrajectories();
				flc->startPlanningStep(step);
				if (step == 0)
				{
					for (int i = 0; i < nUsedTrajectories; i++)
					{
						saveOdeState(contexts[i].trajectoryIdx, masterContext);
					}
				}
				else
				{
					for (int i = 0; i < nUsedTrajectories; i++)
					{
						saveOdeState(contexts[i].trajectoryIdx, contexts[i].trajectoryIdx);
					}

				}

				std::deque<std::future<void>> workers;

				for (int t = nUsedTrajectories - 1; t >= 0; t--)
				{
					//lambda to be executed in the thread of the simulation context
					auto controlStep = [step](int data) {
						if (frameIdx == 0 && step == 0 && useThreads)
						{
							allocateODEDataForThread();
							odeRandSetSeed(randomSeed);
						}
						SimulationContext &c = contexts[data];
						setCurrentOdeContext(c.trajectoryIdx);
						restoreOdeState(flc->getPreviousSampleIdx(c.trajectoryIdx));

						if (use_external_prior) {
							//pose prior (towards zero angles)
							character.getCurrentMotorAngles(c.priorMean.data());

							//In pose-based control we use the extra prior to limit acceleration.
							//We first compute the predicted pose based on current pose and motor angular velocities, and then
							//set the prior there.
							c.angleRates.setZero();
							character.getCurrentAngleRates(c.angleRates.data());
							c.priorMean = c.priorMean + c.angleRates / poseSpringConstant;
							c.priorMean = c.priorMean.cwiseMax(controlMin);
							c.priorMean = c.priorMean.cwiseMin(controlMax);
							c.priorSd.setConstant(controlAccSd * (inRecoveryMode ? recoveryModeAccSdMult : 1.0f));

						}

						//sample control
						if (use_external_prior) {
							flc->getControl(c.trajectoryIdx, &c.control[0], c.priorMean.data(), c.priorSd.data());
						}
						else {
							flc->getControl(c.trajectoryIdx, &c.control[0], nullptr, nullptr);
						}

						if (character_root_bone >= 0) {
							c.initialPosition = Eigen::Vector3f(odeBodyGetPosition(character.bones[character_root_bone]->body));
						}
						else {
							character.computeCOM(c.initialPosition);
						}

						bool broken = false;

						//step physics
						float controlCost = 0;
						for (int k = 0; k < nPhysicsPerStep; k++)
						{
							applyControl(&c.control[0]);
							if (useFastStep)
								broken = !stepOdeFast(timeStep, false);
							else
								broken = !stepOde(timeStep, false);
							if (broken)
							{
								restoreOdeState(flc->getPreviousSampleIdx(c.trajectoryIdx));
								break;
							}

							controlCost += character.getAppliedSqJointTorques();
							controlCost /= squared(defaultFmax);

						}

						if (character_root_bone >= 0) {
							c.resultPosition = Eigen::Vector3f(odeBodyGetPosition(character.bones[character_root_bone]->body));
						}
						else {
							character.computeCOM(c.resultPosition);
						}

						if (!broken)
						{
							float brokenDistanceThreshold = 0.25f;
							if ((c.resultPosition - c.initialPosition).norm() > brokenDistanceThreshold)
							{
								restoreOdeState(flc->getPreviousSampleIdx(c.trajectoryIdx));
								c.resultPosition = c.initialPosition;
								broken = true;
							}
						}

						//evaluate state cost
						float stateCost = computeStateCost(character);
						if (broken)
							stateCost += 1000000.0f;
						computeStateVector(&c.stateFeatures[0]);

						flc->updateResults(c.trajectoryIdx, c.control.data(), c.stateFeatures.data(), stateCost + controlCost);
						c.stateCost = stateCost;
						c.controlCost = controlCost;
					};
					if (!useThreads)
						controlStep(t);
					else
						workers.push_back(std::async(std::launch::async, controlStep, t));
				}
				if (useThreads)
				{
					for (std::future<void>& worker : workers) {
						worker.wait();
					}
				}

				flc->endPlanningStep(step);

				bool plot_trajectories = true;

				if (plot_trajectories) {
					//debug visualization
					for (int t = nUsedTrajectories - 1; t >= 0; t--)
					{
						SimulationContext &c = contexts[t];
						if (flc->experience_[step + 1][t].particleRole == ParticleRole::OLD_BEST) {
							rcSetColor(0, 1, 0, 1);
						}
						else if (flc->experience_[step + 1][t].particleRole == ParticleRole::NEAREST_NEIGHBOR)
						{
							rcSetColor(0, 0, 1, 1);
						}
						else
						{
							rcSetColor(1, 1, 1, 1.0f);
						}
						rcDrawLine(c.initialPosition.x(), c.initialPosition.y(), c.initialPosition.z(), c.resultPosition.x(), c.resultPosition.y(), c.resultPosition.z());
					}
				}
				rcSetColor(1, 1, 1, 1);
			}

			flc->endIteration();

			//print profiling info
			int controllerUpdateMs = getDurationMs();
			rcPrintString("Controller update time: %d ms", controllerUpdateMs);
			cumulative_controller_update_time += controllerUpdateMs;

			//check whether best trajectory will fall
			setCurrentOdeContext(contexts[flc->getBestSampleLastIdx()].trajectoryIdx);

			willFall = fallen();

		}

		if (useAcceleration)
			rcPrintString("Target speed: %.2f m/s", fabs(targetSpeed));

		//step master context
		setCurrentOdeContext(masterContext);
		restoreOdeState(masterContext);

		Eigen::VectorXf machine_learning_control = contexts[masterContext].control;

		computeStateVector(contexts[masterContext].stateFeatures.data());
		flc->getMachineLearningControl(contexts[masterContext].stateFeatures.data(), machine_learning_control.data());

		float stochasticity_check = 0.0f;
		if (flc->input_noise_dim_ > 0) {
			Eigen::VectorXf tmp_control = machine_learning_control;
			flc->getMachineLearningControl(contexts[masterContext].stateFeatures.data(), machine_learning_control.data());
			stochasticity_check = (tmp_control - machine_learning_control).norm();
		}

		if (run_on_neural_network) {
			contexts[masterContext].control = machine_learning_control;
		}
		else {
			flc->getBestControl(0, contexts[masterContext].control.data());
		}

		machine_learning_control_sequence.push_back(machine_learning_control);
		control_sequence.push_back(contexts[masterContext].control);

		float best_trajectory_cost = (float)flc->getBestTrajectoryCost();
		for (int k = 0; k < nPhysicsPerStep; k++)
		{
			//apply control (and random impulses)
			applyControl(contexts[masterContext].control.data());
			if (applyImpulse || (useRandomImpulses && !realtimeMode && ((frameIdx % randomImpulseInterval) == 0)))
			{
				applyImpulse = false;
				float impulse[3] = { randomf()*randomImpulseMagnitude,randomf()*randomImpulseMagnitude,randomf()*randomImpulseMagnitude };
				odeBodyAddForce(character.bones[0]->body, impulse);
			}
			if (useFastStep)
				stepOdeFast(timeStep, false);
			else
				stepOde(timeStep, false);
		}

		saveOdeState(masterContext, masterContext);

		float controlCost = character.getAppliedSqJointTorques();
		controlCost /= squared(defaultFmax);

		previousCost = contexts[flc->getBestSampleLastIdx()].stateCost;
		float state_cost = computeStateCost(character);
		rcPrintString("state cost %.2f, current control cost %.2f, end state cost %.2f", state_cost, controlCost, previousCost);
		//rcPrintString("Stochasticity check %.9f\n", stochasticity_check);

		total_simulations += flc->amount_samples_*flc->steps_;

		rcPrintString("Total training simulation steps: %d", total_simulations);

		std::vector<float> cost;
		cost.push_back(best_trajectory_cost);
		cost.push_back(state_cost + controlCost);

		flc->getMachineLearningChosenControlDiscrepancy();
		cost.push_back(flc->machine_learning_and_chosen_control_discrepancy_);

		float ml_mse = flc->getMachineLearningMSE();
		cost.push_back(ml_mse);

		bool on_the_ground = fallen();
		cost.push_back((float)on_the_ground);

		costs.push_back(cost);

	} //!rigTestMode
	setCurrentOdeContext(masterContext);

	//in simple forward walking without targets, if moved too far from origin, move back
	if (!useWalkTargets)
	{
		Vector3f com;
		character.computeCOM(com);
		if (captureVideo && com.norm() > maxDistanceFromOrigin)
		{
			Vector3f disp = -com;
			disp.z() = 0;
			for (size_t i = 0; i < character.bones.size(); i++)
			{
				Vector3f pos(odeBodyGetPosition(character.bones[i]->body));
				pos += disp;
				odeBodySetPosition(character.bones[i]->body, pos.x(), pos.y(), pos.z());
			}
			saveOdeState(masterContext, masterContext);
		}
	}

	bool reset_on_fall = true;

	if (test == TestType::GET_UP && frameIdx >= start_get_up_test_at) {
		reset_on_fall = false;
	}

	if (reset_on_fall) {
		bool on_the_ground = fallen();

		//if (captureVideo) {
		//	on_the_ground = false;
		//}

		if (on_the_ground) {

			saveOdeState(masterContext, resetSaveSlot);
			run_on_neural_network = false;

			for (int i = 0; i <= nTrajectories; i++)
			{
				setCurrentOdeContext(contexts[i].trajectoryIdx);
				restoreOdeState(resetSaveSlot);
				saveOdeState(contexts[i].trajectoryIdx, contexts[i].trajectoryIdx);
			}

			if (useAcceleration)
				targetSpeed *= 0;
		}
	}

	if (rigTestMode)
	{
		applyControl(rigTestControl);
		if (useFastStep)
			stepOdeFast(timeStep);
		else
			stepOde(timeStep, false);
		saveOdeState(masterContext, masterContext);
	}
	static bool setVP = true;
	if (setVP)
	{
		setVP = false;
		rcSetViewPoint(-(2.0f + maxDistanceFromOrigin), -3, 1.5f, -(maxDistanceFromOrigin - 1), 0, 0.9f);
		rcSetLightPosition(-0, -10, 10);
	}
	rcDrawAllObjects((dxSpace *)odeGetSpace());

	character.debugVisualize();

	//state transitions
	if (willFall)
	{
		rcPrintString("Falling predicted!");
		if (enableRecoveryMode)
			inRecoveryMode = true;
	}
	if (!willFall && recovered())
		inRecoveryMode = false;
	if (inRecoveryMode) {
		if (flc->sampling_mode_ == SCAControl::SamplingMode::PRODUCT_OF_GAUSSIANS) {
			rcPrintString("Recovery mode with stronger movements and wider search distributions.");
		}

		if (flc->sampling_mode_ == SCAControl::SamplingMode::MINIMIZING_INFORMATION_LOSS) {
			rcPrintString("Recovery mode with wider search distributions.");
		}
	}

	//turn on real-time mode automatically when learned enough
	if (frameIdx > startRealtimeModeAt)
		realtimeMode = true;

	if (no_settings_exit) {
		exit(0);
	}

	//quit
	if (frameIdx > autoExitAt)
	{
		std::deque<std::string> settings = flc->get_settings();

		for (std::string setting : settings) {
			comments.push_back(setting);
		}

		comments.push_back("Turns: " + std::to_string(useWalkTargets));

		std::string file_time_string = get_time_string();

		std::string cost_file_name = file_time_string + "_costs.csv";
		write_vector_to_file(cost_file_name, costs, comments);

		std::string control_file_name = file_time_string + "_controls.csv";
		write_deque_of_eigen_to_file(control_file_name, control_sequence, comments);

		control_file_name = file_time_string + "_controls_machine_learning.csv";
		write_deque_of_eigen_to_file(control_file_name, machine_learning_control_sequence, comments);

		if (fileExists("out.mp4"))
			remove("out.mp4");
		system("screencaps2mp4.bat");

		exit(0);
	}

	if (captureVideo)
	{
		//During learning, capture video at the beginning and every now and then.
		if (realtimeMode || (frameIdx < nInitialCapturedFrames || (frameIdx % (60 * fps) < nFramesToCaptureEveryMinute)))
		{
			rcTakeScreenShot();
		}
	}

	//spheres
	if (useSpheres && (frameIdx > lastSphereLaunchTime + sphereInterval))
	{
		Vector3f com;
		character.computeCOM(com);
		if (useWalkTargets || (com.x() > -maxDistanceFromOrigin + 2.0f) && (com.x() < -0.5f))  //don't throw a sphere right before character is about to be teleported
			throwSphere();
	}

	for (size_t i = 0; i < spheres.size(); i++)
	{
		SphereData &sd = spheres[i];
		if (sd.spawnFrame < frameIdx - 5 * fps)
		{
			setCurrentOdeContext(ALLTHREADS);
			odeGeomDestroy(sd.geom);
			odeBodyDestroy(sd.body);
			saveOdeState(masterContext, masterContext);
			spheres[i] = spheres[spheres.size() - 1];
			spheres.resize(spheres.size() - 1);
		}
	}

	frameIdx++;

}


#include <InverseFourierTransform.h>
#include <OpenCLContext.h>
#include <boost/log/trivial.hpp>

using namespace neneta;
using namespace neneta::net;

InverseFourierTransform::InverseFourierTransform(const std::string& layerId, const conf::ConfigurationReader& confReader, const gpu::OpenCLProgram& program, const gpu::OpenCLContext& oclContext)
    : gpu::OpenCLExecutionPlan(layerId, confReader, program)
    , m_layerParameters(conf::FourierConfiguration(layerId, confReader).getParamSet(layerId))
    , m_oclKernelParameters(m_layerParameters.m_size, m_layerParameters.m_size, getKernelConfiguration().getLWS(1), getKernelConfiguration().getLWS(2))
    , m_clContext(oclContext)
    , m_io()
    , m_reTmp(m_clContext.getContext(), CL_MEM_READ_WRITE, m_layerParameters.m_size*m_layerParameters.m_size*sizeof(cmn::GPUFLOAT))
    , m_imTmp(m_clContext.getContext(), CL_MEM_READ_WRITE, m_layerParameters.m_size*m_layerParameters.m_size*sizeof(cmn::GPUFLOAT))
{
}

InverseFourierTransform::~InverseFourierTransform()
{
}

void InverseFourierTransform::init()
{
    for(unsigned int channel = 0; channel < m_layerParameters.m_channels; ++channel)
    {
        // plan("fftShift", {m_layerParameters.m_size, m_layerParameters.m_size, m_layerParameters.m_size/2, m_layerParameters.m_size/2}, *reKernelIt, *imKernelIt, (cl_int)m_layerParameters.m_size);
        planFwd("rowIndexReverse", m_oclKernelParameters, m_io.m_reChannels[channel], m_io.m_imChannels[channel], (cl_int)m_layerParameters.m_size, (cl_int)std::log2(m_layerParameters.m_size));
        planFwd("columnIndexReverse", m_oclKernelParameters, m_io.m_reChannels[channel], m_io.m_imChannels[channel], (cl_int)m_layerParameters.m_size, (cl_int)std::log2(m_layerParameters.m_size));
        for(int stage = 1; stage <= std::log2(m_layerParameters.m_size); ++stage)
        {
           planFwd("dit_2x2radix_2dfft_1st", m_oclKernelParameters, m_io.m_reChannels[channel], m_io.m_imChannels[channel], m_reTmp, m_imTmp, (cl_int)m_layerParameters.m_size, (cl_int)stage, (cl_int)1);
           planFwd("dit_2x2radix_2dfft_2nd", m_oclKernelParameters, m_io.m_reChannels[channel], m_io.m_imChannels[channel], m_reTmp, m_imTmp, (cl_int)m_layerParameters.m_size, (cl_int)stage);
        }
        planFwd("fftScale", m_oclKernelParameters, m_io.m_reChannels[channel], m_io.m_imChannels[channel], (cl_int)m_layerParameters.m_size);
    }
}

void InverseFourierTransform::getRe(std::vector<std::vector<cmn::GPUFLOAT>>& re)
{
    cmn::GPUFLOAT* data = new cmn::GPUFLOAT[m_layerParameters.m_size*m_layerParameters.m_size];
    for(unsigned int channel = 0; channel < m_layerParameters.m_channels; ++channel)
    {
        re.emplace_back();
        readFromBuffer(m_clContext.getCommandQueue(), m_io.m_reChannels[channel], m_layerParameters.m_size*m_layerParameters.m_size*sizeof(cmn::GPUFLOAT), data);
        re.back().assign(data, data + m_layerParameters.m_size*m_layerParameters.m_size);
    }
    delete [] data;
}

void InverseFourierTransform::getIm(std::vector<std::vector<cmn::GPUFLOAT>>& im)
{
    cmn::GPUFLOAT* data = new cmn::GPUFLOAT[m_layerParameters.m_size*m_layerParameters.m_size];
    for(unsigned int channel = 0; channel < m_layerParameters.m_channels; ++channel)
    {
        im.emplace_back();
        readFromBuffer(m_clContext.getCommandQueue(), m_io.m_imChannels[channel], m_layerParameters.m_size*m_layerParameters.m_size*sizeof(cmn::GPUFLOAT), data);
        im.back().assign(data, data + m_layerParameters.m_size*m_layerParameters.m_size);
    }
    delete [] data;
}

void neneta::net::InverseFourierTransform::setInput(gpu::BufferIO input)
{
    m_io = input;
    init();
}

gpu::BufferIO neneta::net::InverseFourierTransform::getOutput()
{
    return m_io;
}

void InverseFourierTransform::setBkpInput(gpu::BufferIO input)
{

}

gpu::BufferIO InverseFourierTransform::getBkpOutput()
{
    return m_io;
}


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// 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_OVERLAY_GET_INTERSECTION_POINTS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_INTERSECTION_POINTS_HPP

#include <cstddef>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/algorithms/detail/overlay/get_turns.hpp>

#include <boost/geometry/geometries/segment.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace get_intersection_points
{

template
<
    typename Point1,
    typename Point2,
    typename TurnInfo
>
struct get_turn_without_info
{
    typedef strategy_intersection
        <
            typename cs_tag<typename TurnInfo::point_type>::type,
            Point1,
            Point2,
            typename TurnInfo::point_type
        > si;

    typedef typename si::segment_intersection_strategy_type strategy;

    template <typename OutputIterator>
    static inline OutputIterator apply(
                Point1 const& pi, Point1 const& pj, Point1 const& pk,
                Point2 const& qi, Point2 const& qj, Point2 const& qk,
                TurnInfo const& tp_model,
                OutputIterator out)
    {
        typedef model::referring_segment<Point1 const> segment_type1;
        typedef model::referring_segment<Point1 const> segment_type2;
        segment_type1 p1(pi, pj), p2(pj, pk);
        segment_type2 q1(qi, qj), q2(qj, qk);

        //
        typename strategy::return_type result = strategy::apply(p1, q1);

        for (std::size_t i = 0; i < result.template get<0>().count; i++)
        {

            TurnInfo tp;
            geometry::convert(result.template get<0>().intersections[i], tp.point);
            *out++ = tp;
        }

        return out;
    }
};

}} // namespace detail::get_intersection_points
#endif // DOXYGEN_NO_DETAIL

template
<
    typename Geometry1,
    typename Geometry2,
    typename Turns
>
inline void get_intersection_points(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            Turns& turns)
{
    concept::check_concepts_and_equal_dimensions<Geometry1 const, Geometry2 const>();

    typedef detail::get_intersection_points::get_turn_without_info
                        <
                            typename point_type<Geometry1>::type,
                            typename point_type<Geometry2>::type,
                            typename boost::range_value<Turns>::type
                        > TurnPolicy;

    typedef typename strategy_intersection
        <
            typename cs_tag<Geometry1>::type,
            Geometry1,
            Geometry2,
            typename boost::range_value<Turns>::type
        >::segment_intersection_strategy_type segment_intersection_strategy_type;

    detail::get_turns::no_interrupt_policy interrupt_policy;

    boost::mpl::if_c
        <
            reverse_dispatch<Geometry1, Geometry2>::type::value,
            dispatch::get_turns_reversed
            <
                typename tag<Geometry1>::type,
                typename tag<Geometry2>::type,
                Geometry1, Geometry2,
                false, false,
                Turns, TurnPolicy,
                //segment_intersection_strategy_type,
                detail::get_turns::no_interrupt_policy
            >,
            dispatch::get_turns
            <
                typename tag<Geometry1>::type,
                typename tag<Geometry2>::type,
                Geometry1, Geometry2,
                false, false,
                Turns, TurnPolicy,
                //segment_intersection_strategy_type,
                detail::get_turns::no_interrupt_policy
            >
        >::type::apply(
            0, geometry1,
            1, geometry2,
            turns, interrupt_policy);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_OVERLAY_GET_INTERSECTION_POINTS_HPP


// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_TEST_PRODUCTS_ELLIPTIC_HH
#define DUNE_GDT_TEST_PRODUCTS_ELLIPTIC_HH

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

#include <dune/stuff/la/container/common.hh>
#include <dune/stuff/functions/constant.hh>
#include <dune/stuff/functions/expression.hh>
#include <dune/stuff/test/common.hh>

#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/operators/projections.hh>
#include <dune/gdt/products/elliptic.hh>

#include "products.hh"

using namespace Dune;
using namespace Dune::GDT;

template< class SpaceType >
struct EllipticProductBase
  : public ::testing::Test
{
  typedef typename SpaceType::GridViewType GridViewType;
  typedef typename GridViewType::Grid      GridType;
  typedef Dune::Stuff::Grid::Providers::Cube< GridType > GridProviderType;
  typedef typename GridViewType::template Codim< 0 >::Entity EntityType;
  typedef typename SpaceType::DomainFieldType DomainFieldType;
  static const size_t                         dimDomain = SpaceType::dimDomain;
  typedef typename SpaceType::RangeFieldType  RangeFieldType;
  static const size_t                         dimRange = SpaceType::dimRange;
  typedef Stuff::Functions::Expression
      < EntityType, DomainFieldType, dimDomain, RangeFieldType, dimRange >             FunctionType;
  typedef Stuff::Functions::Constant
      < EntityType, DomainFieldType, dimDomain, RangeFieldType, dimDomain, dimDomain > TensorType;

  EllipticProductBase()
   : grid_(GridProviderType(0.0, 1.0, Stuff::Test::grid_elements()).grid_ptr())
   , leaf_view_(Dune::GDT::SpaceTools::GridPartView< SpaceType >::create_leaf(*grid_))
   , space_(leaf_view_)
   , one_("x", "1.0", 1, "constant gradient", {{"1.0", "1.0", "1.0"}})
   , unit_matrix_(Stuff::Functions::internal::unit_matrix< RangeFieldType, dimDomain >())
  {}

  virtual RangeFieldType compute(const FunctionType& function) const = 0;

  void constant_arguments() const
  {
    const FunctionType constant_gradient("x", "fake_value", 1, "constant gradient", {{"1.0", "1.0", "1.0"}});
    check(compute(constant_gradient), dimDomain*1.0);
  } // ... constant_arguments(...)

  void linear_arguments() const
  {
    const FunctionType linear_gradient("x", "fake_value", 2, "affine gradient",
                                       {{"x[0] - 1.0", "x[0] - 1.0", "x[0] - 1.0"}});
    check(compute(linear_gradient), dimDomain*(1.0/3.0));
  }

  void quadratic_arguments() const
  {
    const FunctionType quadratic_gradient("x", "fake_value", 3, ", quadratic gradient",
                                          {{"x[0]*x[0]", "x[0]*x[0]", "x[0]*x[0]"}});
    check(compute(quadratic_gradient), dimDomain*(1.0/5.0));
  }

  void check(const RangeFieldType& result, const RangeFieldType& expected, const RangeFieldType epsilon = 1e-14) const
  {
    const auto error = std::abs(expected - result);
    EXPECT_LE(error, epsilon)
        << "result:   " << result << "\n"
        << "expected: " << expected << "\n"
        << "difference: " << std::scientific << error;
  } // ... check(...)

  std::shared_ptr< GridType > grid_;
  typename Dune::GDT::SpaceTools::GridPartView< SpaceType >::LeafGridViewType leaf_view_;
  const SpaceType space_;
  const FunctionType one_;
  const TensorType unit_matrix_;
}; // struct EllipticProductBase

template< class SpaceType >
struct EllipticLocalizableProduct
  : public EllipticProductBase< SpaceType >
{
  typedef EllipticProductBase< SpaceType > BaseType;
  typedef typename BaseType::GridViewType   GridViewType;
  typedef typename BaseType::FunctionType   FunctionType;
  typedef typename BaseType::TensorType     TensorType;
  typedef typename BaseType::RangeFieldType RangeFieldType;

  virtual RangeFieldType compute(const FunctionType& function) const override final
  {
    return Products::EllipticLocalizable
        < GridViewType, FunctionType, FunctionType, FunctionType, RangeFieldType, TensorType >
        (this->space_.grid_view(), function, function, this->one_, this->unit_matrix_).apply2();
  } // ... compute(...)

  void fulfills_interface() const
  {
    typedef Products::EllipticLocalizable
        < GridViewType, FunctionType, FunctionType, FunctionType, RangeFieldType, TensorType > ProductType;
    ProductType product(this->space_.grid_view(), this->one_, this->one_, this->one_, this->unit_matrix_);
    LocalizableProductBase< SpaceType, ProductType >::fulfills_interface(product);
  }
}; // struct EllipticLocalizableProduct

template< class SpaceType >
struct EllipticAssemblableProduct
  : public EllipticProductBase< SpaceType >
{
  typedef EllipticProductBase< SpaceType > BaseType;
  typedef typename BaseType::RangeFieldType  RangeFieldType;
  typedef typename BaseType::FunctionType    FunctionType;
  typedef typename BaseType::TensorType      TensorType;
  typedef typename BaseType::GridViewType    GridViewType;
  typedef typename Dune::Stuff::LA::CommonDenseVector< RangeFieldType > VectorType;
  typedef typename Dune::Stuff::LA::CommonDenseMatrix< RangeFieldType > MatrixType;
  typedef Dune::GDT::DiscreteFunction< SpaceType, VectorType > DiscreteFunctionType;
  typedef Dune::GDT::Operators::Projection< GridViewType > ProjectionOperatorType;

  virtual RangeFieldType compute(const FunctionType& function) const
  {
    // create the product
    typedef Products::EllipticAssemblable< MatrixType, FunctionType, SpaceType, GridViewType,
                                           SpaceType, RangeFieldType, TensorType > Product;
    Product product(this->space_, this->one_, this->unit_matrix_);
    product.assemble(false);
    // project the function
    DiscreteFunctionType discrete_function(this->space_);
    ProjectionOperatorType(this->space_.grid_view()).apply(function, discrete_function);
    // compute the product
    const auto result = product.apply2(discrete_function, discrete_function);
    Product product_tbb(this->space_, this->one_, this->unit_matrix_);
    product_tbb.assemble(true);
    const auto result_tbb = product_tbb.apply2(discrete_function, discrete_function);
    EXPECT_DOUBLE_EQ(result_tbb, result);
    return result;
  } // ... compute(...)

  /**
   * \note we can not use the base variant bc. of the projection in compute()
   */
  void constant_arguments() const
  {
    const FunctionType constant_gradient("x", "x[0]", 1, "constant gradient", {{"1.0", "0.0", "0.0"}});
    this->check(compute(constant_gradient), 1.0, 1e-13);
  } // ... constant_arguments(...)

  /**
   * \note we can not use the base variant bc. of the projection in compute()
   */
  void linear_arguments() const
  {
    const FunctionType linear_gradient("x", "0.5 * x[0] * x[0] - x[0]", 2, "affine gradient",
                                       {{"x[0] - 1.0", "0.0", "0.0"}});
    this->check(compute(linear_gradient), 1.0/3.0, 1e-13);
  }

  /**
   * \note we can not use the base variant bc. of the projection in compute()
   */
  void quadratic_arguments() const
  {
    const FunctionType quadratic_gradient("x", "(1.0/3.0) * x[0] * x[0] * x[0]", 3, ", quadratic gradient",
                                          {{"x[0]*x[0]", "0.0", "0.0"}});
    this->check(compute(quadratic_gradient), 1.0/5.0);
  }

  void fulfills_interface() const
  {
    typedef Products::EllipticAssemblable
        < MatrixType, FunctionType, SpaceType, GridViewType, SpaceType, RangeFieldType, TensorType > ProductType;
    ProductType product(this->space_, this->one_, this->unit_matrix_);
    AssemblableProductBase< SpaceType, ProductType, VectorType >::fulfills_interface(product);
  }
}; // EllipticAssemblableProduct

template< class SpaceType >
struct EllipticProduct
  : public EllipticProductBase< SpaceType >
{
  typedef EllipticProductBase< SpaceType > BaseType;
  typedef typename BaseType::RangeFieldType  RangeFieldType;
  typedef typename BaseType::FunctionType    FunctionType;
  typedef typename BaseType::TensorType      TensorType;
  typedef typename BaseType::GridViewType    GridViewType;

  virtual RangeFieldType compute(const FunctionType& function) const
  {
    Products::Elliptic< GridViewType, FunctionType, RangeFieldType, TensorType >
        product(this->space_.grid_view(), this->one_, this->unit_matrix_);
    return product.apply2(function, function);
  } // ... compute(...)

  void fulfills_interface() const
  {
    typedef Products::Elliptic< GridViewType, FunctionType, RangeFieldType, TensorType > ProductType;
    ProductBase< SpaceType, ProductType >::fulfills_interface(ProductType(this->space_.grid_view(),
                                                                          this->one_,
                                                                          this->unit_matrix_));
  }
}; // struct EllipticProduct

template< class SpaceType >
struct SimplifiedEllipticLocalizableProduct
  : public EllipticProductBase< SpaceType >
{
  typedef EllipticProductBase< SpaceType > BaseType;
  typedef typename BaseType::GridViewType   GridViewType;
  typedef typename BaseType::FunctionType   FunctionType;
  typedef typename BaseType::TensorType     TensorType;
  typedef typename BaseType::RangeFieldType RangeFieldType;

  virtual RangeFieldType compute(const FunctionType& function) const override final
  {
    return Products::EllipticLocalizable< GridViewType, FunctionType, FunctionType, FunctionType >
        (this->space_.grid_view(), function, function, this->one_).apply2();
  } // ... compute(...)

  void fulfills_interface() const
  {
    typedef Products::EllipticLocalizable< GridViewType, FunctionType, FunctionType, FunctionType > ProductType;
    ProductType product(this->space_.grid_view(), this->one_, this->one_, this->one_);
    LocalizableProductBase< SpaceType, ProductType >::fulfills_interface(product);
  }
}; // struct SimplifiedEllipticLocalizableProduct

template< class SpaceType >
struct SimplifiedEllipticAssemblableProduct
  : public EllipticAssemblableProduct< SpaceType >
{
  typedef EllipticAssemblableProduct< SpaceType > BaseType;
  typedef typename BaseType::RangeFieldType  RangeFieldType;
  typedef typename BaseType::FunctionType    FunctionType;
  typedef typename BaseType::GridViewType    GridViewType;
  typedef typename Dune::Stuff::LA::CommonDenseVector< RangeFieldType > VectorType;
  typedef typename Dune::Stuff::LA::CommonDenseMatrix< RangeFieldType > MatrixType;
  typedef Dune::GDT::DiscreteFunction< SpaceType, VectorType > DiscreteFunctionType;
  typedef Dune::GDT::Operators::Projection< GridViewType > ProjectionOperatorType;

  virtual RangeFieldType compute(const FunctionType& function) const
  {
    // create the product
    typedef Products::EllipticAssemblable< MatrixType, FunctionType, SpaceType, GridViewType, SpaceType > Product;
    Product product(this->space_, this->one_);
    product.assemble(false);
    // project the function
    DiscreteFunctionType discrete_function(this->space_);
    ProjectionOperatorType(this->space_.grid_view()).apply(function, discrete_function);
    // compute the product
    const auto result = product.apply2(discrete_function, discrete_function);
    Product product_tbb(this->space_, this->one_);
    product_tbb.assemble(true);
    const auto result_tbb = product_tbb.apply2(discrete_function, discrete_function);
    EXPECT_DOUBLE_EQ(result_tbb, result);
    return result;
  } // ... compute(...)

  void fulfills_interface() const
  {
    typedef Products::EllipticAssemblable< MatrixType, FunctionType, SpaceType, GridViewType, SpaceType > ProductType;
    ProductType product(this->space_, this->one_);
    AssemblableProductBase< SpaceType, ProductType, VectorType >::fulfills_interface(product);
  }
}; // struct SimplifiedEllipticAssemblableProduct

template< class SpaceType >
struct SimplifiedEllipticProduct
  : public EllipticProductBase< SpaceType >
{
  typedef EllipticProductBase< SpaceType >  BaseType;
  typedef typename BaseType::RangeFieldType RangeFieldType;
  typedef typename BaseType::FunctionType   FunctionType;
  typedef typename BaseType::TensorType     TensorType;
  typedef typename BaseType::GridViewType   GridViewType;

  virtual RangeFieldType compute(const FunctionType& function) const
  {
    Products::Elliptic< GridViewType, FunctionType > product(this->space_.grid_view(), this->one_);
    return product.apply2(function, function);
  } // ... compute(...)

  void fulfills_interface() const
  {
    typedef Products::Elliptic< GridViewType, FunctionType > ProductType;
    ProductBase< SpaceType, ProductType >::fulfills_interface(ProductType(this->space_.grid_view(), this->one_));
  }
}; // struct SimplifiedEllipticProduct

#endif // DUNE_GDT_TEST_PRODUCTS_ELLIPTIC_HH


#include <stdio.h>
#include <cfloat>
#include <list>
#include <vector>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <boost/algorithm/clamp.hpp>
#include <random>
#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/util/work_sharder.h"
#include <opencv2/opencv.hpp>
#include <boost/mpl/map.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/at.hpp>
#include "bboxes.h"
#include "wtoolkit.h"

using namespace tensorflow;
using namespace std;
namespace bm=boost::mpl;
cv::RotatedRect getRotatedRect(const cv::Mat& img)
{
    vector<vector<cv::Point>> contours;
    vector<cv::Vec4i> hierarchy;
    vector<cv::Point> points;

    cv::findContours(img, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE, cv::Point(0,0));

    for (auto &cont:contours)
        points.insert(points.end(),cont.begin(),cont.end());

    return  cv::minAreaRect(points);
}
cv::RotatedRect getRotatedRect(const uint8_t* data,int height,int width)
{
    const cv::Mat img(height,width,CV_8UC1,(uint8_t*)data);
    return getRotatedRect(img);
}

typedef Eigen::ThreadPoolDevice CPUDevice;

REGISTER_OP("MedianBlur")
    .Attr("T: {uint8}")
	.Attr("ksize:int")
    .Input("image: T")
	.Output("outimage:T")
    .SetShapeFn(shape_inference::UnchangedShape);

template <typename Device, typename T>
class MedianBlurOp: public OpKernel {
    private:
        using Tensor3D = Eigen::Tensor<T,3,Eigen::RowMajor>;
        using Tensor4D = Eigen::Tensor<T,4,Eigen::RowMajor>;
	public:
		explicit MedianBlurOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("ksize", &ksize_));
		}
        void process_one_image(T* data, int width,int height)
        {
            cv::Mat img(height,width,CV_8UC1,data);
            cv::medianBlur(img,img,ksize_);
        }
		void Compute(OpKernelContext* context) override
		{
			const Tensor &_input_image  = context->input(0);
			TensorShape   output_shape  = _input_image.shape();
			Tensor       *output_tensor = NULL;

			OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &output_tensor));

            if(_input_image.dims() == 3) {
                output_tensor->template tensor<T,3>() = _input_image.template tensor<T,3>();
                OP_REQUIRES(context, _input_image.dim_size(2) == 1, errors::InvalidArgument("Error channel size, should be 1"));
                process_one_image(output_tensor->template flat<T>().data(),_input_image.dim_size(1),_input_image.dim_size(0));
            } else if(_input_image.dims()==4) {
                output_tensor->template tensor<T,4>() = _input_image.template tensor<T,4>();
                OP_REQUIRES(context, _input_image.dim_size(3) == 1, errors::InvalidArgument("Error channel size, should be 1"));
                const auto img_size = _input_image.dim_size(1)*_input_image.dim_size(2);
                auto data = output_tensor->template flat<T>().data();
                for(auto i=0; i<_input_image.dim_size(0); ++i) {
                    auto d = data+i*img_size;
                    process_one_image(d,_input_image.dim_size(2),_input_image.dim_size(1));
                }
            } else {
                OP_REQUIRES(context, _input_image.dims() == 3, errors::InvalidArgument("Error dims size."));
            }
        }

	private:
        int ksize_ = 0;
};
REGISTER_KERNEL_BUILDER(Name("MedianBlur").Device(DEVICE_CPU).TypeConstraint<uint8_t>("T"), MedianBlurOp<CPUDevice, uint8_t>);

REGISTER_OP("BilateralFilter")
    .Attr("T: {float,uint8}")
	.Attr("d:int")
	.Attr("sigmaColor:float")
	.Attr("sigmaSpace:float")
    .Input("image: T")
	.Output("outimage:T")
    .SetShapeFn(shape_inference::UnchangedShape);

template <typename Device, typename T>
class BilateralFilterOp: public OpKernel {
    private:
        using Tensor3D = Eigen::Tensor<T,3,Eigen::RowMajor>;
        using Tensor4D = Eigen::Tensor<T,4,Eigen::RowMajor>;
	public:
		explicit BilateralFilterOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("d", &d_));
			OP_REQUIRES_OK(context, context->GetAttr("sigmaColor", &sigmaColor_));
			OP_REQUIRES_OK(context, context->GetAttr("sigmaSpace", &sigmaSpace_));
		}
        template<typename TT>
        void process_one_image(const TT* data_i, const TT* data_o,int width,int height,int channel)
        {
            assert(false);
        }
        void process_one_image(const uint8_t* data_i, uint8_t* data_o,int width,int height,int channel)
        {
            if(1==channel) {
                __process_one_image<CV_8UC1>(data_i,data_o,width,channel);
            } else {
                __process_one_image<CV_8UC3>(data_i,data_o,width,channel);
            }
        }
        void process_one_image(const float* data_i, float* data_o,int width,int height,int channel)
        {
            cout<<data_i<<","<<data_o<<endl;
            if(1==channel) {
                __process_one_image<CV_32FC1>(data_i,data_o,width,height,channel);
            } else {
                __process_one_image<CV_32FC3>(data_i,data_o,width,height,channel);
            }
        }
        template<int type,typename TT>
        void __process_one_image(const TT* data_i, TT* data_o,int width,int height,int channel)
        {
            cv::Mat img_i(height,width,type,const_cast<TT*>(data_i));
            cv::Mat img_o(height,width,type,data_o);
            cv::bilateralFilter(img_i,img_o,d_,sigmaColor_,sigmaSpace_);
        }
		void Compute(OpKernelContext* context) override
		{
			const Tensor &_input_image  = context->input(0);
			TensorShape   output_shape  = _input_image.shape();
			Tensor       *output_tensor = NULL;

			OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &output_tensor));

            if(_input_image.dims() == 3) {
                output_tensor->template tensor<T,3>() = _input_image.template tensor<T,3>();
                OP_REQUIRES(context, _input_image.dim_size(2) == 1, errors::InvalidArgument("Error channel size, should be 1"));
                auto data_o = output_tensor->template flat<T>().data();
                auto data_i = _input_image.template flat<T>().data();
                process_one_image(data_i,data_o,_input_image.dim_size(1),_input_image.dim_size(0),_input_image.dim_size(2));
            } else if(_input_image.dims()==4) {
                output_tensor->template tensor<T,4>() = _input_image.template tensor<T,4>();
                OP_REQUIRES(context, _input_image.dim_size(3) == 1, errors::InvalidArgument("Error channel size, should be 1"));
                const auto img_size = _input_image.dim_size(1)*_input_image.dim_size(2);
                auto data_o = output_tensor->template flat<T>().data();
                auto data_i = _input_image.template flat<T>().data();
                for(auto i=0; i<_input_image.dim_size(0); ++i) {
                    auto d_i = data_i+i*img_size;
                    auto d_o = data_o+i*img_size;
                    process_one_image(d_i,d_o,_input_image.dim_size(2),_input_image.dim_size(1),_input_image.dim_size(3));
                }
            } else {
                OP_REQUIRES(context, _input_image.dims() == 3, errors::InvalidArgument("Error dims size."));
            }
        }

	private:
        int d_ = 0;
        float sigmaColor_ = 0;
        float sigmaSpace_ = 0;
};
REGISTER_KERNEL_BUILDER(Name("BilateralFilter").Device(DEVICE_CPU).TypeConstraint<uint8_t>("T"), MedianBlurOp<CPUDevice, uint8_t>);
REGISTER_KERNEL_BUILDER(Name("BilateralFilter").Device(DEVICE_CPU).TypeConstraint<float>("T"), BilateralFilterOp<CPUDevice, float>);

/*
 * image:[N,H,W], 二值图像, 整图尺寸的mask
 * 当res_points==True时
 * return:
 * [N,4,2] 分别为[[[p0.x,p0.y],[p1.x,p1.y],..[p3.x,p3.y]],[....]]]
 * 当res_points==False时:
 * [N,3,2] 分别为[[[center.x,center.y],[width,height],[angle,_]],[.....]]]
 * 旋转角度θ是水平轴（x轴）逆时针旋转，与碰到的矩形的第一条边的夹角。并且这个边的边长是width，另一条边边长是height。也就是说，在这里，width与height不是按照长短来定义的。
 * 在opencv中，坐标系原点在左上角，相对于x轴，逆时针旋转角度为负，顺时针旋转角度为正。所以，θ∈（-90度，0]
 */
REGISTER_OP("MinAreaRect")
    .Attr("T: {uint8}")
    .Attr("res_points: bool = True")
    .Input("image: T")
	.Output("box:float32")
    .SetShapeFn([](shape_inference::InferenceContext* c){
            bool res_points = true;
            c->GetAttr("res_points",&res_points);
            auto input_shape0 = c->input(0);
            const auto batch_size = c->Dim(input_shape0,0);
                auto shape = c->MakeShape({batch_size,res_points?4:3,2});
                c->set_output(0, shape);
			return Status::OK();
            });

template <typename Device, typename T>
class MinAreaRectOp: public OpKernel {
    private:
        using Tensor3D = Eigen::Tensor<T,3,Eigen::RowMajor>;
        using Tensor4D = Eigen::Tensor<T,4,Eigen::RowMajor>;
	public:
		explicit MinAreaRectOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("res_points", &res_points_));
		}
		void Compute(OpKernelContext* context) override
		{
			const Tensor &_input_image  = context->input(0);

			OP_REQUIRES(context, _input_image.dims() == 3, errors::InvalidArgument("image must be at 3-dimensional"));

            auto         input_image   = _input_image.template flat<T>().data();
            const auto   batch_size    = _input_image.dim_size(0);
            const auto   img_height    = _input_image.dim_size(1);
            const auto   img_width     = _input_image.dim_size(2);
            const int    dim3d[]       = {batch_size,res_points_?4:3,2};
            TensorShape  output_shape;
            Tensor      *output_tensor = nullptr;

            TensorShapeUtils::MakeShape(dim3d,3,&output_shape);

			OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &output_tensor));
            auto      o_tensor = output_tensor->template tensor<float,3>();

            o_tensor.setZero();

            for(auto i=0; i<batch_size; ++i) {
                try{
                    auto rect = getRotatedRect(input_image+i*img_height*img_width,img_height,img_width);

                    if(res_points_) {
                        cv::Point2f P[4];
                        rect.points(P);
                        for(auto j=0; j<4; ++j) {
                            o_tensor(i,j,0) = P[j].x;
                            o_tensor(i,j,1) = P[j].y;
                        }
                    } else {
                        o_tensor(i,0,0) = rect.center.x;
                        o_tensor(i,0,1) = rect.center.y;
                        o_tensor(i,1,0) = rect.size.width;
                        o_tensor(i,1,1) = rect.size.height;
                        o_tensor(i,2,0) = rect.angle;
                        o_tensor(i,2,1) = 0;
                    }
                } catch(...) {
                }
            }
        }

	private:
        bool res_points_ = true;
};
REGISTER_KERNEL_BUILDER(Name("MinAreaRect").Device(DEVICE_CPU).TypeConstraint<uint8_t>("T"), MinAreaRectOp<CPUDevice, uint8_t>);

/*
 * mask:[N,H,W], 二值图像, 仅instance bboxes内部部分
 * bboxes:[N,4]绝对坐标
 * 当res_points==True时
 * return:
 * [N,4,2] 分别为[[[p0.x,p0.y],[p1.x,p1.y],..[p3.x,p3.y]],[....]]]
 * 当res_points==False时:
 * [N,3,2] 分别为[[[center.x,center.y],[width,height],[angle,_]],[.....]]]
 * 旋转角度θ是水平轴（x轴）逆时针旋转，与碰到的矩形的第一条边的夹角。并且这个边的边长是width，另一条边边长是height。也就是说，在这里，width与height不是按照长短来定义的。
 * 在opencv中，坐标系原点在左上角，相对于x轴，逆时针旋转角度为负，顺时针旋转角度为正。所以，θ∈（-90度，0]
 */
REGISTER_OP("MinAreaRectWithBboxes")
    .Attr("res_points: bool = True")
    .Attr("size_limit: list(int)= [63,1024]")
    .Input("mask: uint8")
    .Input("bboxes: float")
	.Output("box:float")
    .SetShapeFn([](shape_inference::InferenceContext* c){
            bool res_points = true;
            c->GetAttr("res_points",&res_points);
            auto input_shape0 = c->input(0);
            const auto batch_size = c->Dim(input_shape0,0);
                auto shape = c->MakeShape({batch_size,res_points?4:3,2});
                c->set_output(0, shape);
			return Status::OK();
            });

template <typename Device>
class MinAreaRectWithBboxesOp: public OpKernel {
    private:
        using bbox_t = Eigen::Tensor<float,1,Eigen::RowMajor>;
        using mask_t = Eigen::Tensor<uint8_t,2,Eigen::RowMajor>;
        using Tensor3D = Eigen::Tensor<uint8_t,3,Eigen::RowMajor>;
        using Tensor4D = Eigen::Tensor<uint8_t,4,Eigen::RowMajor>;
	public:
		explicit MinAreaRectWithBboxesOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("res_points", &res_points_));
			OP_REQUIRES_OK(context, context->GetAttr("size_limit", &size_limit_));
		}
        static cv::Mat to_mat(const mask_t& msk)
        {
            cv::Mat res(msk.dimension(0),msk.dimension(1),CV_8UC1,(void*)msk.data());
            return res.clone();
        }
		void Compute(OpKernelContext* context) override
		{
			const Tensor &_input_mask= context->input(0);
			const Tensor &_input_bboxes = context->input(1);

			OP_REQUIRES(context, _input_mask.dims() == 3, errors::InvalidArgument("mask must be at 3-dimensional"));
			OP_REQUIRES(context, _input_bboxes.dims() == 2, errors::InvalidArgument("bboxes must be at 2-dimensional"));

            auto         input_mask = _input_mask.template tensor<uint8_t,3>();
            auto         input_bboxes = _input_bboxes.template tensor<float,2>();
            const auto   batch_size    = _input_mask.dim_size(0);
            const auto   msk_height    = _input_mask.dim_size(1);
            const auto   msk_width     = _input_mask.dim_size(2);
            const int    dim3d[]       = {batch_size,res_points_?4:3,2};
            TensorShape  output_shape;
            Tensor      *output_tensor = nullptr;

            TensorShapeUtils::MakeShape(dim3d,3,&output_shape);

			OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &output_tensor));
            auto      o_tensor = output_tensor->template tensor<float,3>();

            o_tensor.setZero();

            for(auto i=0; i<batch_size; ++i) {
                try{
                    mask_t r_mask = input_mask.chip(i,0);
                    auto m_mask = to_mat(r_mask);
                    bbox_t box = input_bboxes.chip(i,0);
                    auto size = get_size(box);
                    cv::Mat n_mask;

                    cv::resize(m_mask,n_mask,cv::Size(get<1>(size),get<0>(size)),0,0,cv::INTER_LINEAR);

                    auto rect = getRotatedRect(n_mask);
                    auto scale = get<2>(size);

                    if(res_points_) {
                        cv::Point2f P[4];
                        rect.points(P);
                        for(auto j=0; j<4; ++j) {
                            o_tensor(i,j,0) = P[j].x/scale+box(1);
                            o_tensor(i,j,1) = P[j].y/scale+box(0);
                        }
                    } else {
                        o_tensor(i,0,0) = rect.center.x/scale+box(1);
                        o_tensor(i,0,1) = rect.center.y/scale+box(0);
                        o_tensor(i,1,0) = rect.size.width/scale;
                        o_tensor(i,1,1) = rect.size.height/scale;
                        o_tensor(i,2,0) = rect.angle;
                        o_tensor(i,2,1) = 0;
                    }
                } catch(...) {
                }
            }
        }
        auto get_size(const bbox_t& box) {
            auto         w              = box(3)-box(1);
            auto         h              = box(2)-box(0);
            const double min_size_limit = size_limit_[0] *size_limit_[0];
            const double max_size_limit = size_limit_[1] *size_limit_[1];
            double       scale          = 1.0;

            if(h*w>max_size_limit) {
                scale = sqrt(max_size_limit/(h*w));
            } else if(h*w<min_size_limit) {
                scale = sqrt(min_size_limit/(h*w));
            }
            h = h*scale;
            w = w*scale;
            return make_tuple(h,w,scale);
        }

	private:
        bool        res_points_ = true;
        vector<int> size_limit_;
};
REGISTER_KERNEL_BUILDER(Name("MinAreaRectWithBboxes").Device(DEVICE_CPU), MinAreaRectWithBboxesOp<CPUDevice>);

/*
 * 对输入image[H,W,C] C=1 or C=3 旋转任意角度
 */
REGISTER_OP("TensorRotate")
    .Attr("T: {uint8,float}")
    .Input("image: T")
    .Input("angle: float")
	.Output("o_image:T")
    .SetShapeFn([](shape_inference::InferenceContext* c){
            auto input_shape0 = c->input(0);
            c->set_output(0, input_shape0);
			return Status::OK();
            });

template <typename Device, typename T>
class TensorRotateOp: public OpKernel {
    private:
        using Tensor3D = Eigen::Tensor<T,3,Eigen::RowMajor>;
        using type_to_int_c1 = bm::map<
              bm::pair<uint8_t,bm::int_<CV_8UC1>>
                  , bm::pair<float,bm::int_<CV_32FC1>>
             >;
        using type_to_int_c3 = bm::map<
              bm::pair<uint8_t,bm::int_<CV_8UC3>>
                  , bm::pair<float,bm::int_<CV_32FC3>>
             >;
	public:
		explicit TensorRotateOp(OpKernelConstruction* context) : OpKernel(context) {
		}
		void Compute(OpKernelContext* context) override
		{
			const Tensor &_input_img = context->input(0);
			const Tensor &_angle     = context->input(1);

			OP_REQUIRES(context, _input_img.dims() == 3, errors::InvalidArgument("tensor must be a 3-dimensional tensor"));
			OP_REQUIRES(context, _angle.dims() == 0, errors::InvalidArgument("angle be a 0-dimensional tensor"));

            const auto img_height  = _input_img.dim_size(0);
            const auto img_width   = _input_img.dim_size(1);
            const auto img_channel = _input_img.dim_size(2);

			OP_REQUIRES(context, (img_channel == 1)||(img_channel==3), errors::InvalidArgument("image channel must be 1 or 3."));

            auto        input_img     = _input_img.template flat<T>().data();
            auto        angle         = _angle.template flat<float>().data()[0];
            Tensor     *output_tensor = nullptr;
            const auto  cv_type       = (img_channel==1?bm::at<type_to_int_c1,T>::type::value:bm::at<type_to_int_c3,T>::type::value);

			OP_REQUIRES_OK(context, context->allocate_output(0, _input_img.shape(), &output_tensor));

            auto      o_tensor = output_tensor->template flat<T>().data();
            cv::Mat i_img(img_height,img_width,cv_type,(uint8_t*)input_img);
            cv::Mat o_img(img_height,img_width,cv_type,(uint8_t*)o_tensor);
            const cv::Point2f cp(img_width/2,img_height/2);
            cv::Mat r = cv::getRotationMatrix2D(cp,angle,1.0);

            cv::warpAffine(i_img,o_img,r,cv::Size(img_width,img_height));
        }
};
REGISTER_KERNEL_BUILDER(Name("TensorRotate").Device(DEVICE_CPU).TypeConstraint<uint8_t>("T"), TensorRotateOp<CPUDevice, uint8_t>);
REGISTER_KERNEL_BUILDER(Name("TensorRotate").Device(DEVICE_CPU).TypeConstraint<float>("T"), TensorRotateOp<CPUDevice, float>);

/*
 * 输入绝对坐标
 */
REGISTER_OP("BboxesRotate")
    .Attr("T: {uint8,float}")
    .Attr("type:int=0")
    .Input("bboxes: T")
    .Input("angle: float")
    .Input("img_size: int32")
	.Output("o_bboxes:T")
    .SetShapeFn([](shape_inference::InferenceContext* c){
            auto input_shape0 = c->input(0);
            c->set_output(0, input_shape0);
			return Status::OK();
            });

template <typename Device, typename T>
class BboxesRotateOp: public OpKernel {
	public:
		explicit BboxesRotateOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("type", &type_));
		}
		void Compute(OpKernelContext* context) override
		{
			const auto &_input_bboxes = context->input(0);
			const auto &_angle     = context->input(1);
			const auto &_size = context->input(2);

			OP_REQUIRES(context, _input_bboxes.dims() == 2, errors::InvalidArgument("tensor must be a 2-dimensional tensor"));
			OP_REQUIRES(context, _angle.dims() == 0, errors::InvalidArgument("angle be a 0-dimensional tensor"));
			OP_REQUIRES(context, _size.dims() == 1, errors::InvalidArgument("size be a 1-dimensional tensor"));

            const auto  img_size      = _size.template flat<int32>().data();
            const auto  img_height    = img_size[0];
            const auto  img_width     = img_size[1];
            const auto  boxes_nr      = _input_bboxes.dim_size(0);
            auto        input_bboxes  = _input_bboxes.template tensor<T,2>();
            auto        angle         = _angle.template flat<float>().data()[0];
            Tensor     *output_tensor = NULL;

			OP_REQUIRES_OK(context, context->allocate_output(0, _input_bboxes.shape(), &output_tensor));

            auto      o_tensor = output_tensor->template tensor<T,2>();
            const cv::Point2f cp(img_width/2,img_height/2);
            cv::Mat r = cv::getRotationMatrix2D(cp,angle,1.0);
            for(auto i=0; i<boxes_nr; ++i) {
                transform(input_bboxes,i,r,o_tensor);
            }
        }
        template<typename BT0,typename BT1>
        void transform(const BT0& input_bboxes,int index, const cv::Mat& r,BT1& output_bboxes) {
            auto points = get_points(input_bboxes,index);
            vector<cv::Point> out_points(points.size());
            cv::transform(points,out_points,r);
            auto xmin = 1e10;
            auto xmax = 0;
            auto ymin = 1e10;
            auto ymax = 0;
            for(auto& p:out_points) {
                if(p.x<xmin)
                    xmin = p.x;
                if(p.x>xmax)
                    xmax = p.x;
                if(p.y<ymin)
                    ymin = p.y;
                if(p.y>ymax)
                    ymax = p.y;
            }
            output_bboxes(index,0) = ymin;
            output_bboxes(index,1) = xmin;
            output_bboxes(index,2) = ymax;
            output_bboxes(index,3) = xmax;
        }
        template<typename BT>
            vector<cv::Point> get_points(const BT& bboxes,int index) {
                const auto kNr = 100;
                vector<cv::Point> points;
                switch(type_) {
                    case 0:
                        {
                            auto ymin = bboxes(index,0);
                            auto xmin = bboxes(index,1);
                            auto ymax = bboxes(index,2);
                            auto xmax = bboxes(index,3);
                            const auto bw = xmax-xmin;
                            const auto bh = ymax-ymin;
                            const auto cx = (xmax+xmin)/2;
                            const auto cy = (ymax+ymin)/2;
                            const auto a = bw/2;
                            const auto b = bh/2;
                            for(auto i=0; i<kNr; ++i) {
                                const auto theta = 2*M_PI*i/kNr;
                                const auto x = cx+a*cos(theta);
                                const auto y = cy+b*sin(theta);
                                points.emplace_back(x,y);
                            }
                        }
                        break;
                    case 1:
                    default:
                        {
                            auto ymin = bboxes(index,0);
                            auto xmin = bboxes(index,1);
                            auto ymax = bboxes(index,2);
                            auto xmax = bboxes(index,3);
                            const auto bw = xmax-xmin;
                            const auto bh = ymax-ymin;
                            const auto cx = (xmax+xmin)/2;
                            const auto cy = (ymax+ymin)/2;
                            const auto a = bw/2;
                            const auto b = bh/2;
                            points.emplace_back(xmin,ymin);
                            points.emplace_back(xmin,ymax);
                            points.emplace_back(xmax,ymax);
                            points.emplace_back(xmax,ymin);
                        }
                        break;

                }
                return points;
            }
    private:
        int type_ = 0;
};
REGISTER_KERNEL_BUILDER(Name("BboxesRotate").Device(DEVICE_CPU).TypeConstraint<double>("T"), BboxesRotateOp<CPUDevice, double>);
REGISTER_KERNEL_BUILDER(Name("BboxesRotate").Device(DEVICE_CPU).TypeConstraint<float>("T"), BboxesRotateOp<CPUDevice, float>);


/**
 * Copyright (c) 2015 Carnegie Mellon University, Daniel Maturana <dimatura@cmu.edu>
 *
 * For License information please see the LICENSE file in the root directory.
 *
 */

#ifndef BOX_HPP_4WFLKG9Q
#define BOX_HPP_4WFLKG9Q

#include <ros/ros.h>
#include <Eigen/Core>
#include <Eigen/Dense>

namespace ca
{
namespace scrollgrid
{

/**
 * Describes an axis-aligned volume in space.
 */
template<typename Scalar, int Dim>
class Box {
 public:
  typedef Eigen::Matrix<Scalar, Dim, 1> Vec;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  Box() :
      center_(Vec::Zero()),
      radius_(Vec::Zero()),
      bounds_(Vec::Zero(), Vec::Zero())
  {
  }

  Box(const Vec& min_pt,
      const Vec& max_pt) :
      center_( (min_pt+max_pt)/2 ),
      radius_( (max_pt-min_pt)/2 ),
      bounds_( min_pt, max_pt )
  {
    ROS_ASSERT(check_bounds());
  }

  Box(const Box& other) :
      center_(other.center_),
      radius_(other.radius_),
      bounds_(other.bounds_)
  {
    ROS_ASSERT(check_bounds());
  }

  Box& operator=(const Box& other) {
    if (this==&other) { return *this; }
    center_ = other.center_;
    radius_ = other.radius_;
    bounds_ = other.bounds_;
    return *this;
  }

  const Vec& min_pt() const { return bounds_.first; }

  const Vec& max_pt() const { return bounds_.second; }

  const Vec& bound(int ix) const {
    ROS_ASSERT (ix >= 0 && ix < 2);
    if (ix==0) { return bounds_.first; }
    return bounds_.second;
  }

  void set_bound(int ix, const Vec& v) {
    ROS_ASSERT (ix >= 0 && ix < 2);
    ROS_ASSERT( check_bounds());
    if (ix==0) {
      bounds_.first = v;
    } else {
      bounds_.second = v;
    }
  }

  void set_min_pt(const Vec& min_pt) {
    bounds_.first = min_pt;
    //ROS_ASSERT( check_bounds());
    this->reset_center_radius();
  }

  void set_max_pt(const Vec& max_pt) {
    bounds_.second = max_pt;
    //ROS_ASSERT( check_bounds());
    this->reset_center_radius();
  }

  const Vec& center() const {
    return center_;
  }

  const Vec& radius() const {
    return radius_;
  }

  void set_center(const Vec& v) {
    Vec delta = v - center_;
    this->translate(delta);
  }

  void set_radius(const Vec& v) {
    radius_ = v;
    bounds_.first = center_ - radius_;
    bounds_.second = center_ + radius_;
  }

  void translate(const Vec& v) {
    center_ += v;
    bounds_.first += v;
    bounds_.second += v;
  }

  bool contains(const Vec& v) const {
    return (bounds_.first.array() <= v.array()).all() &&
        (v.array() <= bounds_.second.array()).all();
  }

#if 0
  template<typename Scalar2>
  Box<Scalar2, Dim> cast() {
    Box<Scalar2, Dim> ret(bounds_[0].template cast<Scalar2>(),
                          bounds_[1].template cast<Scalar2>());
    return ret;
  }
#endif

 private:
  // call after bounds change
  void reset_center_radius() {
    center_ = (bounds_.first+bounds_.second)/2;
    radius_ = (bounds_.second-bounds_.first)/2;
  }

  bool check_bounds() {
    return bounds_.first.x() <= bounds_.second.x() &&
           bounds_.first.y() <= bounds_.second.y() &&
           bounds_.first.z() <= bounds_.second.z();
  }

 private:

  Vec center_;
  Vec radius_;
  std::pair<Vec, Vec> bounds_;
};

}
} /* ca */

#endif /* end of include guard: BOX_HPP_4WFLKG9Q */


#ifndef CONCEPTS_DIMENSION_ORDERABLE_HPP_INCLUDED
#define CONCEPTS_DIMENSION_ORDERABLE_HPP_INCLUDED

#include <boost/hana/less.hpp>
#include <boost/hana/less_equal.hpp>
#include <boost/hana/greater.hpp>
#include <boost/hana/greater_equal.hpp>

#include <gem/concept/dimensions_common.hpp>
#include <gem/fwd/dimensions.hpp>

namespace gem {

template <concepts::Dimension D1, concepts::Dimension D2>
constexpr auto
operator >=(const D1& d1, const D2& d2) noexcept
{
    return d1.value() >= d2.value();
}

concepts::InvalidCommonDimensionPair {D1, D2}
constexpr auto
operator >=(const D1& d1, const D2& d2) noexcept
{
    return d1.value() >= d2.value();
}

template <concepts::Dimension D1, concepts::detail::Integral T2>
constexpr auto
operator >=(const D1& d1, const T2& d2) noexcept
{
    return d1.value() >= d2;
}

template <concepts::detail::Integral T1, concepts::Dimension D2>
constexpr auto
operator >=(const T1& d1, const D2& d2) noexcept
{
    return d1 >= d2.value();
}

template <concepts::Dimension D1, concepts::Dimension D2>
constexpr auto
operator >(const D1& d1, const D2& d2) noexcept
{
    return d1.value() > d2.value();
}

concepts::InvalidCommonDimensionPair {D1, D2}
constexpr auto
operator >(const D1& d1, const D2& d2) noexcept
{
    return d1.value() > d2.value();
}

template <concepts::Dimension D1, concepts::detail::Integral T2>
constexpr auto
operator >(const D1& d1, const T2& d2) noexcept
{
    return d1.value() > d2;
}

template <concepts::detail::Integral T1, concepts::Dimension D2>
constexpr auto
operator >(const T1& d1, const D2& d2) noexcept
{
    return d1 > d2.value();
}

template <concepts::Dimension D1, concepts::Dimension D2>
constexpr auto
operator <=(const D1& d1, const D2& d2) noexcept
{
    return d1.value() <= d2.value();
}

concepts::InvalidCommonDimensionPair {D1, D2}
constexpr auto
operator <=(const D1& d1, const D2& d2) noexcept
{
    return d1.value() <= d2.value();
}

template <concepts::Dimension D1, concepts::detail::Integral T2>
constexpr auto
operator <=(const D1& d1, const T2& d2) noexcept
{
    return d1.value() <= d2;
}

template <concepts::detail::Integral T1, concepts::Dimension D2>
constexpr auto
operator <=(const T1& d1, const D2& d2) noexcept
{
    return d1 <= d2.value();
}

template <concepts::Dimension D1, concepts::Dimension D2>
constexpr auto
operator <(const D1& d1, const D2& d2) noexcept
{
    return d1.value() < d2.value();
}

concepts::InvalidCommonDimensionPair {D1, D2}
constexpr auto
operator <(const D1& d1, const D2& d2) noexcept
{
    return d1.value() < d2.value();
}

template <concepts::Dimension D1, concepts::detail::Integral T2>
constexpr auto
operator <(const D1& d1, const T2& d2) noexcept
{
    return d1.value() < d2;
}

template <concepts::detail::Integral T1, concepts::Dimension D2>
constexpr auto
operator <(const T1& d1, const D2& d2) noexcept
{
    return d1 < d2.value();
}

}  // namespace gem

#endif  // !CONCEPTS_DIMENSION_ORDERABLE_HPP_INCLUDED


/**
 * @file forward_visitor.hpp
 * @author Marcus Edel
 *
 * This file provides an abstraction for the Forward() function for different
 * layers and automatically directs any parameter to the right layer type.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_VISITOR_FORWARD_VISITOR_HPP
#define MLPACK_METHODS_ANN_VISITOR_FORWARD_VISITOR_HPP

#include <mlpack/methods/ann/layer/layer_traits.hpp>
#include <mlpack/methods/ann/layer/layer_types.hpp>

#include <boost/variant.hpp>

namespace mlpack {
namespace ann {

/**
 * ForwardVisitor executes the Forward() function given the input and output
 * parameter.
 */
class ForwardVisitor : public boost::static_visitor<void>
{
 public:
  //! Execute the Forward() function given the input and output parameter.
  ForwardVisitor(arma::mat&& input, arma::mat&& output);

  //! Execute the Forward() function.
  template<typename LayerType>
  void operator()(LayerType* layer) const;

  void operator()(MoreTypes layer) const;

 private:
  //! The input parameter set.
  arma::mat&& input;

  //! The output parameter set.
  arma::mat&& output;
};

} // namespace ann
} // namespace mlpack

// Include implementation.
#include "forward_visitor_impl.hpp"

#endif


#ifndef STAN_MATH_PRIM_SCAL_PROB_EXPONENTIAL_LCDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_EXPONENTIAL_LCDF_HPP

#include <boost/random/exponential_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <cmath>

namespace stan {
namespace math {

template <typename T_y, typename T_inv_scale>
typename return_type<T_y, T_inv_scale>::type exponential_lcdf(
    const T_y& y, const T_inv_scale& beta) {
  typedef typename stan::partials_return_type<T_y, T_inv_scale>::type
      T_partials_return;

  static const char* function = "exponential_lcdf";

  using boost::math::tools::promote_args;
  using std::exp;
  using std::log;

  T_partials_return cdf_log(0.0);
  if (size_zero(y, beta))
    return cdf_log;

  check_not_nan(function, "Random variable", y);
  check_nonnegative(function, "Random variable", y);
  check_positive_finite(function, "Inverse scale parameter", beta);

  operands_and_partials<T_y, T_inv_scale> ops_partials(y, beta);

  scalar_seq_view<T_y> y_vec(y);
  scalar_seq_view<T_inv_scale> beta_vec(beta);
  size_t N = max_size(y, beta);
  for (size_t n = 0; n < N; n++) {
    const T_partials_return beta_dbl = value_of(beta_vec[n]);
    const T_partials_return y_dbl = value_of(y_vec[n]);
    T_partials_return one_m_exp = 1.0 - exp(-beta_dbl * y_dbl);
    cdf_log += log(one_m_exp);

    T_partials_return rep_deriv = -exp(-beta_dbl * y_dbl) / one_m_exp;
    if (!is_constant_struct<T_y>::value)
      ops_partials.edge1_.partials_[n] -= rep_deriv * beta_dbl;
    if (!is_constant_struct<T_inv_scale>::value)
      ops_partials.edge2_.partials_[n] -= rep_deriv * y_dbl;
  }
  return ops_partials.build(cdf_log);
}

}  // namespace math
}  // namespace stan
#endif