#include "opencv2/highgui.hpp"
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/stitching.hpp"
#include <fstream>
#include <iostream>
#include <json/json.h>
#include <cmath> 
#include <fstream>
#include <algorithm>
#include <map>
#include <typeinfo>
using namespace cv;
using namespace std;
#include <filesystem>
namespace fs = std::filesystem;

//C++17
//sudo apt install libjsoncpp-dev
//sudo ln -s /usr/include/jsoncpp/json/ /usr/include/json



//struct necessary for comparison between 2 opencv points - when using std::map
//used to sort points according to X value
struct ComparePoints
{
    bool operator () (const cv::Point& a, const cv::Point& b) const
    { 
        return (a.x < b.x) || (a.x == b.x && a.y < b.y);
    }
} CP;



class PointBase {
    public:
        vector <Point2d> points; //all points - points are being removed as adjacencies are found
        vector <Point2d> doneVector; //points that have been successfully searched for adjacencies 
        vector <Point2d> toDoVector; //points yet to be searched for adjacent points
        std::map<Point2d, string, ComparePoints> pathMap; //map for file paths - get the path of a file via pathMap[Point2d(x,y)]



    //loaded points in pointbase are sorted according to X value
    int load(string path)
    {
        //std::string path = "../data/";
        for (const auto & entry : fs::directory_iterator(path))
        {
            string filePath = fs::canonical(entry.path()); // absolute path without .. 
            //cout << filePath.find(".json") << std::endl;
            if (filePath.find(".json") != string::npos) 
            {
                //std::cout << filePath << std::endl;
                std::ifstream coord_file(filePath, std::ifstream::binary);
                Json::Value coords;

                coord_file >> coords;
                //cout << "x: " << coords["depthPos"]["x"].asDouble() << std::endl;
                //cout << "y: " << coords["depthPos"]["y"].asDouble() << std::endl;
                Point2d newPoint = Point2d(coords["depthPos"]["x"].asDouble() , coords["depthPos"]["y"].asDouble());
                pathMap[newPoint] = filePath;
                points.push_back(newPoint);
            }

        }

        if (points.empty()) {
            cout << "No points detected, check path \n";
            return -1;
        }
        else 
        {
            sort(points.begin(),points.end(),CP); 
            return 0;
        }
    }







    void printPoints(int which=0)
    {
        if (which==1) // prints current points vector
        {
            for(int i =0; i<points.size(); i++)
            {
                cout << "point " << i <<'\n';
                cout << "x: " << points[i].x << '\n';
                cout << "y: " << points[i].y << '\n';
            }
        }
        
        if (which==0) // prints Contents of pointBase
        {
            for (const auto& [point, path] : pathMap) 
            {
                std::cout << "path: " << path<<   "; \n";
                std::cout << "x: " << point.x  << "; \n";
                std::cout << "y: " << point.y  << "; \n";
            }
        }
        

    }







    string getPath(double x, double y)
    {
        string path = pathMap[Point2d(x,y)];
        return(path);
    }









    //shows the location of loaded points on an images
    //useful for debugging
    void showPointImg()
    {
        Mat img(1200, 360, CV_8UC3,   Scalar(0, 0, 0));
        for(int i =0; i<points.size(); i++)
        {
            circle(img, Point2d(points[i].x, (points[i].y)/10),3, Scalar(255, 0, 0), 3);
        }
        imshow("Location of Images", img);//Showing the circle//
        waitKey(0);//Waiting for Keystroke//
    }


    /*DESTRUCTIVE!! - removes found adj. points from points vector, adds them to the toBeDone vector
     Searches for adjacent points to a given point
     Returns either empty vector, or a vector with the found adjacent points
    */
    vector<Point2d> getAdjacents(Point2d inputPoint)
    {
        vector<Point2d> resultVector;
        for (int i = 0; i < points.size(); i++)
        {
            
            double yDifference = inputPoint.y-points[i].y;
            double xDifference = inputPoint.x - points[i].x;
            
            

            if(abs(yDifference)<15 && (xDifference<45) && (xDifference>0))
            {
                resultVector.push_back(points[i]);
                toDoVector.push_back(points[i]);
                points.erase(points.begin()+i);
                
            }
               
            
            
            if(abs(yDifference)<15 && (xDifference> -45) && (xDifference<0))
            {
                resultVector.push_back(points[i]);
                toDoVector.push_back(points[i]);
                points.erase(points.begin()+i);
            }
                

           
            if(abs(xDifference)<15 && (yDifference<45) && (yDifference>0))
            {
                resultVector.push_back(points[i]);
                toDoVector.push_back(points[i]);
                points.erase(points.begin()+i);

            }
               

            
            if(abs(xDifference)<15 && (yDifference> -45) && (yDifference<0))
            {
                resultVector.push_back(points[i]);
                toDoVector.push_back(points[i]);
                points.erase(points.begin()+i);

            }
        }
        return resultVector;

    }







