#include "SearchTree.h"
#include "Feature.h"

void SourceFeature::extendFeatureVector(FeatureVector& feature, bool luminanceOnly) const {
    // Extract the features.  The order is chosen to match the order of computations in 
    // distanceSquared.

    feature.append(derivative.x);
    feature.append(derivative.y);

    feature.append(color.y);
    if (! luminanceOnly) {
        feature.append(color.i);
        feature.append(color.q);
    }
}

//extern bool PRINT_DISTANCE;
float SourceFeature::distanceSquared(const SourceFeature& s, bool luminanceOnly) const {
    if (luminanceOnly) {
        /*
        if (PRINT_DISTANCE) {
            printf("dx: (%f - %f)^2 + \n", derivative.x, s.derivative.x);
            printf("dy: (%f - %f)^2 + \n", derivative.y, s.derivative.y);
            printf("Y:  (%f - %f)^2 + \n", color.y, s.color.y);
        }
        */
        return (derivative - s.derivative).squaredLength() + square(color.y - s.color.y);
    } else {
        return (derivative - s.derivative).squaredLength() + color.distanceSquared(s.color);
    }
}
