#include "MaxHeap.h"
#include "utils.h"

#define Point(i)    (&(data[i * nFeatures]))


pyWord findFarestPointTo(SearchParams params, pyReal * point) {
    pyReal d, bestDist = 0.0;
    pyWord i, bestIdx = 0 - 1;

    for(i = 0 ; i < params->nPoints ; i ++ ) {
        d = distSquared(point, Point(i));
        if (d > bestDist) {
            bestDist = d;
            bestIdx = i;
        }
    }

    return bestIdx;
}



void NeighborhoodHeuristic(const pyWord nbhSize, const pyWord nPoints, const pyWord nFeatures, const pyReal * data, pyWord * neighborhoods) {
    SearchParams params = {
        .nbhSize = nbhSize,
        .nPoints = nPoints,
        .nFeatures = nFeatures,
        .data = data,
        .neighborhoods = neighborhoods
    };

    int success = 0;
    pyWord i, j;
    pyReal d;
    pyReal lineVector = NULL;
    pyReal accu = NULL;

    if(initNbhHeaps(&params)) {
        return;
    }

    do {

        const pyWord lineStartIdx = findFarestPointTo(params, Point(0));
        if(lineStartIdx >= params->nPoints) {
            break;
        }

        pyReal * lineStartPoint = Point(lineStartIdx);
        const pyWord lineEndIdx = findFarestPointTo(params, lineStartPoint);
        pyReal * lineEndPoint = Point(lineEndIdx);


        pyReal lineVector * = malloc(params->nFeatures * sizeof(pyReal));
        if(!lineVector) {
            break;
        }
        lineVector = vecSub(nFeatures, lineEndPoint, lineStartPoint); 
        d = vecNorm(lineVector);
        if(d == 0.0) {
            break;
        }
        vecScale(nFeatures, lineVector, 1.0 / d, lineVector);


        pyReal accu * = malloc(params->nFeatures * sizeof(pyReal));
        if(!accu) {
            break;
        }

        // Berechne alle Distanzen
        for(i = 0; i < nPoints; i ++) {
            const pyReal *x = Point(i);

            vecSub(nFeatures, accu, x, lineStartPoint);
            d = vecScalarProduct(nFeatures, lineVector, accu);
            vecScale(nFeatures, accu, d, lineVector);
            vecAdd(nFeatures, accu, accu, lineStartPoint);

            const pyReal di1 = sqrt(distSquared(accu, lineStartPoint, nFeatures));
            const pyReal di2 = sqrt(distSquared(accu, x, nFeatures));

            
            for(j = i + 1; j < nPoints; j ++) {
                const pyReal * y = Point(j);
                d = distSquared(x, y, nFeatures);
                
                maxHeap_insert(&(params.nbhHeaps.heaps[i]), j, d);
                maxHeap_insert(&(params.nbhHeaps.heaps[j]), i, d);
            }
        }
    } while(0);

    if(lineVector) {
        free(lineVector);
    }

    if(!success) {
        nbhSearchBruteForce(&params);
    }
    
    freeNbhHeaps(&params);
}