#include <ros/ros.h>
#include <string>
#include <thread>
#include <std_msgs/String.h>
#include <std_srvs/SetBool.h>
#include "gimbal.h"
#include "ViewLink.h"
#include "amov_gimbal.h"
#include "prometheus_gimbal_control/GimbalControl.h"
#include "prometheus_gimbal_control/GimbalState.h"
#include "prometheus_gimbal_control/VisionDiff.h"
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// amovGimbal::gimbal *gimbal = nullptr;
GimbalBase *g_gimbal = nullptr;

prometheus_gimbal_control::GimbalState g_pub_info;
bool g_stop_track = false;

void ctlCallback(const prometheus_gimbal_control::GimbalControl::ConstPtr &ctl)
{
    switch (ctl->rpyMode)
    {
    case prometheus_gimbal_control::GimbalControl::manual:
        uint8_t rpy_ctl[3];
        double rpy_vel[3];
        rpy_ctl[0] = ctl->roll;
        rpy_ctl[1] = ctl->yaw;
        rpy_ctl[2] = ctl->pitch;
        rpy_vel[0] = ctl->rValue;
        rpy_vel[1] = ctl->pValue;
        rpy_vel[2] = ctl->yValue;



        g_gimbal->ctl_gimbal(rpy_ctl, rpy_vel);
        

        ROS_INFO_STREAM("Manual "
                        << "rpy control mode: " << (int)rpy_ctl[0] << " " << (int)rpy_ctl[1] << " " << (int)rpy_ctl[2]
                        << " value: " << rpy_vel[0] << " " << rpy_vel[1] << " " << rpy_vel[2]);

        break;
    case prometheus_gimbal_control::GimbalControl::home:
        g_gimbal->home();
        ROS_INFO_STREAM("Back Home");
        break;
    case prometheus_gimbal_control::GimbalControl::hold:
        g_gimbal->hold();
        ROS_INFO_STREAM("Hold");
        break;
    case prometheus_gimbal_control::GimbalControl::fellow:
        g_gimbal->fellow();
        ROS_INFO_STREAM("Fellow");
        break;
    }
    switch (ctl->focusMode)
    {
    case prometheus_gimbal_control::GimbalControl::focusIn:
        g_gimbal->focus_in();
        ROS_INFO_STREAM("Focus In");
        break;
    case prometheus_gimbal_control::GimbalControl::focusOut:
        g_gimbal->focus_out();
        ROS_INFO_STREAM("Focus Out");
        break;
    case prometheus_gimbal_control::GimbalControl::focusStop:
        g_gimbal->focus_stop();
        ROS_INFO_STREAM("Focus Stop");
        break;
    }
    switch (ctl->zoomMode)
    {
    case prometheus_gimbal_control::GimbalControl::zoomIn:
        g_gimbal->zoom_in();
        ROS_INFO_STREAM("Zoom In");
        break;

    case prometheus_gimbal_control::GimbalControl::zoomOut:
        g_gimbal->zoom_out();
        ROS_INFO_STREAM("Zoom Out");
        break;
    case prometheus_gimbal_control::GimbalControl::zoomStop:
        g_gimbal->zoom_stop();
        ROS_INFO_STREAM("Zoom Stop");
        break;
    }
};

void trackCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &cent)
{
    if (g_stop_track)
        return;
    float tmp_cent[2]{0, 0};
    tmp_cent[0] = cent->objectX + cent->objectWidth / 2 - cent->frameWidth / 2;
    tmp_cent[1] = cent->objectY + cent->objectHeight / 2 - cent->frameHeight / 2;
    int roll_mode = cent->ctlMode;
    float kp = cent->kp;
    float ki = cent->ki;
    float kd = cent->kd;

    static bool flag_zoom = false;
    static bool flag_hold = false;

    if (cent->detect != 0)
    {
        int zoom_val = cent->currSize ? std::fmax(g_gimbal->zoom_val, 1) : 1;
        float scale = std::fmax(g_gimbal->max_zoom - zoom_val, 1.) / g_gimbal->max_zoom;
        g_gimbal->tracking(tmp_cent, roll_mode, kp * scale, ki, kd, cent->trackIgnoreError * zoom_val / g_gimbal->max_zoom);
        // std::cout << "step 3 : " << std::endl;
        if (cent->autoZoom && cent->currSize && tmp_cent[0] < cent->trackIgnoreError && tmp_cent[1] < cent->trackIgnoreError)
        {
            g_gimbal->auto_zoom(cent->currSize, zoom_val, cent->maxSize, cent->minSize);
            flag_zoom = true;
        }
        // std::cout << "step 4 : " << std::endl;
        flag_hold = true;
    }
    else
    {
        if (cent->currSize && flag_zoom)
        {
            g_gimbal->zoom_stop();
            flag_zoom = false;
        }
        if (flag_hold)
        {
            g_gimbal->hold();
            flag_hold = false;
        }
    }
};

