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

#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <offboard_ex/RefSignal.h>
#include <offboard_ex/EulerAngles.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Vector3.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 <mavros_msgs/Thrust.h>
#include <mavros_msgs/VFR_HUD.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <iostream>
#include <cmath> // for std::abs

/********* CALLBACK FUNCTIONS **********************/
// Initiate variables
mavros_msgs::State current_state;

geometry_msgs::PoseStamped desPose;

tf2::Quaternion q_cmd /*What I send*/, q_Jae; // What should be sent;
double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = cmd
geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg
offboard_ex::EulerAngles eulAng;

mavros_msgs::AttitudeTarget thrust;

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

geometry_msgs::Quaternion q_des;
double alt_des; // Check desired height

// Cb to determine desired pose *Only needed for orientation 
void q_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    desPose = *msg;
		alt_des = desPose.pose.position.z;
    q_des   = desPose.pose.orientation;
    tf2::convert(q_des,q_cmd);	// Convert msg type to tf2::quaternion type
}


// 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;
		}
	}
}

// To determine throttle ... type
void thrust_cb(const mavros_msgs::AttitudeTarget msg){
	thrust = msg;	
	tf2::convert(thrust.orientation,q_Jae); // Convert from msg to quat
}


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 quaternions from klausen control
    ros::Subscriber q_sub = nh.subscribe<geometry_msgs::PoseStamped>
		("command/quaternions",10,q_cb);

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

    // Throttle: Jaeyoung-Lim
    ros::Subscriber thro_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
		("command/bodyrate_command",10,thrust_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 = nh.advertise<mavros_msgs::AttitudeTarget>
		("mavros/setpoint_raw/attitude",10);

    // Publish attitude cmds in euler angles
    ros::Publisher euler_pub = nh.advertise<offboard_ex::EulerAngles>
		("command/euler_angles",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");

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

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
	ROS_INFO("Waiting for FCU connection");
        rate.sleep();
    }
   
    /*********** Initiate variables ************************/
    // Needed for attitude command
    mavros_msgs::AttitudeTarget attitude;
    
    attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO 
    attitude.type_mask = 1|2|4; // Ignore body rates

		// Retrieve parameters from Ctrl_param.yaml file
		/*std::vector<double> waypoints;		
		nh.getParam("sim/waypoints",waypoints);		

    // Need to send pose commands until throttle has been established
    buff_pose.pose.position.x = waypoints(1);
    buff_pose.pose.position.y = waypoints(2);
    buff_pose.pose.position.z = waypoints(3);*/

    // 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();
        rate.sleep();
    }

    while(ros::ok()){
		if(gps_read == true){
		// Now need to convert publish q to euler ang
		tf2::Matrix3x3 m_K(q_cmd); tf2::Matrix3x3 m_J(q_Jae);
		m_K.getRPY(rol_K, pch_K, yaw_K);
		m_J.getRPY(rol_J, pch_J, yaw_J);

		// Populate msg
		K_ang.x = 180*rol_K/3.141;
		K_ang.y = 180*pch_K/3.141;
		K_ang.z = yaw_K;

		J_ang.x = rol_J;
		J_ang.y = pch_J;
		J_ang.z = yaw_J;

		eulAng.commanded = K_ang;
		eulAng.desired = J_ang;

		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){
		        ROS_INFO("Offboard enabled");
		    } 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(3.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;
			}
		}
		attitude.thrust = thrust.thrust;
		
		// Determine which messages to send
		if(ros::Time::now() - tkoff_req > ros::Duration(25.0) && takeoff){
			attitude.orientation = q_des;
			attitude.header.stamp = ros::Time::now();
			att_targ.publish(attitude);
			att_cmds = true;	
		} else {
			local_pos_pub.publish(buff_pose);
		}

		if(!att_cmds && ros::Time::now() - last_request > ros::Duration(2.0)){
			ROS_INFO("Thro: %.2f", attitude.thrust);
		} else {
			ROS_INFO("Thro: %.2f", attitude.thrust);
			ROS_INFO("Des Altitude: %.2f", alt_des);
			ROS_INFO("Cur Altitude: %.2f", current_altitude);
			//ROS_INFO("Sending points:\nRoll = %.2f\nPitch = %.2f",
			//	  180*rol_K/3.141,180*pch_K/3.141);
		}

		// Publish euler angs
		euler_pub.publish(eulAng);
		
		ros::spinOnce();
		rate.sleep();
	}
    }

    return 0;
}