#include <algorithm>
#include <memory>
#include <iostream>
#include <tut.h>

#include <TH2F.h>
#include <TFile.h>

///////////////////////////////////
// A TRULY EVIL HACK TO LET THE TESTS GET AT THE PRIVATE MEMBERS
#define private public
#include "IHoughLines.hxx"
#undef private
///////////////////////////////////

namespace tut {
    struct baseHoughLines {
        baseHoughLines() {
            // Run before each test.
        }
        ~baseHoughLines() {
            // Run after each test.
        }
    };

    // Declare the test
    typedef test_group<baseHoughLines>::object testHoughLines;
    test_group<baseHoughLines> groupHoughLines("HoughLines");

    // Test the default constructor and destructor.  Make sure the default
    // values are all null.
    template<> template<>
    void testHoughLines::test<1> () {
        COMET::IHoughLines* hough = new COMET::IHoughLines(-300,-300,  300, 300,  5);

        hough->AddPoint(new COMET::IHoughPointXY(0,100));
        hough->AddPoint(new COMET::IHoughPointXY(100,0));
        hough->AddPoint(new COMET::IHoughPointXY(200,-100));

        ensure("COMET::IHoughLines object created",hough);

        TH2F* hist = hough->CreateHistogram("oneLine");
        ensure_distance("Default X and Y bins are aproximately equal",
                      hist->GetNbinsX(), hist->GetNbinsY(),1);

        delete hist;
        delete hough;
    }

    // Check that a single line can be found
    template<> template<>
    void testHoughLines::test<2> () {
        COMET::IHoughLines* hough = new COMET::IHoughLines(-300,-300,  300,300,  5);

        hough->AddPoint(new COMET::IHoughPointXY(-100,200));
        hough->AddPoint(new COMET::IHoughPointXY(0,100));
        hough->AddPoint(new COMET::IHoughPointXY(100,0));
        hough->AddPoint(new COMET::IHoughPointXY(200,-100));

        TH2F* hist = hough->CreateHistogram("oneLine");

        TFile output("tutHoughLines_test2.root","RECREATE");
        hist->Write();
        output.Close();
            
        ensure_distance("Line was found",
                        hist->GetMaximum(10),4.0,0.01);
                        
        delete hist;
        delete hough;
    }

    // Check that points can be removed from the line based on threshold.
    template<> template<>
    void testHoughLines::test<3> () {
        COMET::IHoughLines* hough = new COMET::IHoughLines(-300,-300,  300,300,  5);

        hough->AddPoint(new COMET::IHoughPointXY(-100,200));
        hough->AddPoint(new COMET::IHoughPointXY(0,100));
        hough->AddPoint(new COMET::IHoughPointXY(100,0));
        hough->AddPoint(new COMET::IHoughPointXY(200,-100));

        int origLines = 0;
        for (COMET::IHoughLines::Lines::iterator ln = hough->GetLines().begin();
             ln != hough->GetLines().end();
             ++ln) {
            if (ln->GetWeight()>0) ++origLines;
        }

        COMET::IHoughLines::Points& points = hough->GetPoints();
        for (COMET::IHoughLines::Points::iterator pt = points.begin();
             pt != points.end();
             ++pt) {
            (*pt)->RemoveFromLines(0.55);
        }

        int culledLines = 0;
        for (COMET::IHoughLines::Lines::iterator ln = hough->GetLines().begin();
             ln != hough->GetLines().end();
             ++ln) {
            if (ln->GetWeight()>0) ++culledLines;
        }
        
        ensure_lessthan("Lines were culled", culledLines, origLines);

        TH2F* hist = hough->CreateHistogram("oneLine");

        ensure_distance("Line was found",
                        hist->GetMaximum(10),4.0,0.01);
                        
        TFile output("tutHoughLines_test3.root","RECREATE");
        hist->Write();
        output.Close();
            
        delete hist;
        delete hough;
    }

    // Make sure that every line referenced by a point actually contains that
    // point.
    template<> template<>
    void testHoughLines::test<4> () {
        COMET::IHoughLines* hough = new COMET::IHoughLines(-300,-300,  300,300,  5);

        hough->AddPoint(new COMET::IHoughPointXY(-100,200));
        hough->AddPoint(new COMET::IHoughPointXY(0,100));
        hough->AddPoint(new COMET::IHoughPointXY(100,0));
        hough->AddPoint(new COMET::IHoughPointXY(200,-100));
        
        for (COMET::IHoughLines::Points::iterator p = hough->fPoints.begin();
             p != hough->fPoints.end();
             ++p) {
            for (std::set<COMET::IHoughLine*>::iterator l = (*p)->fLines.begin();
                 l != (*p)->fLines.end();
                 ++l) {
                ensure("Point in line", 
                       std::find((*l)->fPoints.begin(),
                                 (*l)->fPoints.end(),(*p))
                       != (*l)->fPoints.end());
            }
        }
        
        delete hough;
    }

