/**
 * @file plane3d.cc
 *
 * @auhtor Remus Claudiu Dumitru <r.dumitru@jacobs-university.de>
 * @date 15 Apr 2012
 *
 */

//==============================================================================
//  Includes
//==============================================================================
#include "model/labeledPlane3d.h"
#include "model/util.h"

#include <math.h>

#include <set>
#include <limits>
#include <stdexcept>
#include <iostream>
#include <fstream>
using namespace std;

//==============================================================================
//  Static member initializations
//==============================================================================
const std::string model::LabeledPlane3d::RED            = "180 0 0";
const std::string model::LabeledPlane3d::GREEN          = "0 180 0";
const std::string model::LabeledPlane3d::BLUE           = "0 0 180";
const std::string model::LabeledPlane3d::WHITE          = "255 255 255";

const double model::LabeledPlane3d::LINE_EPSILON        = 0.2;

const double model::LabeledPlane3d::SOBEL_SCALE         = 1.0;
const double model::LabeledPlane3d::SOBEL_DELTA         = 0.0;
const int model::LabeledPlane3d::SOBEL_KERNEL           = 3;

const double model::LabeledPlane3d::CANNY_THRESH1       = 40.0;
const double model::LabeledPlane3d::CANNY_THRESH2       = 120.0;

const double model::LabeledPlane3d::HOUGH_RHO           = 1.0;
const double model::LabeledPlane3d::HOUGH_THETA         = CV_PI / 180.0;
const int model::LabeledPlane3d::HOUGH_THRESH           = 80;
const double model::LabeledPlane3d::HOUGH_MIN_LINE_LEN  = 15.0;

const double model::LabeledPlane3d::MIN_EMPTY_AREA      = 0.67;
const double model::LabeledPlane3d::MIN_TOTAL_AREA      = 0.0075;
const double model::LabeledPlane3d::MAX_TOTAL_AREA      = 0.400;

//==============================================================================
//  Class implementation
//==============================================================================
model::LabeledPlane3d::LabeledPlane3d() : Plane3d() {}

model::LabeledPlane3d::LabeledPlane3d(const Point3d& pt, const Vector3d& normal) :
        Plane3d(pt, normal)
{

}

model::LabeledPlane3d::LabeledPlane3d(const Point3d& pt, const Vector3d& normal, const std::vector<Point3d>& hull) :
        Plane3d(pt, normal, hull)
{}

model::LabeledPlane3d::LabeledPlane3d(const LabeledPlane3d& other) : Plane3d(other) {
    this->patches= other.patches;
    this->depthMap = other.depthMap;
    this->depthImg = other.depthImg;
    this->correctedDepthImg = other.correctedDepthImg;
    this->depthMapDistances = other.depthMapDistances;
}

model::LabeledPlane3d::~LabeledPlane3d() {}

model::LabeledPlane3d& model::LabeledPlane3d::operator=(const LabeledPlane3d& other) {
    if (this != &other) {
        Plane3d::operator =(other);
        this->patches= other.patches;
        this->depthMap = other.depthMap;
        this->depthImg = other.depthImg;
        this->correctedDepthImg = other.correctedDepthImg;
        this->depthMapDistances = other.depthMapDistances;
    }

    return *this;
}

void model::LabeledPlane3d::detectEdges(cv::Mat& canny, cv::Mat& hSobel, cv::Mat& vSobel, cv::Mat& combined) const {
    int imgHeight = static_cast<int>(this->depthMap.size());
    int imgWidth  = static_cast<int>(this->depthMap.front().size());

    if (imgHeight == 0 || imgWidth == 0) {
        throw runtime_error("please initialize the patches and depth map for labeled plane");
    }

    // apply Canny edge detection so we can see the lines contained in the image
    cv::Canny(this->depthImg, canny, CANNY_THRESH1, CANNY_THRESH2);

    // apply Sobel on same image so we get better defined horizontal and vertical lines
    cv::Sobel(canny, hSobel, CV_8UC1, 0, 1, SOBEL_KERNEL, SOBEL_SCALE, SOBEL_DELTA, cv::BORDER_DEFAULT);
    cv::Sobel(canny, vSobel, CV_8UC1, 1, 0, SOBEL_KERNEL, SOBEL_SCALE, SOBEL_DELTA, cv::BORDER_DEFAULT);

    // combine all of them for even better edges
    cv::addWeighted(canny, 0.5, hSobel, 0.5, 0, combined);
    cv::threshold(combined, combined, 1, MAX_IMG_VAL, CV_8UC1);
    cv::addWeighted(combined, 0.5, vSobel, 0.5, 0, combined);
    cv::threshold(combined, combined, 1, MAX_IMG_VAL, CV_8UC1);

    cv::imwrite("./img/edgeCanny.png", canny);
    cv::imwrite("./img/edgeSobelHoriz.png", hSobel);
    cv::imwrite("./img/edgeSobelVert.png", vSobel);
    cv::imwrite("./img/edgeCombined.png", combined);
}

