/* 
 * Copyright (C) 2021, Yubao Liu, AISL, TOYOHASHI UNIVERSITY of TECHNOLOGY 
 * Email: yubao.liu.ra@tut.jp 
 * 
 */
#ifndef _SEMANTIC_H_
#define _SEMANTIC_H_

#include "Common.h"
// ORB SLAM
#include "Atlas.h"
#include "KeyFrame.h"
#include "Tracking.h"

#include <actionlib/client/simple_action_client.h>
#include "maskrcnn_ros/maskrcnn_client.h"
#include "segnet_ros/segnet_client.h"
#include "yolov8_client.h"
//myx :
#include <set>
#include <utility>
#include <opencv2/opencv.hpp>

using namespace maskrcnn_ros;
using namespace cv;
using namespace std;

namespace ORB_SLAM3 {

class maskrcnn_wrapper;

class Semantic {
public:
    //myx :增加深度图对mask的处理
    void computeDepthStatistics(const Mat& depthImage, const Mat& mask, double& mean, double& stddev, ushort& minVal, ushort& maxVal);
    Mat processDepthAndMask(Mat& depthImage, Mat& mask, int index);
    //myx :查询语义关键帧前一个关键帧，提供对极几何函数的输入 
    KeyFrame* GetPreviousKeyFrame(const std::list<KeyFrame*>& mlNewKeyFrames, int currentID);
    
    static Semantic* GetInstance();
    void IsEnableSemantic(bool in_isEnable);
    void SetTracker(Tracking* pTracker);
    void SetAtlas(Atlas* pAtlas);
    void SetSemanticMethod(const std::string& cnn_method);

    // threads related
    void Run();
    void SemanticTrackingThread();
    void SemanticSegmentationThread();
    void SemanticBAThread();
    void RequestFinish();

    // keyframe queue management
    void InsertKeyFrame(KeyFrame* pKF);
    void InsertKeyFrameAll(KeyFrame* pKF); //myx mlNewKeyFrames_all
    void InsertSemanticRequest(KeyFrame* pKF);
    size_t GetLatestSemanticKeyFrame();

    // Check the current map point whether is dynamic
    bool IsDynamicMapPoint(const MapPoint* pMP);
    static void getBinMask(const cv::Mat& comMask, cv::Mat& binMask);

    // Evaluation
    std::vector<float> mvTimeUpdateMovingProbability;
    std::vector<float> mvTimeMaskGeneration;
    std::vector<float> mvTimeSemanticOptimization;
    // To evaluate the time delay between sequential model and bi-direction model
    std::vector<size_t> mvSemanticDelay;

    // void FinalStage();
    ~Semantic();
private:
    // latest keyframe that has semantic label
    size_t mnLatestSemanticKeyFrameID;
    // Total number of keyframes that has semantic label
    size_t mnTotalSemanticFrameNum;

    int mBatchSize;
    Tracking* mpTracker;
    Map* mpMap;
    Atlas* mpAtlas;

    std::string msCnnMethod;

    // Morphological filter
    int mDilation_size;
    cv::Mat mKernel;
    int mErode_size;
    cv::Mat mKernel2;

    // disable or enable semantic moving probability
    bool mbIsUseSemantic;

    // segement the first few frames
    int mnSegmentFrameNum;

    // threshold of moving probability for map point
    float mthDynamicThreshold;

    // wait semantic result
    std::mutex mMutexResult;
    std::condition_variable mcvWaitResult;

    // priori dynamic objects
    std::map<std::string, int> mmDynamicObjects;

    // New semantic request
    bool CheckNewSemanticRequest();
    // inset keyframe
    bool CheckNewKeyFrames();
    bool CheckSemanticBARequest();

    bool CheckNewSemanticTrackRequest();
    void AddSemanticTrackRequest(KeyFrame* pKF);
    void AddSemanticBARequest(KeyFrame* pKF);
    void GenerateMask(KeyFrame* pKF, const bool isDilate = false ,const bool isErode = true);

    std::mutex mMutexNewSemanticRequest;
    std::list<KeyFrame*> mlNewSemanticRequest;

    KeyFrame* mpCurrentKeyFrame;
    std::list<KeyFrame*> mlNewKeyFrames; //删除已有语义信息的关键帧
    std::list<KeyFrame*> mlNewKeyFrames_all; //所有关键帧

    std::mutex mMutexSemanticTrack;
    std::list<KeyFrame*> mlSemanticTrack;
    std::list<KeyFrame*> mlSemanticNew;
    std::list<KeyFrame*> mlSemanticBA;

    std::mutex mMutexNewKFs;
    std::mutex mMutexSemanticBA;

    bool IsInImage(const float& x, const float& y, const cv::Mat& img);

    // Finish Request
    bool CheckFinish();
    // 当前线程的主函数是否已经终止
    bool mbFinishRequested;
    // 和"线程真正结束"有关的互斥锁
    std::mutex mMutexFinish;

    // MaskRCNN
    maskrcnn_ros::MaskRCNN_Client_Batch::Ptr mMaskRCNN;
    segnet_ros::SegNetClient::Ptr mSegNet;
    maskrcnn_ros::YoloV8_Client_Batch::Ptr mYoloV8;

    // semantic thread
    std::thread* mptSemanticSegmentation;
    std::thread* mptSemanticBA;
    // semantic tracking thread
    std::thread* mptSemanticTracking;

    static Semantic* mInstance;
    Semantic();
};
}
#endif