    // Make sure that the points along the line are properly sorted, and
    // within one half of the line width of the line center.
    template<> template<>
    void testHoughLines::test<5> () {
        COMET::IHoughLines* hough = new COMET::IHoughLines(-300,-300,  300,300,  5);

        hough->AddPoint(new COMET::IHoughPointXY(-100,200));
        hough->AddPoint(new COMET::IHoughPointXY(0,100));
        hough->AddPoint(new COMET::IHoughPointXY(100,0));
        hough->AddPoint(new COMET::IHoughPointXY(200,-100));
        
        hough->AddPoint(new COMET::IHoughPointXY(-100,0));
        hough->AddPoint(new COMET::IHoughPointXY(-200,-100));
        hough->AddPoint(new COMET::IHoughPointXY(200,100));
        
        for (COMET::IHoughLines::Lines::iterator ln = hough->GetLines().begin();
             ln != hough->GetLines().end();
             ++ln) {
            COMET::IHoughLine::Points& points = ln->GetPoints();
            if (points.size()<2) continue;
            for (COMET::IHoughLine::Points::iterator p = points.begin();
                 p != points.end();
                 ++p) {
                ln->GetDeviation(*p);
                ensure_lessthan("Close to line",
                                std::abs(ln->GetDeviation((*p))),5);
                COMET::IHoughLine::Points::iterator next = p;
                ++next;
                if (next == points.end()) break;
                double len = ln->GetLength(*next) - ln->GetLength(*p);
                ensure_greaterthan("Length increases", len,0);
            }
        }
        
        delete hough;
    }

    // Make sure a point adds the correct width curve to the hough space for
    // an oversampling of two.
    template<> template<>
    void testHoughLines::test<6> () {
        COMET::IHoughLines* hough = new COMET::IHoughLines(-30,-30,  30,30,  5, 2);

        hough->AddPoint(new COMET::IHoughPointXY(5,5));
        
        TH2F* hist = hough->CreateHistogram("oneLine");

        TFile output("tutHoughLines_test6.root","RECREATE");
        hist->Write();
        output.Close();
            
        int twos = 0;
        int threes = 0;
        for (int x = 0; x<hist->GetNbinsX(); ++x) {
            int nonZero = 0;
            for (int y = 0; y<hist->GetNbinsX(); ++y) {
                double val = hist->GetBinContent(x,y);
                if (val>0) ++nonZero;
            }
            ensure_greaterthan("Hough line fills all angles",
                               nonZero,1);
            ensure_lessthan("Hough Line is not too wide", nonZero,4);
            if (nonZero == 2) ++twos;
            else ++threes;
        }
        ensure_greaterthan("Hough line width is OK",
                           twos,threes);

        delete hist;
        delete hough;
    }

    // Make sure a point adds the correct width curve to the hough space for
    // an oversampling of three.
    template<> template<>
    void testHoughLines::test<7> () {
        COMET::IHoughLines* hough = new COMET::IHoughLines(-30,-30,  30,30,  5, 3);

        hough->AddPoint(new COMET::IHoughPointXY(5,5));
        
        TH2F* hist = hough->CreateHistogram("oneLine");

        TFile output("tutHoughLines_test7.root","RECREATE");
        hist->Write();
        output.Close();
            
        int threes = 0;
        int fours = 0;
        for (int x = 0; x<hist->GetNbinsX(); ++x) {
            int nonZero = 0;
            for (int y = 0; y<hist->GetNbinsX(); ++y) {
                double val = hist->GetBinContent(x,y);
                if (val>0) ++nonZero;
            }
            ensure_greaterthan("Hough line fills all angles",
                               nonZero,2);
            ensure_lessthan("Hough Line is not too wide", nonZero,5);
            if (nonZero == 3) ++threes;
            else ++fours;
        }
        ensure_greaterthan("Hough line width is OK",
                           threes,fours);

        delete hist;
        delete hough;
    }

    // Check that shared points can be removed from lines.
    template<> template<>
    void testHoughLines::test<8> () {
        COMET::IHoughLines* hough = new COMET::IHoughLines(-300,-300,  300,300,  5);

        hough->AddPoint(new COMET::IHoughPointXY(-100,200));
        hough->AddPoint(new COMET::IHoughPointXY(0,100));
        hough->AddPoint(new COMET::IHoughPointXY(100,0));
        hough->AddPoint(new COMET::IHoughPointXY(200,-100));

        int origLines = 0;
        for (COMET::IHoughLines::Lines::iterator ln = hough->GetLines().begin();
             ln != hough->GetLines().end();
             ++ln) {
            if (ln->GetWeight()>0) ++origLines;
        }

        hough->RemoveShared(0.95);

        int culledLines = 0;
        for (COMET::IHoughLines::Lines::iterator ln = hough->GetLines().begin();
             ln != hough->GetLines().end();
             ++ln) {
            if (ln->GetWeight()>0) ++culledLines;
        }
        
        ensure_lessthan("Lines were culled", culledLines, origLines);

        TH2F* hist = hough->CreateHistogram("oneLine");

        ensure_distance("Line was found",
                        hist->GetMaximum(10),4.0,0.01);
                        
        TFile output("tutHoughLines_test8.root","RECREATE");
        hist->Write();
        output.Close();
            
        delete hist;
        delete hough;
    }


};