// OpenCV
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
// standard c++ includes
#include <iostream>
#include <vector>
#include <set>
#include <cmath>
#include <string>
#include <fstream>
#include <iostream>
#include <stdio.h>
// Eigen library for matrix/vector computations
#include <Eigen/Dense>
// Coin-Or library with Cbc mixed integer programming solver
#include <coin/OsiClpSolverInterface.hpp>
#include <coin/CoinModel.hpp>
#include <coin/CbcModel.hpp>
#include <coin/CbcHeuristicFPump.hpp>
// Coin-Or library with Clp linear programming solver
#include <coin/ClpSimplex.hpp>
// Boost libraries
#include <boost/config.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/adjacency_list.hpp>
// package specific includes
#include <ipa_building_navigation/A_star_pathplanner.h>
#include <ipa_building_navigation/distance_matrix.h>
#include <ipa_building_navigation/contains.h>
#include <ipa_room_exploration/fov_to_robot_mapper.h>
#include <ipa_room_exploration/room_rotator.h>
// msgs
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/Point32.h>
// if available, use Gurobi
#ifdef GUROBI_FOUND
	#include "gurobi_c++.h"
#endif

/*!
 *****************************************************************
 * \file
 *
 * \note
 * Copyright (c) 2016 \n
 * Fraunhofer Institute for Manufacturing Engineering
 * and Automation (IPA) \n\n
 *
 *****************************************************************
 *
 * \note
 * Project name: Care-O-bot
 * \note
 * ROS stack name: autopnp
 * \note
 * ROS package name: ipa_room_exploration
 *
 * \author
 * Author: Florian Jordan
 * \author
 * Supervised by: Richard Bormann
 *
 * \date Date of creation: 11.2016
 *
 * \brief
 *
 *
 *****************************************************************
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * - Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer. \n
 * - Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution. \n
 * - Neither the name of the Fraunhofer Institute for Manufacturing
 * Engineering and Automation (IPA) nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission. \n
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 ****************************************************************/

// This struct is used to create arcs for the flow network, used to plan a coverage path trough the given map.
struct arcStruct
{
	cv::Point start_point;
	cv::Point end_point;
	double weight;
	std::vector<cv::Point> edge_points;
};

// typedef for boost, defining a directed graph
typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::directedS > directedGraph;

// TODO: update
// This class provides a coverage path planning algorithm based on a flow network. It spans such a network by going trough
// the given room map with a sweep line and defining edges of it whenever it touches an obstacle. From this touchpoint
// the edge is generated in a distance along the sweep line, that is equivalent to the given coverage radius, because the
// free space should determine the area that should be covered after this procedure. After this the arcs of the network
// are generated by taking the distance of two edges as its weight (gives two directed edges), if the arc is not too long or
// not straight enough (to reduce the dimensionality). To find a coverage path trough this flow network then a linear
// program of the form
//		min 	c^T*w
//
// 		s.t. 	Vc >= 1 (row-wise)
//				sum(flows_into_node) - sum(flows_out_of_node) = 0;
//					flows_into_nodes: variables corresponding to arcs flowing into an edge/node, excluding the final arcs
//					flows_out_of_nodes: variables corresponding to arcs flowing out of an edge/node, excluding the start arcs
//				sum(start_flows) = 1
//				sum(terminal_flows) = 1
//				sum(flows_of_subset) <= |subset|-1; for any node subset S with |S|>=2
//				c(i)={0;1}
// is solved, where 3 stages are taken, the initial step to start the path somewhere, the coverage stage in which the arcs
// should cover the most of the area and the final step where the path can be terminated. These three stages are necessary,
// because the second constraint ensures that the path is connected and not consisting of separated parts. The initial step
// then starts the path at the desired position and the final step allows a termination of this flow conservativity constraint.
// The first constraint ensures that each cell of the free space is covered. The fourth constraint simply ensures to start
// at the node that one specifies. The last constraint prevents that the solution consists of different cycles. A cycle would
// fulfill the conservativity constraint, but of course may not be connected to the rest of the path. By saying that the
// number of goen arcs in a subset of nodes has to be smaller than the size of the subset lesser 1, not all arcs can be
// gone together, which prevents cycles. These cosntraints are added in a lazy fashion, meaning that after a solution has
// been obtained it is checked if the constraints have been violated in any way. If so, the constraints are added and the
// model is resolved. This step repeats, until no more cycles, that are not connected to the rest of the path, appear.
//
#pragma once

