#include "gLocalize.h"

#include <qeventloop.h>
#include <qapplication.h>
#include <vtkGeometryFilter.h>
#include <vtkThreshold.h>


gLocalize::gLocalize(){

    abortSignal=false;
}

void gLocalize::localize(vtkImageData* vol,
                         int march_tresh,
                         double thrDown_D,
                         double thrUp_D,
                         double thr_HAUSD,
                         double thr_S,
                         QList<bool> selectedFilters)
{
    std::cout << "march_tresh: " << march_tresh << std::endl;
    std::cout << "thrDown_D: "   << thrDown_D   << std::endl;
    std::cout << "thrUp_D: "     << thrUp_D     << std::endl;
    std::cout << "thr_HAUSD: "   << thr_HAUSD   << std::endl;
    std::cout << "thr_S: "       << thr_S       << std::endl;

    abortSignal = false;
    int progress = 0;

    vtkSmartPointer<vtkMarchingCubes> marchingcubes = vtkSmartPointer<vtkMarchingCubes>::New();
    vtkSmartPointer<vtkWindowedSincPolyDataFilter> smoother = vtkSmartPointer<vtkWindowedSincPolyDataFilter>::New();
    vtkSmartPointer<vtkPolyDataNormals> normals = vtkSmartPointer<vtkPolyDataNormals>::New();

    // Connectivity filter used ONLY to label regions (AllRegions + RegionId).
    vtkSmartPointer<vtkPolyDataConnectivityFilter> regionLabeler =
        vtkSmartPointer<vtkPolyDataConnectivityFilter>::New();

    marchingcubes->SetInputData(vol);
    marchingcubes->SetValue(0, march_tresh);
    marchingcubes->SetComputeNormals(0);

    if (marchingcubes->GetNumberOfInputConnections(0) == 0)
        return;

    marchingcubes->Update();
    std::cout << "inizio run" << std::endl;

    // Smoothing
    smoother->SetInputConnection(marchingcubes->GetOutputPort());
    smoother->SetNumberOfIterations(20);
    smoother->NormalizeCoordinatesOn();
    smoother->SetPassBand(0.01);
    smoother->BoundarySmoothingOn();
    smoother->Update();

    // Normals
    normals->SetInputConnection(smoother->GetOutputPort());
    normals->Update();

    // Label regions (this produces RegionId in *PointData* in your build)
    regionLabeler->SetInputConnection(normals->GetOutputPort());
    regionLabeler->SetExtractionModeToAllRegions();
    regionLabeler->ColorRegionsOn();  // IMPORTANT
    regionLabeler->Update();

    vtkPolyData* labeledPoly = regionLabeler->GetOutput();
    if (!labeledPoly)
        throw("CRITICAL: connectivity produced no output");

    // Debug prints (optional)
    // labeledPoly->GetCellData()->Print(std::cout);
    // labeledPoly->GetPointData()->Print(std::cout);

    // RegionId is in PointData:
    vtkDataArray* ridArr = labeledPoly->GetPointData()->GetArray("RegionId");
    if (!ridArr)
        throw("CRITICAL: RegionId array not found in PointData (ColorRegionsOn did not produce it)");

    // Determine region id range robustly
    double rRange[2];
    ridArr->GetRange(rRange);
    int minRid = static_cast<int>(rRange[0]);
    int maxRid = static_cast<int>(rRange[1]);
    int nRegions = regionLabeler->GetNumberOfExtractedRegions();

    std::cout << "nRegions=" << nRegions
              << " RegionId range=" << minRid << ".." << maxRid << "\n";

    if (nRegions == 0)
        throw("CRITICAL: No surfaces in ConnectivityFilter output");
    if (nRegions > 3000)
        std::cout << "ho trovato troppe superfici: " << nRegions << std::endl;

    QList<surf> surflist;
    surflist.clear();
    surf tempsurf;

    int OKd = 0, OKh = 0, OKs = 0;

    // We iterate over RegionId range (more reliable than assuming 0..nRegions-1)
    for (int idouble = minRid; idouble <= maxRid; ++idouble)
    {
        QApplication::processEvents();
        progress++;

        // progress relative to range:
        const double denom = (maxRid - minRid + 1);
        emit localizationProgress((double)progress / denom * 100.0);

        if (abortSignal)
        {
            abortSignal = false;
            emit localizationAborted();
            return;
        }

        int ok = 1;

        // --- Extract ONE region by thresholding RegionId on POINTS ---
        vtkNew<vtkThreshold> th;
        th->SetInputData(labeledPoly);

        th->SetInputArrayToProcess(
            0, 0, 0,
            vtkDataObject::FIELD_ASSOCIATION_POINTS,  // IMPORTANT: RegionId is point data
            "RegionId"
            );
        th->SetLowerThreshold(idouble);
        th->SetUpperThreshold(idouble);

        vtkNew<vtkGeometryFilter> geom;
        geom->SetInputConnection(th->GetOutputPort());
        geom->Update();

        vtkPolyData* out0 = geom->GetOutput();
        if (!out0 || out0->GetNumberOfCells() == 0)
            continue;

        // Keep only one connected surface (thresholding on points can leave fragments)
        vtkNew<vtkPolyDataConnectivityFilter> keepLargest;
        keepLargest->SetInputData(out0);
        keepLargest->SetExtractionModeToLargestRegion();
        keepLargest->Update();

        vtkPolyData* out = keepLargest->GetOutput();
        if (!out || out->GetNumberOfCells() == 0)
            continue;

        out->ComputeBounds();
        double b[6]; out->GetBounds(b);
        double len = out->GetLength();

        std::cout << "id " << idouble
                  << " cells " << out->GetNumberOfCells()
                  << " len " << len
                  << " bounds "
                  << b[0] << " " << b[1] << " "
                  << b[2] << " " << b[3] << " "
                  << b[4] << " " << b[5] << "\n";

        // --- Your filters ---
        if (selectedFilters.at(0) && ok == 1)
        {
            tempsurf.lenght = len;
            if (tempsurf.lenght < thrDown_D || tempsurf.lenght > thrUp_D)
                ok = 0;
        }

        if (selectedFilters.at(1) && ok == 1)
        {
            OKd++;
            out->GetCenter(tempsurf.center);

            tempsurf.hausdorff = hausdorff(out, tempsurf);

            if (tempsurf.hausdorff > thr_HAUSD)
                ok = 0;
        }

        if (selectedFilters.at(2) && ok == 1)
        {
            OKh++;
            out->GetBounds(tempsurf.bounds);
            tempsurf.confronto_lati = lati(tempsurf.bounds);
            if (tempsurf.confronto_lati > thr_S)
                ok = 0;
        }

        if (ok == 1)
        {
            OKs++;
            tempsurf.sorting = idouble; // store RegionId
            surflist.append(tempsurf);
        }
    }

    std::cout << "Nsurfaces " << nRegions << std::endl;
    std::cout << "OKD " << OKd << " OKH " << OKh << " OKS " << OKs << std::endl;

    // --- marker center calculation ---
    QList<surf> final_list;
    surf tmp_p;

    if (surflist.count() != 0)
    {
        for (int ii = 0; ii < surflist.count(); ++ii)
        {
            tmp_p = surflist.at(ii);

            // Re-extract the chosen region using the SAME pipeline as above:
            vtkNew<vtkThreshold> th;
            th->SetInputData(labeledPoly);
            th->SetInputArrayToProcess(
                0, 0, 0,
                vtkDataObject::FIELD_ASSOCIATION_POINTS,
                "RegionId"
                );
            th->SetLowerThreshold(tmp_p.sorting);
            th->SetUpperThreshold(tmp_p.sorting);

            vtkNew<vtkGeometryFilter> geom;
            geom->SetInputConnection(th->GetOutputPort());
            geom->Update();

            vtkPolyData* out0 = geom->GetOutput();
            if (!out0 || out0->GetNumberOfCells() == 0)
                continue;

            vtkNew<vtkPolyDataConnectivityFilter> keepLargest;
            keepLargest->SetInputData(out0);
            keepLargest->SetExtractionModeToLargestRegion();
            keepLargest->Update();

            vtkPolyData* out = keepLargest->GetOutput();
            if (!out || out->GetNumberOfCells() == 0)
                continue;

            // centroid calculation needs mesh vertex/edges
            vtkSmartPointer<vtkExtractEdges> edge_extractor = vtkSmartPointer<vtkExtractEdges>::New();
            edge_extractor->SetInputData(out);
            edge_extractor->Update();

            vtkPolyData* tmp = edge_extractor->GetOutput();
            tmp->BuildCells();
            tmp->BuildLinks();

            QList<point> punti;
            punti.reserve((int)tmp->GetNumberOfPoints());

            point ptemp;
            for (int id = 0; id < (int)tmp->GetNumberOfPoints(); ++id)
            {
                double* P = tmp->GetPoint(id);
                ptemp.x = P[0];
                ptemp.y = P[1];
                ptemp.z = P[2];
                punti.append(ptemp);
            }

            tmp_p.centroid = centroid(punti);
            punti.clear();

            final_list.append(tmp_p);
        }
    }

    for (int ii = 0; ii < final_list.size(); ++ii)
    {
        std::cout << 1000 * final_list.at(ii).centroid.x << " "
                  << 1000 * final_list.at(ii).centroid.y << " "
                  << 1000 * final_list.at(ii).centroid.z << std::endl;
    }

    emit localizationEnd(final_list);
}

