/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef MAPPOINT_H
#define MAPPOINT_H

#include "Frame.h"
#include "KeyFrame.h"
#include "Map.h"

#include <mutex>
#include <opencv2/core/core.hpp>

#include <boost/serialization/array.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/serialization.hpp>

namespace ORB_SLAM3 {

class KeyFrame;
class Map;
class Frame;

class MapPoint {
    template <class Archive>
    void serializeMatrix(Archive& ar, cv::Mat& mat, const unsigned int version)
    {
        int cols, rows, type;
        bool continuous;

        if (Archive::is_saving::value) {
            cols = mat.cols;
            rows = mat.rows;
            type = mat.type();
            continuous = mat.isContinuous();
        }

        ar& cols& rows& type& continuous;
        if (Archive::is_loading::value)
            mat.create(rows, cols, type);

        if (continuous) {
            const unsigned int data_size = rows * cols * mat.elemSize();
            ar& boost::serialization::make_array(mat.ptr(), data_size);
        } else {
            const unsigned int row_size = cols * mat.elemSize();
            for (int i = 0; i < rows; i++) {
                ar& boost::serialization::make_array(mat.ptr(i), row_size);
            }
        }
    }

    friend class boost::serialization::access;
    template <class Archive>
    void serialize(Archive& ar, const unsigned int version)
    {
        ar& mnId;
        ar& mnFirstKFid;
        ar& mnFirstFrame;
        ar& nObs;
        // Variables used by the tracking
        ar& mTrackProjX;
        ar& mTrackProjY;
        ar& mTrackDepth;
        ar& mTrackDepthR;
        ar& mTrackProjXR;
        ar& mTrackProjYR;
        ar& mbTrackInView;
        ar& mbTrackInViewR;
        ar& mnTrackScaleLevel;
        ar& mnTrackScaleLevelR;
        ar& mTrackViewCos;
        ar& mTrackViewCosR;
        ar& mnTrackReferenceForFrame;
        ar& mnLastFrameSeen;

        // Variables used by local mapping
        ar& mnBALocalForKF;
        ar& mnFuseCandidateForKF;

        // Variables used by loop closing and merging
        ar& mnLoopPointForKF;
        ar& mnCorrectedByKF;
        ar& mnCorrectedReference;
        serializeMatrix(ar, mPosGBA, version);
        ar& mnBAGlobalForKF;
        ar& mnBALocalForMerge;
        serializeMatrix(ar, mPosMerge, version);
        serializeMatrix(ar, mNormalVectorMerge, version);

        // Protected variables
        serializeMatrix(ar, mWorldPos, version);
        //ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId);
        ar& mBackupObservationsId1;
        ar& mBackupObservationsId2;
        serializeMatrix(ar, mNormalVector, version);
        serializeMatrix(ar, mDescriptor, version);
        ar& mBackupRefKFId;
        ar& mnVisible;
        ar& mnFound;

        ar& mbBad;
        ar& mBackupReplacedId;

        ar& mfMinDistance;
        ar& mfMaxDistance;
    }

public:
    // Semantic
    bool IsDynamicMapPoint();
    void SetMovingProbability(const float& in_mp);
    float GetMovingProbability();

    MapPoint();
    // construct map use Keframe and pos
    MapPoint(const cv::Mat& Pos, KeyFrame* pRefKF, Map* pMap);
    MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF, KeyFrame* pHostKF, Map* pMap);
    // construct map use Frame and pos
    MapPoint(const cv::Mat& Pos, Map* pMap, Frame* pFrame, const int& idxF);

    void SetWorldPos(const cv::Mat& Pos);
    cv::Mat GetWorldPos();

    cv::Mat GetNormal();
    KeyFrame* GetReferenceKeyFrame();

    std::map<KeyFrame*, std::tuple<int, int>> GetObservations();
    int Observations();

    void AddObservation(KeyFrame* pKF, int idx);
    void EraseObservation(KeyFrame* pKF);

