#include "OctreeSupport.hpp"
#include "Octree.hpp"

#include <cmath>

#include "genFilterDefs.h"

#include "TerrainMapOctree.h"

double
TerrainMapOctree::
GetRangeError(double& mapVariance, const double* const startPoint, const double* const directionVector, double expectedDistance) {
	Vector octreeVectorStartPoint (startPoint[0], startPoint[1], startPoint[2]);
	Vector octreeVectorDirectionVector (directionVector[0], directionVector[1], directionVector[2]);
	
	//TODO work out variance properly 
	mapVariance = OctreeMap.GetTrueResolution().Norm()/1.0;//1.73;//3.4641 is 2*sqrt(3)
	
	
	double predictedDistance = OctreeMap.RayTrace(octreeVectorStartPoint, octreeVectorDirectionVector);
	if(predictedDistance == -1){
		//missed the map
		return NAN;
	}
	return expectedDistance - predictedDistance;
}
/*
double
TerrainMapOctree::
QueryMap(const double* const queryPoint) {
	Vector queryPoint = 
	//return static_cast<double>(OctreeMap.Query(queryPoint));
	return OctreeMap.InterpolatingQuery(queryPoint);
}
*/


TerrainMapOctree::
TerrainMapOctree(const char* mapName) {
	//OctreeMap = Octree<PlanarFitNode>();
	OctreeMap = Octree<bool>();
	if(!OctreeMap.LoadFromFile(mapName)){
		printf("\nOctree Load Failed\n");
		exit(1);
	}
}
TerrainMapOctree::
~TerrainMapOctree() {
}

bool
TerrainMapOctree::
withinRefMap(const double northPos, const double eastPos){
	Vector LowerBounds = OctreeMap.GetLowerBounds();
	Vector UpperBounds = OctreeMap.GetUpperBounds();
	
	return ((northPos < UpperBounds.x)
		&& (northPos > LowerBounds.x)
		&& (eastPos < UpperBounds.y)
		&& (eastPos > LowerBounds.y));
}

bool 
TerrainMapOctree::
withinValidMapRegion(const double northPos, const double eastPos){
	return withinRefMap(northPos, eastPos);
}

bool 
TerrainMapOctree::
withinSubMap(const double northPos, const double eastPos){
	return withinRefMap(northPos, eastPos);
}

bool 
TerrainMapOctree::
GetMapT(mapT& currMap){
	printf("\n\nGood news everyone, GetMapT isn't written yet for Octrees\n\n");
	return false;
}

bool 
TerrainMapOctree::
GetMapBounds(double* currMapBounds){
	printf("\n\nGood news everyone, GetMapBounds isn't written yet for Octrees\n\n");
	return false;
}