#include "ros/ros.h"
#include <papillon/BoundingBox.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Empty.h>
#include <ncurses.h>

#include <opencv/cv.h>

#include <sstream>

using namespace std;

class Drone_control {
	public:
		constexpr static float MAX_ANG_VEL = 0.3;

		ros::NodeHandle n;
		ros::Publisher pub_cmd;
		ros::Publisher pub_takeoff;
		ros::Publisher pub_land;
		ros::Subscriber sub_box;
		std_msgs::Empty nope;

		float THRES_TURN = 0.1;
		bool emergency = false;

		// 1 left | 0 center | -1 right
		vector<int> decision_acc;
		const static int NB_ACC = 5;


		Drone_control() : n("~") {
			pub_cmd = n.advertise<geometry_msgs::Twist>("/bebop/cmd_vel", 1);
			pub_takeoff = n.advertise<std_msgs::Empty>("/bebop/takeoff", 1);
			pub_land = n.advertise<std_msgs::Empty>("/bebop/land", 1);
			sub_box = n.subscribe<papillon::BoundingBox>("/papillon/bbox", 1, &Drone_control::on_msg, this);

			// Initialise decision accumulator
			for (size_t i = 0; i < NB_ACC; ++i)
				decision_acc.push_back(0);

			ROS_INFO("Built !");
		}


		// This processes an image and publishes the result.
		void on_msg(const papillon::BoundingBox::ConstPtr& bbox) {
			geometry_msgs::Twist twist = geometry_msgs::Twist();
			cv::Point2f r_center = cv::Point2f(bbox->x + bbox->width/2, bbox->y + bbox->height/2);
			if (!emergency) {
				decision_acc.pop_back();
				// Image is 1.0 in width and height
				if (r_center.x < 0.5 - THRES_TURN)
					decision_acc.insert(decision_acc.begin(), 1);
				else if (r_center.x > 0.5 + THRES_TURN)
					decision_acc.insert(decision_acc.begin(), -1);
				else
					decision_acc.insert(decision_acc.begin(), 0);

				int moy = 0;
				for (const auto& d : decision_acc)
					moy += d;
				twist.angular.z = moy / (float)NB_ACC * MAX_ANG_VEL;
			}

			pub_cmd.publish(twist);
		}

		void key_spin() {
			// >>>>> nCurses initization
			initscr();
			keypad(stdscr, TRUE);
			cbreak();
			noecho();
			timeout(0); // Non blocking getch()
			// <<<<< nCurses initization

			int c;

			printw(" ------------------------\n");
			printw("|      Papillon HQ       |\n");
			printw(" ------------------------ \n");
			printw("| Press F to take off.   |\n");
			printw("| Press SPACEBAR to land.|\n");
			printw("| Press Q to exit.       |\n");
			printw(" ------------------------\n");

			bool stop = false;

			ros::Rate rate(1.0/30.0);  // 30 Hz
			while(!stop)
			{
				c = getch();

				switch(c)
				{
				case 'f':
				{
					// Takeoff
					ROS_DEBUG_STREAM("Taking off...\r");
					pub_takeoff.publish(nope);
					emergency = false;
					printw("Taking off...\r");
					break;
				}
				case ' ':
				{
					// Takeoff
					ROS_DEBUG_STREAM("Landing...\r");
					pub_land.publish(nope);
					printw("Landing...\r");
					emergency = true;
					break;
				}
				case 'q':
				case 'Q':
				{
					ROS_DEBUG_STREAM("EXIT\r");
					stop = true;
				}
				}
				ros::spinOnce();
			}
			printw("\nExiting\n");
		}
};


int main(int argc, char **argv)
{
	ros::init(argc, argv, "control");
	Drone_control con=Drone_control();
	con.key_spin();

	return 0;
}