void model::LabeledPlane3d::computeLines(std::vector<int>& verticalResult, std::vector<int>& horizontalResult) const {

    if (this->depthMap.size() != this->patches.size() ||
            this->depthMap.front().size() != this->patches.front().size())
    {
        throw runtime_error("depth map and patch matrix must match in sizes");
    }

    int imgHeight = static_cast<int>(this->depthMap.size());
    int imgWidth  = static_cast<int>(this->depthMap.front().size());

    if (imgHeight == 0 || imgWidth == 0) {
        throw runtime_error("please initialize the patches and depth map for labeled plane");
    }

    // clear the output
    verticalResult.clear();
    horizontalResult.clear();

    // compute the Canny edges normally, and using sobel on x and y
    cv::Mat canny, hSobel, vSobel, combined;
    this->detectEdges(canny, hSobel, vSobel, combined);

    // detect all the proper lines from the image
    vector<cv::Vec4i> lines;
    cv::HoughLinesP(combined, lines, HOUGH_RHO, HOUGH_THETA, HOUGH_THRESH, HOUGH_MIN_LINE_LEN);

    // convert to color image so we can draw some colored lines on it
    cv::cvtColor(combined, combined, CV_GRAY2BGR, 3);
    cv::Mat imgLines      = combined.clone();
    cv::Mat imgFinalLines = combined.clone();

    // filter out the vertical and the horizontal lines
    // XXX use sets so that the numbers are ordered automatically
    set<int> horizontalSet, verticalSet;
    int curr;

    for (vector<cv::Vec4i>::iterator it = lines.begin(); it != lines.end(); ++it) {
        int dX = it->val[2] - it->val[0];
        int dY = it->val[3] - it->val[1];

        double angle = atan2(static_cast<double>(dY), static_cast<double>(dX));

        if (((angle < +M_PI_2 + LINE_EPSILON) && (angle > +M_PI_2 - LINE_EPSILON)) ||
                ((angle < -M_PI_2 + LINE_EPSILON) && (angle > -M_PI_2 - LINE_EPSILON)))
        {
            // detect and add the vertical lines
            curr = static_cast<int>(round((it->val[2] + it->val[0]) / 2.0));
            verticalSet.insert(curr);
            cv::line(imgLines, cv::Point(curr, 0), cv::Point(curr, imgLines.rows-1), cv::Scalar(0, 0, MAX_IMG_VAL), 1);

        } else if (((angle < +0.0 + LINE_EPSILON) && (angle > +0.0 - LINE_EPSILON)) ||
                ((angle < +M_PI + LINE_EPSILON) && (angle > +M_PI - LINE_EPSILON)) ||
                ((angle < -M_PI + LINE_EPSILON) && (angle > -M_PI - LINE_EPSILON)))
        {
            // detect and add the horizontal lines
            curr = static_cast<int>(round((it->val[3] + it->val[1]) / 2.0));
            horizontalSet.insert(curr);
            cv::line(imgLines, cv::Point(0, curr), cv::Point(imgLines.cols-1, curr), cv::Scalar(MAX_IMG_VAL, 0, 0), 1);
        }
    }

    // insert the edges as lines
    horizontalSet.insert(0);
    horizontalSet.insert(imgHeight - 1);
    cv::line(imgLines, cv::Point(0, 0), cv::Point(imgLines.cols-1, 0), cv::Scalar(MAX_IMG_VAL, 0, 0), 1);
    cv::line(imgLines, cv::Point(0, imgHeight - 1), cv::Point(imgLines.cols-1, imgHeight - 1), cv::Scalar(MAX_IMG_VAL, 0, 0), 1);

    verticalSet.insert(0);
    verticalSet.insert(imgWidth - 1);
    cv::line(imgLines, cv::Point(0, 0), cv::Point(0, imgLines.rows-1), cv::Scalar(0, 0, MAX_IMG_VAL), 1);
    cv::line(imgLines, cv::Point(imgWidth - 1, 0), cv::Point(imgWidth - 1, imgLines.rows-1), cv::Scalar(0, 0, MAX_IMG_VAL), 1);

    cv::imwrite("./img/edgeLines.png", imgLines);
    if (!quiet) cout << "** Total lines: " << verticalSet.size() << " vertical, " << horizontalSet.size() << " horizontal" << endl;

    horizontalResult.assign(horizontalSet.begin(), horizontalSet.end());
    verticalResult.assign(verticalSet.begin(), verticalSet.end());
}

