/**
 * @file path_follow.cpp
 * @brief Offboard path trajectory tracking
*/

#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <oscillation_ctrl/RefSignal.h>
#include <oscillation_ctrl/EulerAngles.h>
#include <oscillation_ctrl/WaypointTrack.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <iostream>
#include <cmath> // for std::abs

/********* CALLBACK FUNCTIONS **********************/

// Callback function which will save the current state of the autopilot. 
// Allows to check connection, arming, and Offboard tags*/
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

// Cb to determine attitude target
mavros_msgs::AttitudeTarget att_targ;
void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){
    att_targ = *msg;
}

// Cb to recieve pose information
geometry_msgs::PoseStamped buff_pose;
geometry_msgs::Quaternion q_init;
geometry_msgs::PoseStamped mavPose;
bool gps_read = false;
double current_altitude;
void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
	mavPose = *msg;
	current_altitude = mavPose.pose.position.z;
	while(gps_read == false){
		q_init = mavPose.pose.orientation;
		if(q_init.x == 0.0 && q_init.w == 0.0){
			ROS_INFO("Waiting...");
		} else {
			gps_read = true;
		    	buff_pose.pose.orientation.x = q_init.x;
   				buff_pose.pose.orientation.y = q_init.y;
    			buff_pose.pose.orientation.z = q_init.z;
    			buff_pose.pose.orientation.w = q_init.w;
		}
	}
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "path_follow");
    ros::NodeHandle nh;
    
    /********************** SUBSCRIBERS **********************/
    // Get current state
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
		("mavros/state", 10, state_cb);

    // Get attitude target from klausen control
    ros::Subscriber att_target_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
		("command/att_target",10,att_targ_cb);

    // Pose subscriber
    ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
		("mavros/local_position/pose",10,mavPose_cb);

    /********************** PUBLISHERS **********************/
    // Initiate publisher to publish commanded local position
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
		("mavros/setpoint_position/local", 10);

    // Publish attitude and thrust commands
    ros::Publisher att_targ_pub = nh.advertise<mavros_msgs::AttitudeTarget>
		("mavros/setpoint_raw/attitude",10);

    // Service Clients
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
		("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
    	("mavros/set_mode");
    ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
		("mavros/cmd/takeoff");
	ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
		("status/waypoint_tracker");

    //the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
    ros::Rate rate_wait(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
		ROS_INFO("Waiting for FCU connection");
        rate_wait.sleep();
    }

	if (current_state.connected){
		ROS_INFO("Connected to FCU");
	} else {
		ROS_INFO("Never Connected");
	}
   
    /*********** Initiate variables ************************/
	//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
    ros::Rate rate_pub(25.0);

	// Retrieve desired waypoints
	oscillation_ctrl::WaypointTrack wpoint_srv;
	wpoint_srv.request.query = false;
	if (waypoint_cl.call(wpoint_srv))
	{
		ROS_INFO("Waypoints received");
	}
	
	// populate desired waypoints - this is only for original hover as 
	// a change of waypoints is handled by ref_signal.py
    buff_pose.pose.position.x = wpoint_srv.response.xd.x;
    buff_pose.pose.position.y = wpoint_srv.response.xd.y;
    buff_pose.pose.position.z = wpoint_srv.response.xd.z;
	double alt_des = buff_pose.pose.position.z; // Desired height

    // Desired mode is offboard
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // Arm UAV
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // Take off command    
    bool takeoff = false, att_cmds = false;

    // Keep track of time since requests
    ros::Time tkoff_req = ros::Time::now();
    ros::Time last_request = ros::Time::now();

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(buff_pose);
        ros::spinOnce();
		ROS_INFO("Publishing position setpoints");
        rate_wait.sleep();
    }

    while(ros::ok()){
		if(gps_read == true){
			if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
				if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
				} else {
					ROS_INFO("Could not enter offboard mode");
				}
				//last_request = ros::Time::now();
			} else {
				if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
					if( arming_client.call(arm_cmd) && arm_cmd.response.success){
						ROS_INFO("Vehicle armed");
					}
					last_request = ros::Time::now();
				}
			}
			
			if(current_state.mode == "OFFBOARD" && current_state.armed){
				ROS_INFO_ONCE("Spiri is taking off");
				if(!takeoff){		
					tkoff_req = ros::Time::now();
					takeoff = true;
				}
			}
			
			// Determine which messages to send
			if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
				att_targ.header.stamp = ros::Time::now();
				att_targ_pub.publish(att_targ);
				att_cmds = true;
				ROS_INFO("ATTITUDE CTRL");
				ROS_INFO("Des Altitude: %.2f", alt_des);
				ROS_INFO("Cur Altitude: %.2f", current_altitude);	
			} else {
				local_pos_pub.publish(buff_pose);
				ROS_INFO("POSITION CTRL");
				ROS_INFO("Des Altitude: %.2f", alt_des);
				ROS_INFO("Cur Altitude: %.2f", current_altitude);	
			}			
			ros::spinOnce();
			rate_pub.sleep();
		}
    }

    return 0;
}