#define PI 3.14159265359

#ifdef GUROBI_FOUND
	class CyclePreventionCallbackClass: public GRBCallback
	{
	  public:
		std::vector<GRBVar>* vars;
		int n;
		std::vector<std::vector<uint> > flows_out_of_nodes, flows_into_nodes;
		std::vector<uint> start_arcs;
		std::vector<std::vector<double> > lhs;
		std::vector<double> rhs;

		CyclePreventionCallbackClass(std::vector<GRBVar>* xvars, int xn, const std::vector<std::vector<uint> >& outflows,
				const std::vector<std::vector<uint> >& inflows, const std::vector<uint>& start_indices)
		{
		  vars = xvars;
		  n = xn;
		  flows_out_of_nodes = outflows;
		  flows_into_nodes = inflows;
		  start_arcs = start_indices;
		}

	  protected:
		void callback()
		{
		  try
		  {
			if (where==GRB_CB_MIPSOL)
			{
			  // Found an integer feasible solution
			  std::vector<double> solution(vars->size());
			  for (int var=0; var<vars->size(); var++)
			  {
				  double current_value = GRBCallback::getSolution(vars->operator[](var));
				  solution[var] = (current_value>=0) ? current_value : 0.0;
//				  std::cout << solution[var] << std::endl;
			  }

			  // check if cycles appear in the solution
			  // get the arcs that are used in the previously calculated solution
			  std::set<uint> used_arcs; // set that stores the indices of the arcs corresponding to non-zero elements in the solution

			  // go trough the start arcs
			  std::cout << "getting used arcs: ";
			  // TODO: add again, better reading out of the paths
//			  for(uint start_arc=0; start_arc<start_arcs.size(); ++start_arc)
//			  {
////				  std::cout << solution[start_arc] << std::endl;
//				  if(solution[start_arc]>0.01) // precision of the solver
//				  {
//					  // insert start index
//					  used_arcs.insert(start_arcs[start_arc]);
//					  std::cout << start_arcs[start_arc] << " ";
//				  }
//			  }

			  // go trough the coverage stage
			  std::cout << "| ";
			  for(uint cover_arc=start_arcs.size(); cover_arc<start_arcs.size()+n; ++cover_arc)
			  {
//				  std::cout << cover_arc << std::endl;
				  if(solution[cover_arc]>0.01) // precision of the solver
				  {
					  // insert index, relative to the first coverage variable
					  used_arcs.insert(cover_arc-start_arcs.size());
					  std::cout << cover_arc-start_arcs.size() << " ";
				  }
			  }

			  std::cout << "| ";
			  // TODO: add again, better reading out of the paths
			  // go trough the final stage and find the remaining used arcs
//			  for(uint flow=start_arcs.size()+n; flow<start_arcs.size()+2*n; ++flow)
//			  {
//				  if(solution[flow]>0.01) // precision of the solver
//				  {
//					  // insert saved outgoing flow index
//					  used_arcs.insert(flow-start_arcs.size()-n);
//					  std::cout << flow-start_arcs.size()-n << " ";
//				  }
//			  }

			  std::cout << "got " << used_arcs.size() << " used arcs" << std::endl;

			  // construct the directed edges out of the used arcs
			  std::vector<std::vector<int> > directed_edges; // vector that stores the directed edges for each node
			  for(uint start_node=0; start_node<flows_out_of_nodes.size(); ++start_node)
			  {
				  // check if a used arc is flowing out of the current start node
				  std::vector<uint> originating_flows;
				  bool originating = false;
				  for(std::set<uint>::iterator arc=used_arcs.begin(); arc!=used_arcs.end(); ++arc)
				  {
					  if(contains(flows_out_of_nodes[start_node], *arc)==true)
					  {
						  originating = true;
						  originating_flows.push_back(*arc);
					  }
				  }

				  // if the arc is originating from this node, find its destination
				  std::vector<int> current_directed_edges;
				  if(originating==true)
				  {
					  for(uint end_node=0; end_node<flows_into_nodes.size(); ++end_node)
					  {
						  if(end_node==start_node)
							  continue;

						  for(std::vector<uint>::iterator arc=originating_flows.begin(); arc!=originating_flows.end(); ++arc)
							  if(contains(flows_into_nodes[end_node], *arc)==true)
								  current_directed_edges.push_back(end_node);
					  }
				  }

				  // if the current node doesn't flow into another node insert a vector storing -1
				  if(current_directed_edges.size()==0)
					  current_directed_edges.push_back(-1);

				  // save the found used directed edges
				  directed_edges.push_back(current_directed_edges);
			  }

			  // construct the support graph out of the directed edges
			  directedGraph support_graph(flows_out_of_nodes.size()); // initialize with the right number of edges

			  int number_of_not_used_nodes = 0;
			  for(size_t start=0; start<directed_edges.size(); ++start)
			  {
//				  std::cout << "node " << start << " has destinations: " << std::endl;
				  for(size_t end=0; end<directed_edges[start].size(); ++end)
				  {
//					  std::cout << directed_edges[start][end] << std::endl;
					  // if no destination starting from this node could be found ignore this node
					  if(directed_edges[start][end]==-1)
					  {
						  break;
						  ++number_of_not_used_nodes;
					  }

					  // add the directed edge
					  boost::add_edge(start, directed_edges[start][end], support_graph);
				  }
			  }

			  // search for the strongly connected components
			  std::vector<int> c(flows_into_nodes.size());
			  int number_of_strong_components = boost::strong_components(support_graph, boost::make_iterator_property_map(c.begin(), boost::get(boost::vertex_index, support_graph), c[0]));
			  std::cout << "got " << number_of_strong_components << " strongly connected components" << std::endl;
//			  for (std::vector <int>::iterator i = c.begin(); i != c.end(); ++i)
//			  		std::cout << "Vertex " << i - c.begin() << " is in component " << *i << std::endl;

			  // check how many cycles there are in the solution (components with a size >= 2)
			  int number_of_cycles = 0;
			  std::set<int> done_components; // set that stores the component indices for that the nodes already were found
			  for(std::vector<int>::iterator comp=c.begin(); comp!=c.end(); ++comp)
			  {
				  // don't check a component more than one time
				  if(done_components.find(*comp)!=done_components.end())
					  continue;

				  int elements = std::count(c.begin(), c.end(), *comp);
				  if(elements>=2)
					  ++number_of_cycles;

				  // check if a tsp path is computed (number of arcs is same as number of nodes), each arc is a strongly
				  // connected component itself or all the nodes belong to one strongly connected component
				  // TODO: add again, better reading out of the paths
//				  if(elements==used_arcs.size() || elements==flows_out_of_nodes.size())
//					  number_of_cycles=0;

				  // add it to done components
				  done_components.insert(*comp);
			  }
			  std::cout << "current number of cycles: " << number_of_cycles << std::endl;

			  // if more than one cycle appears find it and add the prevention constraint to the problem and resolve it
			  if(number_of_cycles!=0)
			  {
				  // go trough the components and find components with more than 1 element in it
				  std::vector<std::vector<uint> > cycle_nodes;
				  done_components.clear();
				  for (int component=0; component<c.size(); ++component)
				  {
					  // check if the component hasn't been done yet
					  if(done_components.find(c[component])==done_components.end())
					  {
						  std::vector<uint> current_component_nodes;
						  int elements = std::count(c.begin(), c.end(), c[component]);
//						  std::cout << c[component] << std::endl;
						  // TODO: if connected to rest of path --> okay, don't add constraint
						  if(elements>=2)
						  {
//							  std::cout << "size: " << elements << std::endl;
							  for(std::vector<int>::iterator element=c.begin(); element!=c.end(); ++element)
							  {
								  if(*element==c[component])
								  {
									  current_component_nodes.push_back(element-c.begin());
								  }
							  }

							  // save the found cycle
							  if(current_component_nodes.size()>0)
								  cycle_nodes.push_back(current_component_nodes);

							  // add it to done components
							  done_components.insert(c[component]);
						  }
					  }
				  }

				  // add the cycle prevention constraints for each found cycle to the optimization problem
				  for(size_t cycle=0; cycle<cycle_nodes.size(); ++cycle)
				  {
					  // for each cycle find the arcs that lie in it
//					  std::cout << "size: " << cycle_nodes[cycle].size() << std::endl;
//					  std::cout << "constraint: " << std::endl;
					  std::vector<int> cpc_indices;
					  std::vector<double> cpc_coefficients;
					  for(size_t node=0; node<cycle_nodes[cycle].size(); ++node)
					  {

						  for(std::vector<uint>::iterator outflow=flows_out_of_nodes[cycle_nodes[cycle][node]].begin(); outflow!=flows_out_of_nodes[cycle_nodes[cycle][node]].end(); ++outflow)
						  {
							  for(size_t neighbor=0; neighbor<cycle_nodes[cycle].size(); ++neighbor)
							  {
								  if(neighbor==node)
									  continue;

								  // if a cycle-node contains this arc is incoming node and was used in the solution, it belongs to the subset
								  if(contains(flows_into_nodes[cycle_nodes[cycle][neighbor]], *outflow)==true && used_arcs.find(*outflow)!=used_arcs.end())
								  {
									  cpc_indices.push_back(*outflow+start_arcs.size());
									  cpc_coefficients.push_back(1.0);
								  }
							  }
						  }

						  // gather the arcs in outgoing from all neighbors
//						  for(size_t neighbor=0; neighbor<cycle_nodes[cycle].size(); ++neighbor)
//						  {
//							  // for the node itself gather the outflows that belong to the cycle
//							  if(neighbor==node)
//							  {
////								  std::cout << "node itself: " << cycle_nodes[cycle][neighbor] << std::endl;
//								  int flow_index = -1;
//								  for(std::vector<uint>::const_iterator outflow=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].begin(); outflow!=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].end(); ++outflow)
//								  {
//									  // if the current arc is used in the solution, search for it in the incoming flows of
//									  // the other nodes in the cycle
//									  if(used_arcs.find(*outflow)!=used_arcs.end())
//										  for(size_t new_node=0; new_node<cycle_nodes[cycle].size(); ++new_node)
//											  if(contains(flows_into_nodes[cycle_nodes[cycle][new_node]], *outflow)==true)
//												  flow_index=*outflow;
//								  }
//
//								  // if the outflow is an inflow of another cycle node, add its index to the constraint
//								  if(flow_index!=-1)
//								  {
//									  cpc_indices.push_back(flow_index+start_arcs.size());
//									  cpc_coefficients.push_back(-1.0);
//								  }
//							  }
//							  // for other nodes gather outflows that are not part of the cycle
//							  else
//							  {
////									std::cout << "neighbor" << std::endl;
//								  bool in_cycle = false;
//								  for(std::vector<uint>::const_iterator outflow=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].begin(); outflow!=flows_out_of_nodes[cycle_nodes[cycle][neighbor]].end(); ++outflow)
//								  {
//									  // search for the current flow in the incoming flows of the other nodes in the cycle
//									  for(size_t new_node=0; new_node<cycle_nodes[cycle].size(); ++new_node)
//										  if(contains(flows_into_nodes[cycle_nodes[cycle][new_node]], *outflow)==true)
//											  in_cycle=true;
//
//									  // if the arc is not in the cycle add its index
//									  if(in_cycle==false)
//									  {
//										  cpc_indices.push_back(*outflow+start_arcs.size());
//										  cpc_coefficients.push_back(1.0);
//									  }
//
//									  // reset the indication boolean
//									  in_cycle = false;
//								  }
//							  }
//						  }
					  }

//					  std::cout << "adding constraint" << std::endl;

					  // add the constraint
					  GRBLinExpr current_cpc_constraint;
					  std::vector<double> current_lhs;
					  for(size_t var=0; var<cpc_indices.size(); ++var)
					  {
						  current_cpc_constraint += cpc_coefficients[var]*vars->operator[](cpc_indices[var]);
						  current_lhs.push_back(cpc_coefficients[var]*cpc_indices[var]);
//						  std::cout << cpc_coefficients[var] << "*" << cpc_indices[var] << std::endl;
					  }
//					  std::cout << "adding lazy, size of rhs: " << cycle_nodes[cycle].size()-1 << std::endl;
					  addLazy(current_cpc_constraint<=cycle_nodes[cycle].size()-1);
					  lhs.push_back(current_lhs);
					  rhs.push_back(cycle_nodes[cycle].size()-1);
				  }
			  }
			}
		  }
		  catch (GRBException e)
		  {
			std::cout << "Error number: " << e.getErrorCode() << std::endl;
			std::cout << e.getMessage() << std::endl;
		  }
		  catch (...)
		  {
			  std::cout << "Error during callback" << std::endl;
		  }
		}
	};