//
//
///**
// * Centroid calculation of a given points cloud
// */
point gLocalize::centroid(QList< point > punti)
{
    int i;
    point centroide;
    centroide.x=0,centroide.y=0,centroide.z=0;
    for ( i=0;i<punti.size();i++){
        centroide.x+=punti.at(i).x/1000;
        centroide.y+=punti.at(i).y/1000;
        centroide.z+=punti.at(i).z/1000;
    };

    centroide.x=centroide.x/punti.size();
    centroide.y=centroide.y/punti.size();
    centroide.z=centroide.z/punti.size();

    return centroide;
}
//
//
///**
// * performs bounding box side difference and return the larger one
// */
double gLocalize::lati(double p[6])
{
    double x=p[1]-p[0];double y=p[3]-p[2];double z=p[5]-p[4];
    double d1=(x-y); double d2=(x-z); double  d3=(y-z);
    if(d1<0) d1=d1*-1;if(d2<0) d2=d2*-1;if(d3<0) d3=d3*-1;

    if(d1>d2 && d1>d3) return d1;
    if(d2>d1 && d2>d3) return d2;
    if(d3>d2 && d3>d1) return d3;
    return 9999;
}


int gLocalize::get_byteorder()
{
    union {
        long l;
        char c[4];
    } test;
    test.l = 1;
    if( test.c[3] && !test.c[2] && !test.c[1] && !test.c[0] )
        return ORDER_BIGENDIAN;

    if( !test.c[3] && !test.c[2] && !test.c[1] && test.c[0] )
        return ORDER_LITTLEENDIAN;

    return ORDER_UNKNOWN;
}

