#pragma once
#include "opencv2\core\core.hpp"
#include "opencv\cv.h"
#include "opencv2\highgui\highgui.hpp"
#include <iostream>
#include <fstream>
#define _USE_MATH_DEFINES
#include "math.h"
using namespace std;
using namespace cv;
class WallFinder
{
private:
    ofstream myfile;
    vector<Vec4i> lines;
    vector<Vec4i> secondaryLines;
    double deltaRho;
    double deltaTheta;
    int minVote;
    double minLength;
    double maxGap;
public:
    WallFinder() : deltaRho(1), deltaTheta(M_PI/180), minVote(10), minLength(0.), maxGap(0.) {}
    void setMinVote(int minv) {
    minVote= minv;
    }
    void setLineLengthAndGap(double length,double gap) {
    minLength= length;
    maxGap= gap;
    }
    void findLines(Mat& skeleton)
    {
    lines.clear();
    HoughLinesP(skeleton,lines,deltaRho,deltaTheta,minVote,minLength,maxGap);
    }
    void showLinesOnImage(Mat& skeleton, Scalar color=Scalar(128))
    {
    int i=0,j=0,yaxis=0,xaxis=0,xaxis2=0,yaxis2=0;
    myfile.open("coordinates.txt", ios::out);
    for (vector<Vec4i>::const_iterator it=lines.begin(); it!=lines.end(); it++)
    {
    Point pt1((*it)[0],(*it)[1]);
    Point pt2((*it)[2],(*it)[3]);
    line( skeleton, pt1, pt2, color);
    writeToFile((*it)[0],(*it)[1],(*it)[2],(*it)[3],i);
    ++i;
    }
    for (vector<Vec4i>::const_iterator it2=lines.begin(); it2!=lines.end(); it2++)
    {
        if((*it2)[0]==(*it2)[2])
        {
            yaxis=(*it2)[1];
            while(yaxis!=(*it2)[3]-1)
            {
            xaxis=(*it2)[0];
            xaxis2=(*it2)[0];
            while (static_cast<int>(skeleton.at<uchar>(yaxis,xaxis-5))==255)
            {
                --xaxis;
            }
            while (static_cast<int>(skeleton.at<uchar>(yaxis,xaxis2+5))==255)
            {
                ++xaxis2;
            }

            if(xaxis!=(*it2)[0])
            {
                line(skeleton,Point((*it2)[0],yaxis),Point(xaxis,yaxis),color);
                writeToFile((*it2)[0],yaxis,xaxis,yaxis,i);
                lines.push_back(Vec4i((*it2)[0],yaxis,xaxis,yaxis)); //ERROR
                ++i;
            }

            if(xaxis2!=(*it2)[0])
            {
                line(skeleton,Point((*it2)[0],yaxis),Point(xaxis2,yaxis),color);
                writeToFile((*it2)[0],yaxis,xaxis2,yaxis,i);
                lines.push_back(Vec4i((*it2)[0],yaxis,xaxis2,yaxis)); //ERROR
                ++i;
            }
            --yaxis;
            }
        }
        if((*it2)[1]==(*it2)[3])
        {
            xaxis=(*it2)[2];
            while(xaxis!=(*it2)[0]-1)
            {
            yaxis=(*it2)[1];
            yaxis2=(*it2)[1];
            while (static_cast<int>(skeleton.at<uchar>(yaxis-7,xaxis))==255)
            {
                --yaxis;
            }
            while (static_cast<int>(skeleton.at<uchar>(yaxis2+7,xaxis))==255)
            {
                ++yaxis2;
            }

            if(yaxis!=(*it2)[1])
            {
                line(skeleton,Point(xaxis,(*it2)[1]),Point(xaxis,yaxis),color);
                writeToFile(xaxis,(*it2)[1],xaxis,yaxis,i);
                lines.push_back(Vec4i(xaxis,(*it2)[1],xaxis,yaxis)); //ERROR
                ++i;
            }

            if(yaxis2!=(*it2)[1])
            {
                line(skeleton,Point(xaxis,(*it2)[1]),Point(xaxis,yaxis2),color);
                writeToFile(xaxis,(*it2)[1],xaxis,yaxis2,i);
                lines.push_back(Vec4i(xaxis,(*it2)[1],xaxis,yaxis2)); //ERROR
                ++i;
            }
            --xaxis;
            }
        }
    } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    myfile.close();
    }
    void writeToFile(int x1,int y1,int x2,int y2,int i)
    {
        myfile << "line "<<i<<" starting point= ("<<x1<<","<<y1<<")\n"<<"line "<<i<<" ending point= ("<<x2<<","<<y2<<")\n";

    }

    Mat skeleton(Mat& img)
    {
        cv::threshold(img, img, 127, 255, cv::THRESH_BINARY);
        cv::Mat skel(img.size(), CV_8UC1, cv::Scalar(0));
        cv::Mat temp;
        cv::Mat eroded;
        cv::Mat element = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3));

        bool done;
        do
        {
          cv::erode(img, eroded, element);
          cv::dilate(eroded, temp, element); // temp = open(img)
          cv::subtract(img, temp, temp);
          cv::bitwise_or(skel, temp, skel);
          eroded.copyTo(img);

          done = (countNonZero(img) == 0);
        } while (!done);
        return skel;
    }
};