#endif

class FlowNetworkExplorator
{
protected:
	// function that is used to create and solve a Cbc optimization problem out of the given matrices and vectors, using
	// the three-stage ansatz and single-flow cycle prevention constraints
	void solveThreeStageOptimizationProblem(std::vector<double>& C, const cv::Mat& V, const std::vector<double>& weights,
			const std::vector<std::vector<uint> >& flows_into_nodes, const std::vector<std::vector<uint> >& flows_out_of_nodes,
			const std::vector<uint>& start_arcs);

	// function that is used to create and solve a Gurobi optimization problem out of the given matrices and vectors, using
	// the three-stage ansatz and lazy generalized cutset inequalities (GCI)
	void solveGurobiOptimizationProblem(std::vector<double>& C, const cv::Mat& V, const std::vector<double>& weights,
			const std::vector<std::vector<uint> >& flows_into_nodes, const std::vector<std::vector<uint> >& flows_out_of_nodes,
			const std::vector<uint>& start_arcs);

	// function that is used to create and solve a Cbc optimization problem out of the given matrices and vectors, using
	// the three-stage ansatz and lazy generalized cutset inequalities (GCI)
	void solveLazyConstraintOptimizationProblem(std::vector<double>& C, const cv::Mat& V, const std::vector<double>& weights,
			const std::vector<std::vector<uint> >& flows_into_nodes, const std::vector<std::vector<uint> >& flows_out_of_nodes,
			const std::vector<uint>& start_arcs);

	// function that checks if the given point is more close enough to any point in the given vector
	bool pointClose(const std::vector<cv::Point>& points, const cv::Point& point, const double min_distance);

	// object that plans a path from A to B using the Astar method
	AStarPlanner path_planner_;

public:
	// constructor
	FlowNetworkExplorator();

	// Function that creates an exploration path for a given room. The room has to be drawn in a cv::Mat (filled with Bit-uchar),
	// with free space drawn white (255) and obstacles as black (0). It returns a series of 2D poses that show to which positions
	// the robot should drive at. The footprint stores a polygon that is used to determine the visibility at a specific
	// sensing pose. delta_theta provides an angular step to determine candidates for sensing poses.
	void getExplorationPath(const cv::Mat& room_map, std::vector<geometry_msgs::Pose2D>& path, const float map_resolution,
				const cv::Point starting_position, const cv::Point2d map_origin,
				const int cell_size, const Eigen::Matrix<float, 2, 1>& robot_to_fov_middlepoint_vector, const float coverage_radius,
				const bool plan_for_footprint, const double path_eps, const double curvature_factor, const double max_distance_factor);

	// test function
	void testFunc();
};
