/** @file telemetry_node.cpp
 *  @version 4.0
 *  @date May 2020
 *
 *  @brief sample node of telemetry.
 *
 *  @Copyright (c) 2020 DJI
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

//INCLUDE
#include <ros/ros.h>
#include <dji_osdk_ros/common_type.h>
#include <dji_osdk_ros/dji_vehicle_node.h>

//CODE
using namespace dji_osdk_ros;

sensor_msgs::BatteryState battery_state_;
geometry_msgs::QuaternionStamped attitude_data_;
sensor_msgs::Imu imu_data_;
std_msgs::UInt8 flight_status_;
std_msgs::UInt8 gps_health_;
sensor_msgs::NavSatFix gps_position_;
dji_osdk_ros::VOPosition vo_position_;
std_msgs::Float32 height_above_takeoff_;
geometry_msgs::Vector3Stamped velocity_;
dji_osdk_ros::MobileData from_mobile_data_;
dji_osdk_ros::PayloadData from_payload_data_;
geometry_msgs::Vector3Stamped gimbal_angle_data_;
sensor_msgs::Joy rc_data_;
geometry_msgs::PointStamped local_position_;
sensor_msgs::NavSatFix local_Frame_ref_;
nmea_msgs::Sentence time_sync_nmea_msg_;
dji_osdk_ros::GPSUTC time_sync_gps_utc_;
dji_osdk_ros::FCTimeInUTC time_sync_fc_utc_;
std_msgs::String time_sync_pps_source_;
geometry_msgs::Vector3Stamped angular_rate_;
geometry_msgs::Vector3Stamped acceleration_;
std_msgs::UInt8 display_mode_;
sensor_msgs::TimeReference trigger_;
std_msgs::UInt8 rc_connection_status_;
sensor_msgs::NavSatFix rtk_position_;
geometry_msgs::Vector3Stamped rtk_velocity_;
std_msgs::Int16 rtk_yaw_;
std_msgs::UInt8 rtk_position_info_;
std_msgs::UInt8 rtk_yaw_info_;
std_msgs::UInt8 rtk_connection_status_;
dji_osdk_ros::FlightAnomaly flight_anomaly_;

void attitudeSubCallback(const geometry_msgs::QuaternionStampedConstPtr& attitudeData)
{
  attitude_data_ = *attitudeData;
}

void batteryStateSubCallback(const sensor_msgs::BatteryState::ConstPtr& batteryState)
{
  battery_state_ = *batteryState;
}

void imuSubCallback(const sensor_msgs::Imu::ConstPtr& imuData)
{
  imu_data_ = *imuData;
}

void flightStatusSubCallback(const std_msgs::UInt8::ConstPtr& flightData)
{
  flight_status_ = *flightData;
}

void gpsHealthSubCallback(const std_msgs::UInt8::ConstPtr& gpsHealth)
{

  gps_health_ = *gpsHealth;
}

void gpsPositionSubCallback(const sensor_msgs::NavSatFix::ConstPtr& gpsPosition)
{
  gps_position_ = *gpsPosition;
}

void voPositionSubCallback(const dji_osdk_ros::VOPosition::ConstPtr& voPosition)
{
  vo_position_ = *voPosition;
}

void heightSubCallback(const std_msgs::Float32::ConstPtr& heightAboveTakeoff)
{
  height_above_takeoff_ = *heightAboveTakeoff;
}

void velocitySubCallback(const geometry_msgs::Vector3Stamped::ConstPtr& velocity)
{
  velocity_ = *velocity;
}

void fromMobileDataSubCallback(const dji_osdk_ros::MobileData::ConstPtr& fromMobileData)
{
  from_mobile_data_ = *fromMobileData;
}

void fromPayloadDataSubCallback(const dji_osdk_ros::PayloadData::ConstPtr& fromPayloadData)
{
  from_payload_data_ = *fromPayloadData;
}

void gimbalAngleSubCallback(const geometry_msgs::Vector3Stamped::ConstPtr& gimbalAngleData)
{
  gimbal_angle_data_ = *gimbalAngleData;
}

void rcDataCallback(const sensor_msgs::Joy::ConstPtr& rcData)
{
  rc_data_ = *rcData;
}

void localPositionSubCallback(const geometry_msgs::PointStamped::ConstPtr& localPosition)
{
  local_position_ = *localPosition;
}

void localFrameRefSubCallback(const sensor_msgs::NavSatFix::ConstPtr& localFrameRef)
{
  local_Frame_ref_ = *localFrameRef;
}

void timeSyncNmeaSubSCallback(const nmea_msgs::Sentence::ConstPtr& timeSyncNmeaMsg)
{
  time_sync_nmea_msg_ = *timeSyncNmeaMsg;
}

void timeSyncGpsUtcSubCallback(const dji_osdk_ros::GPSUTC::ConstPtr& timeSyncGpsUtc)
{
  time_sync_gps_utc_ = *timeSyncGpsUtc;
}

void timeSyncFcUtcSubCallback(const dji_osdk_ros::FCTimeInUTC::ConstPtr& timeSyncFcUtc)
{
  time_sync_fc_utc_ = *timeSyncFcUtc;
}

void timeSyncPpsSourceSubCallback(const std_msgs::String::ConstPtr& timeSyncPpsSource)
{
  time_sync_pps_source_ = *timeSyncPpsSource;
}

void angularRateSubSCallback(const geometry_msgs::Vector3Stamped::ConstPtr& angularRate)
{
  angular_rate_ = *angularRate;
}

void accelerationSubCallback(const geometry_msgs::Vector3Stamped::ConstPtr& acceleration)
{
  acceleration_ = *acceleration;
}

void displayModeSubCallback(const std_msgs::UInt8::ConstPtr& displayMode)
{
  display_mode_ = *displayMode;
}

void triggerSubCallback(const sensor_msgs::TimeReference::ConstPtr& trigger)
{
  trigger_ = *trigger;
}

void rcConnectionStatusSubCallback(const std_msgs::UInt8::ConstPtr& rcConnectionStatus)
{
  rc_connection_status_ = *rcConnectionStatus;
}

void rtkPositionSubCallback(const sensor_msgs::NavSatFix::ConstPtr& rtkPosition)
{
  rtk_position_ = *rtkPosition;
}

void rtkVelocitySubCallback(const geometry_msgs::Vector3Stamped::ConstPtr& rtkVelocity)
{
  rtk_velocity_ = *rtkVelocity;
}

void rtkYawSubCallback(const std_msgs::Int16::ConstPtr& rtkYaw)
{
  rtk_yaw_ = *rtkYaw;
}

void rtkPositionInfoSubCallback(const std_msgs::UInt8::ConstPtr& rtkPositionInfo)
{
  rtk_position_info_ = *rtkPositionInfo;
}

void rtkYawInfoSubCallback(const std_msgs::UInt8::ConstPtr& rtkYawInfo)
{
  rtk_yaw_info_ = *rtkYawInfo;
}

void rtkConnectionStatusSubCallback(const std_msgs::UInt8::ConstPtr& rtkConnectionStatus)
{
  rtk_connection_status_ = *rtkConnectionStatus;
}

void flightAnomalySubCallback(const dji_osdk_ros::FlightAnomaly::ConstPtr& flightAnomaly)
{
  flight_anomaly_ = *flightAnomaly;
}

ros::Subscriber batteryStateSub;
ros::Subscriber imuSub;
ros::Subscriber flightStatusSub;
ros::Subscriber gpsHealthSub;
ros::Subscriber gpsPositionSub;
ros::Subscriber heightSub;
ros::Subscriber localPositionSub;
ros::Subscriber velocitySub;
ros::Subscriber gimbalAngleSub;
ros::Subscriber rcDataSub;
ros::Subscriber attitudeSub;

ros::Subscriber fromMobileDataSub;
ros::Subscriber fromPayloadDataSub;

ros::Subscriber localFrameRefSub;
ros::Subscriber timeSyncNmeaSub;
ros::Subscriber timeSyncGpsUtcSub;
ros::Subscriber timeSyncFcUtcSub;
ros::Subscriber timeSyncPpsSourceSub;

ros::Subscriber voPositionSub;
ros::Subscriber angularRateSub;
ros::Subscriber accelerationSub;
ros::Subscriber displayModeSub;
ros::Subscriber triggerSub;

ros::Subscriber rcConnectionStatusSub;
ros::Subscriber rtkPositionSub;
ros::Subscriber rtkVelocitySub;
ros::Subscriber rtkYawSub;
ros::Subscriber rtkPositionInfoSub;
ros::Subscriber rtkYawInfoSub;
ros::Subscriber rtkConnectionStatusSub;
ros::Subscriber flightAnomalySub;

int main(int argc ,char** argv)
{
  ros::init(argc, argv, "telemetry_node");
  ros::NodeHandle nh;
  bool userSelectBroadcast = false;
  nh.getParam("vehicle_node/use_broadcast", userSelectBroadcast);

  batteryStateSub  = nh.subscribe("dji_osdk_ros/battery_state", 10, &batteryStateSubCallback);
  imuSub           = nh.subscribe("dji_osdk_ros/imu", 10, &imuSubCallback);
  flightStatusSub  = nh.subscribe("dji_osdk_ros/flight_status", 10, &flightStatusSubCallback);
  gpsHealthSub     = nh.subscribe("dji_osdk_ros/gps_health", 10, &gpsHealthSubCallback);
  gpsPositionSub   = nh.subscribe("dji_osdk_ros/gps_position", 10, &gpsPositionSubCallback);
  heightSub        = nh.subscribe("dji_osdk_ros/height_above_takeoff", 10, &heightSubCallback);
  localPositionSub = nh.subscribe("dji_osdk_ros/local_position", 10, &localPositionSubCallback);
  velocitySub      = nh.subscribe("dji_osdk_ros/velocity", 10, &velocitySubCallback);
  gimbalAngleSub   = nh.subscribe("dji_osdk_ros/gimbal_angle", 10, &gimbalAngleSubCallback);
  rcDataSub        = nh.subscribe("dji_osdk_ros/rc", 10, &rcDataCallback);

  fromMobileDataSub  = nh.subscribe("dji_osdk_ros/from_mobile_data", 10, &fromMobileDataSubCallback);
  fromPayloadDataSub = nh.subscribe("dji_osdk_ros/from_payload_data", 10, &fromPayloadDataSubCallback);

  /* only if you call the service of set_local_pos_reference ,the topic can be valid" */
  localFrameRefSub     = nh.subscribe("dji_osdk_ros/local_frame_ref", 10, &localFrameRefSubCallback);
  timeSyncNmeaSub      = nh.subscribe("dji_osdk_ros/time_sync_nmea_msg", 10, &timeSyncNmeaSubSCallback);
  timeSyncGpsUtcSub    = nh.subscribe("dji_osdk_ros/time_sync_gps_utc", 10, &timeSyncGpsUtcSubCallback);
  timeSyncFcUtcSub     = nh.subscribe("dji_osdk_ros/time_sync_fc_time_utc", 10, &timeSyncFcUtcSubCallback);
  timeSyncPpsSourceSub = nh.subscribe("dji_osdk_ros/time_sync_pps_source", 10, &timeSyncPpsSourceSubCallback);
  
  if (!userSelectBroadcast)
  {
    voPositionSub   = nh.subscribe("dji_osdk_ros/vo_position", 10, &voPositionSubCallback);
    angularRateSub  = nh.subscribe("dji_osdk_ros/angular_velocity_fused", 10, &angularRateSubSCallback);
    accelerationSub = nh.subscribe("dji_osdk_ros/acceleration_ground_fused", 10, &accelerationSubCallback);
    displayModeSub  = nh.subscribe("dji_osdk_ros/display_mode", 10, &displayModeSubCallback);
    triggerSub      = nh.subscribe("dji_osdk_ros/trigger_time", 10, &triggerSubCallback);

    rcConnectionStatusSub  = nh.subscribe("dji_osdk_ros/rc_connection_status", 10, &rcConnectionStatusSubCallback);
    rtkPositionSub         = nh.subscribe("dji_osdk_ros/rtk_position", 10, &rtkPositionSubCallback);
    rtkVelocitySub         = nh.subscribe("dji_osdk_ros/rtk_velocity", 10, &rtkVelocitySubCallback);
    rtkYawSub              = nh.subscribe("dji_osdk_ros/rtk_yaw", 10, &rtkYawSubCallback);
    rtkPositionInfoSub     = nh.subscribe("dji_osdk_ros/rtk_info_position", 10, &rtkPositionInfoSubCallback);
    rtkYawInfoSub          = nh.subscribe("dji_osdk_ros/rtk_info_yaw", 10, &rtkYawInfoSubCallback);
    rtkConnectionStatusSub = nh.subscribe("dji_osdk_ros/rtk_connection_status", 10, &rtkConnectionStatusSubCallback);
    flightAnomalySub       = nh.subscribe("dji_osdk_ros/flight_anomaly", 10, &flightAnomalySubCallback);
    attitudeSub      = nh.subscribe("dji_osdk_ros/attitude", 10, &attitudeSubCallback);
  }

  ros::Duration(1).sleep();
  ros::AsyncSpinner spinner(4);
  spinner.start();

  int elapsedTimeInS = 0;
  int timeToPrintInS = 30;

  while (elapsedTimeInS < timeToPrintInS)
  {
    std::cout << "Counter = " << elapsedTimeInS << ":\n";
    std::cout << "-------\n";

    ROS_INFO("battery Info :");
    ROS_INFO("battery's capacity: %f", battery_state_.capacity);
    ROS_INFO("battery's voltage: %f", battery_state_.voltage);
    ROS_INFO("battery's current: %f", battery_state_.current);
    ROS_INFO("battery's percentage : %f\n", battery_state_.percentage);

    ROS_INFO("height above takeoff:");
    ROS_INFO("height_above_takeoff_ :%f\n", height_above_takeoff_.data);
    ROS_INFO("gimbalAngleData:");
    ROS_INFO("gimbal_angle_data_(x,y,z) :%f, %f, %f\n", gimbal_angle_data_.vector.x, gimbal_angle_data_.vector.y,
              gimbal_angle_data_.vector.z);
    ROS_INFO("rcData(here only print basic data):");
    if (rc_data_.axes.size() >= 4)
    {
       ROS_INFO("rc_data_(roll,pitch,yaw, throttle) :%f, %f, %f, %f\n", rc_data_.axes[0], rc_data_.axes[1],
                 rc_data_.axes[2], rc_data_.axes[3]);
    }

    ROS_INFO("gps Position:");
    ROS_INFO("gps_position_(latitude, longitude, altitude) :%f, %f, %f \n",
              gps_position_.latitude, gps_position_.longitude, gps_position_.altitude);
    ROS_INFO("gps Health:");
    ROS_INFO("gps_health_ :%d \n", gps_health_.data);
    ROS_INFO("flight Info :");
    ROS_INFO("flight_status_: %d\n",flight_status_.data);
    ROS_INFO("localPosition:");
    ROS_INFO("local_position_(x,y,z) :%f, %f, %f\n", local_position_.point.x, local_position_.point.y,
              local_position_.point.z);
    ROS_INFO("imu Info :");
    ROS_INFO("imu w: %f",imu_data_.orientation.w);
    ROS_INFO("imu x: %f",imu_data_.orientation.x);
    ROS_INFO("imu y: %f",imu_data_.orientation.y);
    ROS_INFO("imu z: %f\n",imu_data_.orientation.z);
    ROS_INFO("imu linear acceleration x: %f", imu_data_.linear_acceleration.x);
    ROS_INFO("imu linear acceleration y: %f", imu_data_.linear_acceleration.y);
    ROS_INFO("imu linear acceleration z: %f", imu_data_.linear_acceleration.z);
    ROS_INFO("fromMobileData:");
    if (from_mobile_data_.data.size() > 1)
    {
      ROS_INFO("from_mobile_data_(0,1....) :%d, %d\n", from_mobile_data_.data[0], from_mobile_data_.data[1]);
    }

    ROS_INFO("fromPayloadData:");
    if (from_payload_data_.data.size() > 1)
    {
      ROS_INFO("from_payload_data_(0,1....) :%d, %d\n", from_payload_data_.data[0], from_payload_data_.data[1]);
    }
    auto local_frame_ref_client = nh.serviceClient<dji_osdk_ros::SetLocalPosRef>("set_local_pos_reference");
    dji_osdk_ros::SetLocalPosRef local_frame_ref;
    local_frame_ref_client.call(local_frame_ref);
    if (local_frame_ref.response.result)
    {
       ROS_INFO("localFrameRef:");
       ROS_INFO("local_Frame_ref_(latitude, longitude, altitude) :%f, %f, %f\n ",
       local_Frame_ref_.latitude, local_Frame_ref_.longitude, local_Frame_ref_.altitude);
    }

    ROS_INFO("timeSyncNmeaMsg:");
    ROS_INFO("time_sync_nmea_msg_ :%s \n",
              time_sync_nmea_msg_.sentence.data());
    ROS_INFO("timeSyncGpsUtc:");
    ROS_INFO("time_sync_gps_utc_(timestamp,UTCTimeData):%d, %s \n",
              time_sync_gps_utc_.stamp.sec, time_sync_gps_utc_.UTCTimeData.data());
    ROS_INFO("timeSyncFcUtc:");      
    ROS_INFO("time_sync_fc_utc_(fc_timestamp_us,fc_utc_yymmdd,fc_utc_hhmmss):%d, %d, %d\n",
              time_sync_fc_utc_.fc_timestamp_us, time_sync_fc_utc_.fc_utc_yymmdd,time_sync_fc_utc_.fc_utc_hhmmss);
    ROS_INFO("timeSyncPpsSource:");
    ROS_INFO("time_sync_pps_source_ :%s\n", time_sync_pps_source_.data.data());
    if (!userSelectBroadcast)
    {
      //5HZ
        ROS_INFO("if RTK is available:");
        ROS_INFO("rtkPosition:");
        ROS_INFO("rtk_position_(latitude, longitude, altitude) :%f, %f, %f \n",
                rtk_position_.latitude, rtk_position_.longitude, rtk_position_.altitude);
        ROS_INFO("rtkVelocity:");
        ROS_INFO("rtk_velocity_(x,y,z) :%f, %f, %f\n", rtk_velocity_.vector.x,
                rtk_velocity_.vector.y, rtk_velocity_.vector.z);
        ROS_INFO("rtkYaw:");
        ROS_INFO("rtk_yaw_: %d\n", rtk_yaw_.data);
        ROS_INFO("rtkPositionInfo:");
        ROS_INFO("rtk_position_info_: %d\n", rtk_position_info_.data);
        ROS_INFO("rtkYawInfo:");
        ROS_INFO("rtk_yaw_info_: %d\n", rtk_yaw_info_.data);
        ROS_INFO("rtkConnectionStatus:");
        ROS_INFO("rtk_connection_status_: %d\n", rtk_connection_status_.data);
      //50HZ
        ROS_INFO("velocity:");
        ROS_INFO("velocity(x,y,z) :%f, %f, %f\n", velocity_.vector.x, velocity_.vector.y,
                  velocity_.vector.z);
        ROS_INFO("vo Position:");
        ROS_INFO("vo_position_(x, y, z ,xHealth, yHealth, zHealth:%f, %f, %f, %d, %d, %d\n ",
                  vo_position_.x, vo_position_.y, vo_position_.z,
                  vo_position_.xHealth, vo_position_.yHealth, vo_position_.zHealth);
        ROS_INFO("displayMode:");
        ROS_INFO("display_mode_: %d\n", display_mode_.data);
        ROS_INFO("rcConnectionStatus:");
        ROS_INFO("rc_connection_status_: %d\n", rc_connection_status_.data);
        ROS_INFO("flightAnomaly:");
        if (flight_anomaly_.data && dji_osdk_ros::FlightAnomaly::COMPASS_INSTALLATION_ERROR)
        {
           ROS_INFO("COMPASS_INSTALLATION_ERROR\n");
        }
        if (flight_anomaly_.data && dji_osdk_ros::FlightAnomaly::IMU_INSTALLATION_ERROR)
        {
           ROS_INFO("IMU_INSTALLATION_ERROR\n");
        }
      //100HZ
        ROS_INFO("attitude Info :");
        ROS_INFO("attitude w: %f",attitude_data_.quaternion.w);
        ROS_INFO("attitude x: %f",attitude_data_.quaternion.x);
        ROS_INFO("attitude y: %f",attitude_data_.quaternion.y);
        ROS_INFO("attitude z: %f\n",attitude_data_.quaternion.z);
        ROS_INFO("angularRate:");
        ROS_INFO("angular_rate_(x,y,z) :%f, %f, %f\n", angular_rate_.vector.x,
                  angular_rate_.vector.y, angular_rate_.vector.z);
        ROS_INFO("acceleration:");
        ROS_INFO("acceleration_(x,y,z) :%f, %f, %f\n", acceleration_.vector.x,
                   acceleration_.vector.y, acceleration_.vector.z);
      //400HZ
        ROS_INFO("trigger:");
        ROS_INFO("trigger_fc_time(if you choose align_time): %d", trigger_.header.stamp.nsec);
        ROS_INFO("trigger_ros_time: %d", trigger_.time_ref.nsec);
        ROS_INFO("trigger_time_source: %s", trigger_.source.c_str());
    }
    std::cout << "-------\n\n";
    ros::Duration(1).sleep();
    elapsedTimeInS += 1;
  }

  std::cout << "Done printing!\n";
  
  ROS_INFO_STREAM("Finished. Press CTRL-C to terminate the node");
  ros::waitForShutdown();

  return 0;
}