    std::tuple<int, int> GetIndexInKeyFrame(KeyFrame* pKF);
    bool IsInKeyFrame(KeyFrame* pKF);

    void SetBadFlag();
    bool isBad();

    void Replace(MapPoint* pMP);
    MapPoint* GetReplaced();

    void IncreaseVisible(int n = 1);
    void IncreaseFound(int n = 1);
    float GetFoundRatio();
    inline int GetFound()
    {
        return mnFound;
    }

    void ComputeDistinctiveDescriptors();

    cv::Mat GetDescriptor();

    void UpdateNormalAndDepth();
    void SetNormalVector(cv::Mat& normal);

    float GetMinDistanceInvariance();
    float GetMaxDistanceInvariance();
    int PredictScale(const float& currentDist, KeyFrame* pKF);
    int PredictScale(const float& currentDist, Frame* pF);

    Map* GetMap();
    void UpdateMap(Map* pMap);

    void PrintObservations();

    void PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP);
    void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid);

public:
    // TODO [semantic]  moving detection
    float mMovingProbability = 0.5; //myx :将地图点的初始概率设置为0.5
    float mStaticProbability;

    int mnObservedStatic;
    int mnObservedDynamic;

    long unsigned int mnId;
    static long unsigned int nNextId;
    long int mnFirstKFid;
    long int mnFirstFrame;
    int nObs;

    // Variables used by the tracking
    float mTrackProjX;
    float mTrackProjY;
    float mTrackDepth;
    float mTrackDepthR;
    float mTrackProjXR;
    float mTrackProjYR;
    bool mbTrackInView, mbTrackInViewR;
    int mnTrackScaleLevel, mnTrackScaleLevelR;
    float mTrackViewCos, mTrackViewCosR;
    long unsigned int mnTrackReferenceForFrame;
    long unsigned int mnLastFrameSeen;

    // Variables used by local mapping
    long unsigned int mnBALocalForKF;

    // TODO Semantic
    long unsigned int mnBASemanticForKF;
    long unsigned int mnFuseCandidateForKF;
    long unsigned int mnSemanticTrackReferenceForFrame;

    // Variables used by loop closing
    long unsigned int mnLoopPointForKF;
    long unsigned int mnCorrectedByKF;
    long unsigned int mnCorrectedReference;
    cv::Mat mPosGBA;
    long unsigned int mnBAGlobalForKF;
    long unsigned int mnBALocalForMerge;

    // Variable used by merging
    cv::Mat mPosMerge;
    cv::Mat mNormalVectorMerge;

    // Fopr inverse depth optimization
    double mInvDepth;
    double mInitU;
    double mInitV;
    KeyFrame* mpHostKF;

    static std::mutex mGlobalMutex;

    unsigned int mnOriginMapId;

protected:
    // Position in absolute coordinates
    cv::Mat mWorldPos;

    // Keyframes observing the point and associated index in keyframe
    std::map<KeyFrame*, std::tuple<int, int>> mObservations;
    // For save relation without pointer, this is necessary for save/load function
    std::map<long unsigned int, int> mBackupObservationsId1;
    std::map<long unsigned int, int> mBackupObservationsId2;

    // Mean viewing direction
    cv::Mat mNormalVector;

    // Best descriptor to fast matching
    cv::Mat mDescriptor;

    // Reference KeyFrame
    KeyFrame* mpRefKF;
    long unsigned int mBackupRefKFId;

    // Tracking counters
    int mnVisible;
    int mnFound;

    // Bad flag (we do not currently erase MapPoint from memory)
    bool mbBad;
    MapPoint* mpReplaced;
    // For save relation without pointer, this is necessary for save/load function
    long long int mBackupReplacedId;

    // Scale invariance distances
    float mfMinDistance;
    float mfMaxDistance;

    Map* mpMap;

    std::mutex mMutexPos;
    std::mutex mMutexFeatures;
    std::mutex mMutexMap;
};

} //namespace ORB_SLAM

#endif // MAPPOINT_H