    /* 
    stitches images
    points have to be loaded in the pointBase first
    uses public toDoVector, points
    BUG: not all adjacent images are used for stitching - OpenCV evaluates them as not appropriate for stitching
    */
    void stitchImgs()
    {
        int imgIdx = 0;
        //main loop
        while (!points.empty())
        {
            //take the first point from the left and find adjts. to it
            Point2d p = points[0];
            vector<Point2d> stitchQueue;
            stitchQueue.push_back(p);
            points.erase(points.begin());

            vector<Point2d> temp = getAdjacents(p); //find adj. of first point - inits toDo vector
            

            if (toDoVector.empty()) //if first point has no adjts. - log him as singleton
            {
                ofstream singletonsFile("singletons.log", ios_base::app);
                if(!singletonsFile)
                {
                    singletonsFile << "x: " << p.x << std::endl;
                    singletonsFile << "y: " << p.y << std::endl;
                    singletonsFile << pathMap[Point2d(p.x,p.y) ] << std::endl;
                    singletonsFile.close();
                }
                else
                {
                    ofstream sing;
                    sing.open("singletons.log",ios_base::app);
                    sing << "x: " << p.x << std::endl;
                    sing << "y: " << p.y << std::endl;
                    sing << pathMap[Point2d(p.x,p.y) ] << std::endl;
                    sing.close();
                }
                stitchQueue.pop_back();
                continue;
            }

            //finds adjacencies to every point
            while(!toDoVector.empty())
            {
                Point2d p2 = toDoVector[0];
                stitchQueue.push_back(p2);
                temp = getAdjacents(p2);
                toDoVector.erase(toDoVector.begin());
            }
            
            //stitching
            // Define object to store the stitched image
            Mat pano;
            bool try_use_gpu=true;
            // Create a Stitcher class object with mode scans
            Ptr<Stitcher> stitcher = Stitcher::create(Stitcher::SCANS);
            //stitcher->setPanoConfidenceThresh(0.85);
            cout << stitcher->panoConfidenceThresh() << "CONFIDENCE TRESH \n" ;

            //Ptr<Feature2D> finder = stitcher->featuresFinder();
            Ptr<Feature2D> finder = SIFT::create();
            stitcher->setFeaturesFinder(finder);
            cout << finder->getDefaultName()<<" FINDER \n";


            vector<Mat> imgs;

            //open stream for logging
            string logFileName = to_string(imgIdx) + ".log";
            ofstream logFile(logFileName);
            for (Point2d coords: stitchQueue)
            {
                //log which images are in which result file
                logFile << "x: " << coords.x << std::endl;
                logFile << "y: " << coords.y << std::endl;
                logFile << pathMap[Point2d(coords.x,coords.y) ] << std::endl;

                //log to stdout
                cout << "x: " << coords.x << std::endl;
                cout << "y: " << coords.y << std::endl;
                

                string jsonPath = pathMap[coords];
                string pngPath = jsonPath.erase(jsonPath.length()-4);
                pngPath = pngPath + "png";
                cout << "file: " << pngPath << std::endl;

                Mat img = imread(pngPath,IMREAD_COLOR);
                //imshow("Location of Images", img);//Showing the circle//

                if (!img.empty())
                {
                    imgs.push_back(img); //store loaded image in vector of images
                }
                else
                {
                     cout << "Error: Cannot read image"<< std::endl;
                }
                
            }
            logFile.close();


            //stitching
            cout << "Stitching" << std::endl;
            Stitcher::Status status = stitcher->stitch(imgs, pano);
            if (status != Stitcher::OK)
            {
                // Check if images could not be stitched
                // status is OK if images are stitched successfully
                cout << "Can't stitch images\n";
            }
            else
            {

                string resultName = "result" + to_string(imgIdx) +".jpg";

                // Store a new image stitched from the given
                //set of images as "result.jpg"
                imwrite(resultName, pano);
                
                // Show the result
                imshow(resultName, pano);
                imgIdx++;
                
                waitKey(0);
            }
            
            
        }
        
    }


};