/**
 * Implementation of the Hausdorff distance from a reference 1 cm diameter sphere
 * Uses the current extracted surface polydata (one region).
 */
double gLocalize::hausdorff(vtkPolyData* surface, surf tempsurf)
{
    if (!surface || surface->GetNumberOfPoints() == 0)
        return 1e9; // or 0, but "far" is safer for filtering

    vtkSmartPointer<vtkSphereSource> source_sphere = vtkSmartPointer<vtkSphereSource>::New();
    source_sphere->SetCenter(tempsurf.center[0], tempsurf.center[1], tempsurf.center[2]);
    source_sphere->SetRadius(5.0);
    source_sphere->Update();

    vtkPolyData* sphere = source_sphere->GetOutput();
    sphere->BuildCells();

    // Extract edges from the *surface polydata*, not from a filter output port
    vtkSmartPointer<vtkExtractEdges> edge_extractor = vtkSmartPointer<vtkExtractEdges>::New();
    edge_extractor->SetInputData(surface);
    edge_extractor->Update();

    vtkPolyData* tmp = edge_extractor->GetOutput();
    tmp->BuildCells();
    tmp->BuildLinks();

    QList<point> punti_sup;
    punti_sup.reserve((int)tmp->GetNumberOfPoints());

    point ptemp;
    for (int id = 0; id < (int)tmp->GetNumberOfPoints(); id++) {
        double* p = tmp->GetPoint(id);
        ptemp.x = p[0];
        ptemp.y = p[1];
        ptemp.z = p[2];
        punti_sup.append(ptemp);
    }

    QList<point> punti_sphere;
    punti_sphere.reserve((int)sphere->GetNumberOfPoints());

    for (int id = 0; id < (int)sphere->GetNumberOfPoints(); id++) {
        double* p = sphere->GetPoint(id);
        ptemp.x = p[0];
        ptemp.y = p[1];
        ptemp.z = p[2];
        punti_sphere.append(ptemp);
    }

    int nr_source_points = punti_sup.count();
    int nr_target_points = punti_sphere.count();

    double maxmin = -1e9;

    for (int i = 0; i < nr_source_points; i++) {
        double smallest = 1e9;

        double x1 = punti_sup.at(i).x;
        double y1 = punti_sup.at(i).y;
        double z1 = punti_sup.at(i).z;

        for (int j = 0; j < nr_target_points; j++) {
            double x2 = punti_sphere.at(j).x;
            double y2 = punti_sphere.at(j).y;
            double z2 = punti_sphere.at(j).z;

            double dx = x1 - x2;
            double dy = y1 - y2;
            double dz = z1 - z2;

            double dist = dx*dx + dy*dy + dz*dz; // squared distance

            if (dist < smallest) smallest = dist;
        }

        if (smallest > maxmin) maxmin = smallest; // max of mins
    }

    punti_sphere.clear();
    punti_sup.clear();

    return maxmin; // squared Hausdorff
}