void currSizeCallback(const prometheus_gimbal_control::VisionDiff::ConstPtr &info)
{
    static bool flag = false;
    int now_flag = 0;
    int zoom_val = std::fmax(g_gimbal->zoom_val, 1);
    if (info->detect != 0 && (std::abs(g_pub_info.imuAngleVel[0]) + std::abs(g_pub_info.imuAngleVel[1]) + std::abs(g_pub_info.imuAngleVel[2])) < 3)
    {
        g_gimbal->auto_zoom(info->currSize, zoom_val, info->maxSize, info->minSize);
        flag = true;
    }
    else
    {
        if (flag)
        {
            g_gimbal->zoom_stop();
            flag = false;
        }
    }
}

bool videoRecord(std_srvs::SetBool::Request &ref, std_srvs::SetBool::Response &req)
{
    if (ref.data)
    {
        g_gimbal->start_record();
        req.success = g_gimbal->outvideo.isOpened();
        ROS_INFO_STREAM("Start Video Record ! ");
    }
    else
    {
        if (!g_gimbal->outvideo.isOpened())
        {
            req.success = false;
            req.message = "video record not is started ! ";
            ROS_WARN_STREAM("video record not is started ! ");
        }
        g_gimbal->finish_record();
        if (g_gimbal->outvideo.isOpened())
        {
            req.success = false;
            req.message = "finish video fail ! ";
            ROS_WARN_STREAM("finish video fail ! ");
        }
        else
        {
            req.success = true;
            ROS_INFO_STREAM("Finish Video Record ! ");
        }
    }
    return true;
}

void picCallback(const std_msgs::String::ConstPtr &info)
{
    ROS_INFO("Get Picture ");
    g_gimbal->get_picture();
}

void callAll(image_transport::Publisher *image_pub)
{
    cv::Mat frame;
    while (true)
    {
        (*g_gimbal) >> frame;
        if (!frame.empty())
        {
            image_pub->publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg());
        }
        std::this_thread::sleep_for(10ms);
    }
}