void model::LabeledPlane3d::computeOpeningCandidates(std::vector<CandidateOpening>& result) const {
    if (!quiet) cout << endl << "== Computing opening candidates for surface centered at " << this->pt << endl;

    // make sure the result is empty
    result.clear();

    if (this->depthMap.size() != this->patches.size() ||
            this->depthMap.front().size() != this->patches.front().size())
    {
        throw runtime_error("depth map and patch matrix must match in sizes");
    }

    int imgHeight = static_cast<int>(this->depthMap.size());
    int imgWidth  = static_cast<int>(this->depthMap.front().size());

    if (imgHeight == 0 || imgWidth == 0) {
        throw runtime_error("please initialize the patches and depth map for labeled plane");
    }

    // convert the sets to vectors
    vector<int> horizontal, vertical;
    this->computeLines(vertical, horizontal);

    // we require the total wall area to discard a few small candidates
    double wCentimetersWall = this->patches[0][0].first.distance(this->patches[0][imgWidth - 1].first);
    double hCentimetersWall = this->patches[0][0].first.distance(this->patches[imgHeight - 1][0].first);
    double wallArea = wCentimetersWall * hCentimetersWall;

    // put temporary candidates in here
    std::vector<CandidateOpening> candidates;

    // count the number of discarded small planes
    unsigned int discarded = 0;

    // there is no other way of determining all the rectangles in the image
    // u - upper, l - lower, h - horizontal, v - vertical
    for (vector<int>::iterator y1 = horizontal.begin(); y1 != horizontal.end() - 1; ++y1) {
        for (vector<int>::iterator y2 = y1 + 1; y2 != horizontal.end(); ++y2) {
            for (vector<int>::iterator x1 = vertical.begin(); x1 != vertical.end() - 1; ++x1) {
                for (vector<int>::iterator x2 = x1 + 1; x2 != vertical.end(); ++x2) {

                    // do not add really small candidates that are smaller than a certain percentage of the whole wall
                    double wCentiMeters = this->patches[0][*x2].first.distance(this->patches[0][*x1].first);
                    double hCentiMeters = this->patches[*y2][0].first.distance(this->patches[*y1][0].first);
                    double area = wCentiMeters * hCentiMeters;

                    if (area < MIN_TOTAL_AREA * wallArea || area > MAX_TOTAL_AREA * wallArea) {
                        discarded++;
                        continue;
                    }

                    // create edges for candidate opening
                    int tempEdges[4] = {*y1, *y2, *x1, *x2};
                    vector<int> edges(tempEdges, tempEdges + 4);

                    // compute the hull
                    vector<Point3d> hull;
                    hull.push_back(this->patches[*y1][*x1].first);
                    hull.push_back(this->patches[*y1][*x2].first);
                    hull.push_back(this->patches[*y2][*x2].first);
                    hull.push_back(this->patches[*y2][*x1].first);

                    // compute the normal application point
                    Point3d pt(0.0, 0.0, 0.0);
                    for (vector<Point3d>::iterator it = hull.begin(); it != hull.end(); ++it) {
                        pt += *it;
                    }
                    pt /= hull.size();

                    CandidateOpening candidate(pt, this->normal, hull, vector<double>(), edges);
                    candidate.normal = candidate.computeAverageNormal();
                    candidates.push_back(candidate);
                }
            }
        }
    }

    if (!quiet) cout << "** Total opening candidates: " << candidates.size()  << endl;
    if (!quiet) cout << "** Discarded openings relative size to wall: " << discarded << endl;
    discarded = 0;

    // determine the features of each candidate opening
    for (vector<CandidateOpening>::iterator it = candidates.begin(); it < candidates.end(); ++it) {
        int y1 = it->edges[0];
        int y2 = it->edges[1];
        int x1 = it->edges[2];
        int x2 = it->edges[3];

        if (y1 > y2 || x1 > x2) {
            throw runtime_error("upper coordinates need to be smaller than the lower ones (candidate opening)");
        }

        int w = abs(x2 - x1);
        int h = abs(y2 - y1);

        // first feature, the area of the opening
        double wCentiMeters = this->patches[0][x2].first.distance(this->patches[0][x1].first);
        double hCentiMeters = this->patches[y2][0].first.distance(this->patches[y1][0].first);
        double area = wCentiMeters * hCentiMeters;
        it->features.push_back(static_cast<double>(area));

        // second feature, w/h
        it->features.push_back(static_cast<double>(w) / h);

        // third feature, w/W
        it->features.push_back(static_cast<double>(w) / imgWidth);

        // fourth feature, h/H
        it->features.push_back(static_cast<double>(h) / imgHeight);

        // distance from every edge
        // upper horizontal line
        it->features.push_back(this->patches[y1][0].first.distance(this->patches.front()[0].first));
        // lower horizontal line
        it->features.push_back(this->patches[y2][0].first.distance(this->patches.back()[0].first));
        // upper vertical line
        it->features.push_back(this->patches[0][x1].first.distance(this->patches[0].front().first));
        // lower vertical line
        it->features.push_back(this->patches[0][x2].first.distance(this->patches[0].back().first));

        // compute the RMS of the plane fit residual for this particular rectangle
        // in the same time compute the area of each label
        int empty = 0, occupied = 0, occluded = 0;

        // TODO figure out how to compute the residual
        double sqSum = 0.0;
        for (int i = y1; i < y2; ++i) {
            for (int j = x1; j < x2; ++j) {
                // TODO should be computed accordingly
                double temp = 0.0;

                switch (this->patches[i][j].second) {
                case EMPTY:
                    empty++;
                    break;
                case OCCUPIED:
                    occupied++;
                    break;
                case OCCLUDED:
                    occluded++;
                    break;
                default:
                    throw runtime_error("default branch taken while computing features for opening candidate");
                    break;
                }
                sqSum += pow(temp, 2.0);
            }
        }

        // add the RMS plane fit
        int totalCount = w * h;
        it->features.push_back(sqrt(sqSum / totalCount));

        // discard a few more candidates
        if ((static_cast<double>(empty) / totalCount) < MIN_EMPTY_AREA) {
            discarded++;
            continue;
        }

        // add the area of each label, empty, occupied, occluded
        it->features.push_back(static_cast<double>(empty) / totalCount);
        it->features.push_back(static_cast<double>(occupied) / totalCount);
        it->features.push_back(static_cast<double>(occluded) / totalCount);

        // determine how many interior rectangles this candidate contains
        // compute the number of interior inverted U-shapes as well
        // start counting from 2 as we include the edges as well
        int vCount = 2, hCount = 2;
        for (vector<int>::iterator jt = horizontal.begin(); jt != horizontal.end(); ++jt) {
            if (*jt > y1 && *jt < y2) {
                hCount++;
            }
        }

        for (vector<int>::iterator jt = vertical.begin(); jt != vertical.end(); ++jt) {
            if (*jt > x1 && *jt < x2) {
                vCount++;
            }
        }

        // add the number of rectangles contained inside
        it->features.push_back((hCount * (hCount - 1) / 2.0) * (vCount * (vCount - 1) / 2.0));

        // add the number of interior inverted U-shapes
        if (y2 == imgHeight - 1) {
            it->features.push_back((hCount - 1) * (vCount * (vCount - 1) / 2.0));
        } else {
            it->features.push_back(0.0);
        }

        // add this current candidate to the final result
        result.push_back(*it);
    }

    if (!quiet) cout << "** Discarded openings due to small empty area: " << discarded << endl;
    if (!quiet) cout << "** Done computing features for all opening candidates" << endl;
}