bool search(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
{
    resp.success = g_gimbal->search(req.data);
    return true;
}

// 切换反馈模式，自动？交换
bool switchTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
{
    static_cast<GimbalQ10f *>(g_gimbal)->isGetRealVelocity = req.data;
    resp.success = true;
    resp.message = static_cast<GimbalQ10f *>(g_gimbal)->isGetRealVelocity ? "return real angle speed" : "return compute angle speed";
    return true;
}

// 屏蔽tracking控制
bool stopTrack(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
{
    g_stop_track = req.data;
    resp.success = true;
    return true;
}

int VLK_DevStatusCallback(int iType, const char* szBuffer, int iBufLen, void* pUserParam)
{
    if (VLK_DEV_STATUS_TYPE_MODEL == iType)
    {
        VLK_DEV_MODEL* pModel = (VLK_DEV_MODEL*)szBuffer;
        std::cout << "model code: " << pModel->cModelCode << ", model name: " << pModel->szModelName << std::endl;
    }
    else if (VLK_DEV_STATUS_TYPE_CONFIG == iType)
    {
        VLK_DEV_CONFIG* pDevConfig = (VLK_DEV_CONFIG*)szBuffer;
        std::cout << "VersionNO: " << pDevConfig->cVersionNO << ", DeviceID: " << pDevConfig->cDeviceID << ", SerialNO: " << pDevConfig->cSerialNO << std::endl;
    }
    else if (VLK_DEV_STATUS_TYPE_TELEMETRY == iType)
    {
        /*
         * once device is connected, telemetry information will keep updating,
         * in order to avoid disturbing user input, comment out printing telemetry information
         */
        // VLK_DEV_TELEMETRY* pTelemetry = (VLK_DEV_TELEMETRY*)szBuffer;
        // cout << "Yaw: " << pTelemetry->dYaw << ", Pitch: " << pTelemetry->dPitch << ", sensor type: " << pTelemetry->emSensorType << ", Zoom mag times: " << pTelemetry->sZoomMagTimes << endl;
    }
    else
    {
        std::cout << "error: unknown status type: " << iType << std::endl;
    }


    return 0;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "gimbal");
    ros::NodeHandle n("~");
    image_transport::ImageTransport it(n);
    image_transport::Publisher image_pub;
    cv::Mat frame;
    sensor_msgs::ImagePtr img;
    // rpy控制, 变焦, 放大
    image_pub = it.advertise("image_raw", 1);
    ros::Subscriber sub_ctl = n.subscribe<prometheus_gimbal_control::GimbalControl>("control", 10, ctlCallback);
    ros::Subscriber sub_track = n.subscribe<prometheus_gimbal_control::VisionDiff>("track", 10, trackCallback);
    ros::Subscriber save_pic = n.subscribe<std_msgs::String>("get_pic", 100, picCallback);
    ros::Publisher pub_rpy = n.advertise<prometheus_gimbal_control::GimbalState>("state", 10);
    ros::ServiceServer srv_search = n.advertiseService("search", search);
    ros::ServiceServer srv_video = n.advertiseService("record_video", videoRecord);
    ros::ServiceServer srv_track = n.advertiseService("stop_track", stopTrack);

    ros::Rate rate(20);

    int camera_width, camera_height, uav_id;
    std::string camera_id;
    std::string tty_url, save_path, gimbal_type;
    n.param<std::string>("camera_id", camera_id, "0");
    n.param<int>("uav_id", uav_id, 0); // 无人机的ID
    n.param<std::string>("tty_url", tty_url, "/dev/ttyUSB0");
    n.param<int>("camera_width", camera_width, 1920);
    n.param<int>("camera_height", camera_height, 1080);
    n.param<std::string>("save_path", save_path, "gimbal_video_data");
    n.param<std::string>("gimbal_type", gimbal_type, "at10");

    if (gimbal_type == "q10f")
    {
        g_gimbal = new GimbalQ10f(save_path);
    }
    else if (gimbal_type == "g1")
    {
        g_gimbal = new AmovG1(save_path);
    }
    else if (gimbal_type == "at10")
    {
        g_gimbal = new GimbalAT10(save_path);
    }
    else
    {
        ROS_ERROR_STREAM("NOT SUPPORT " << gimbal_type);
        return -1;
    }

    try
    {
        g_gimbal->open_tty(tty_url);
    }
    catch (...)
    {
        ROS_ERROR_STREAM("Unable to open Serial Port ! ");
        getchar();
        return -1;
    }

    try
    {
        // 以60帧率打开，仅对AMOV G1有效
        g_gimbal->open_camera(camera_id, camera_width, camera_height, 60);
    }
    catch (...)
    {
        ROS_ERROR_STREAM("Unable to open Camera ! ");
        getchar();
        return -1;
    }

    if (g_gimbal->device_name == "q10f" || g_gimbal->device_name == "at10")
    {
        static ros::ServiceServer srv_track_mode = n.advertiseService("track_mode", switchTrack);
        static ros::Subscriber sub_currSize = n.subscribe<prometheus_gimbal_control::VisionDiff>("currSize", 10, currSizeCallback);
        static_cast<GimbalQ10f *>(g_gimbal)->stopAuto();
        static_cast<GimbalAT10 *>(g_gimbal)->stopAuto();
        // initialize sdk
        int iRet = VLK_Init();
        if (VLK_ERROR_NO_ERROR != iRet)
        {
        std::cout << "VLK_Init failed, error: " << iRet << std::endl;
        return -1;
        }
        // register device status callback
        VLK_RegisterDevStatusCB(VLK_DevStatusCallback, NULL);
        // ROS_INFO_STREAM("AT10 !");
        // 5秒后停止 zoom_out()
        static ros::Timer stop_zoom = n.createTimer(
            ros::Duration(5.0), [&](const ros::TimerEvent &)
            { g_gimbal->zoom_out(); },
            true);
    }

    g_gimbal->home();
    g_gimbal->zoom_out();
    ROS_INFO_STREAM("Program Started ! ");
    std::thread spin(callAll, &image_pub);
    if(g_gimbal->device_name == "AT10")
    {
        ROS_INFO_STREAM("at10 !");
    }else if(g_gimbal->device_name == "q10f")
    {
        ROS_INFO_STREAM("q10f !");
    }else if(g_gimbal->device_name == "g1"){
        ROS_INFO_STREAM("g1 !");
        // print sdk version
        std::cout << "ViewLink SDK version: " << GetSDKVersion() << std::endl;
    }else {
        ROS_INFO_STREAM("unknow device!!!");
        std::cout << "device name: " << g_gimbal->device_name << std::endl;
        std::cout << "device type: " << gimbal_type << std::endl;
    }


    g_pub_info.Id = uav_id;
    while (ros::ok())
    {
        ros::spinOnce();
        float info[12];
        g_gimbal->get_info(info);
        g_pub_info.mode = g_gimbal->move_state;
        g_pub_info.feedbackMode = g_gimbal->feedback_state;
        g_pub_info.isRecording = g_gimbal->is_recording;
        g_pub_info.zoomState = g_gimbal->zoom_state;
        g_pub_info.zoomVal = g_gimbal->fzoom_val;
        g_pub_info.imuAngle[0] = info[0];
        g_pub_info.imuAngle[1] = info[1];
        g_pub_info.imuAngle[2] = info[2];
        g_pub_info.rotorAngleTarget[0] = info[3];
        g_pub_info.rotorAngleTarget[1] = info[4];
        g_pub_info.rotorAngleTarget[2] = info[5];
        g_pub_info.rotorAngle[0] = info[6];
        g_pub_info.rotorAngle[1] = info[7];
        g_pub_info.rotorAngle[2] = info[8];
        g_pub_info.imuAngleVel[0] = info[9];
        g_pub_info.imuAngleVel[1] = info[10];
        g_pub_info.imuAngleVel[2] = info[11];
        pub_rpy.publish(g_pub_info);
        rate.sleep();
        // std::cout << "run ........" << std::endl;
    }
    delete g_gimbal;
}