#include "arFakeMarker.h"
#include "ARToolKitPlus/TrackerSingleMarkerImpl.h"


//-----------------------------------------------------------------------
arFakeMarker::arFakeMarker(){
	isSet		= false;
	marker_info = NULL;
	tracker		= NULL;
}
	
//-----------------------------------------------------------------------
arFakeMarker::~arFakeMarker(){
	if(marker_info != NULL)delete[] marker_info;
	if(tracker != NULL)delete tracker;
}

//-----------------------------------------------------------------------
void arFakeMarker::setup(int cam_w, int cam_h, int markerSizeMM, bool useRobustPose){

	cameraW = cam_w;
	cameraH = cam_h;
	markerW = markerSizeMM;
		
	marker_info = new ARMarkerInfo[1];
	
	// ----------------------------------  AR TK+ STUFF - ripped from the single marker demo app

    // create a tracker that does:
    //  - 6x6 sized marker images
    //  - samples at a maximum of 6x6
    //  - works with luminance (gray) images
    //  - can load a maximum of 1 pattern
    //  - can detect a maximum of 8 patterns in one image
   tracker = new ARToolKitPlus::TrackerSingleMarkerImpl<6,6,6, 1, 8>(cameraW, cameraH);

	const char* description = tracker->getDescription();
	printf("ARToolKitPlus compile-time information:\n%s\n\n", description);

    // set a logger so we can output error messages
    //tracker->setLogger(&logger);
	
	tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);	
		
    if( !tracker->init( (const char *)ofToDataPath("LogitechPro4000.dat").c_str(), 1.0f, 1000.0f) )            // load std. ARToolKit camera file
	{
		printf("ERROR: init() failed\n");
		delete tracker;
		return;
	}

    // define size of the marker
    tracker->setPatternWidth(markerW);

	//none of this stuff really matters
    tracker->setBorderWidth(useBCH ? 0.125f : 0.250f);
    tracker->activateAutoThreshold(true);
	useBCH = true;

    // let's use lookup-table undistortion for high-speed
    // note: LUT only works with images up to 1024x1024
    tracker->setUndistortionMode(ARToolKitPlus::UNDIST_LUT);

    // RPP is more robust than ARToolKit's standard pose estimator
    if( useRobustPose )tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP);

    // switch to simple ID based markers
    // use the tool in tools/IdPatGen to generate markers
    tracker->setMarkerMode(useBCH ? ARToolKitPlus::MARKER_ID_BCH : ARToolKitPlus::MARKER_ID_SIMPLE);
}


//------------------------------------------
void arFakeMarker::setMarkerPoints(ofPoint p1, ofPoint p2, ofPoint p3, ofPoint p4){

	makeMarker(p1, p2, p3, p4, &marker_info[0]);	
	tracker->calcFromMarker(&marker_info[0]);

	isSet = true;
}

//------------------------------------------
string arFakeMarker::getMarkerInfoString(){
	if( !isSet )return "";
	
	string dump = "";
	dump += "area: "+ofToString(marker_info[0].area) + "\n";
	dump += "id: "+ofToString(marker_info[0].id) + "\n";
	dump += "dir: "+ofToString(marker_info[0].dir) + "\n";
	dump += "cf: "+ofToString(marker_info[0].cf) + "\n";
	dump += "pos: "+ofToString(marker_info[0].pos[0]) + " " + ofToString(marker_info[0].pos[1])+"\n";
	dump += "line: \n";
			
	for(int i = 0; i < 4; i++){
		dump += "   "+ofToString(marker_info[0].line[i][0]) + " " + ofToString(marker_info[0].line[i][1]) +  " " + ofToString(marker_info[0].line[i][2])+"\n";
	}
	dump += "\n";

	dump += "vertex: \n";

	for(int i = 0; i < 4; i++){
		dump += "   "+ofToString(marker_info[0].vertex[i][0]) + " " + ofToString(marker_info[0].vertex[i][1]) + "\n";
	}
	dump += "\n";

	return dump;
}

//------------------------------------------
bool arFakeMarker::isMarkerSet(){
	return isSet;
}

//------------------------------------------
void arFakeMarker::loadARMatrix(){
	if(!isSet)return;

	glViewport(0, 0, cameraW, cameraH);
	glMatrixMode( GL_PROJECTION );
	glLoadMatrixf(tracker->getProjectionMatrix());
	glMatrixMode( GL_MODELVIEW );
	glLoadMatrixf(tracker->getModelViewMatrix());
}

//------------------------------------------
void arFakeMarker::unloadARMatrix(){
	if(!isSet)return;

	ofSetupScreen();
}
 




//static utils are at the bottom of this file
//figure out the equation of a line described by two points
static void lineEquation(float x1, float y1, float x2, float y2, float &a, float &b, float &c){
	//   ax	     +    by      +      c		    = 0
	//(y1 - y2)x + (x2 - x1)y + (x1*y2 - x2*y1) = 0
	
	a = y1-y2;
	b = x2-x1;
	c = x1*y2 - x2*y1;
	
}

static float areaOfAQuad(ofPoint p1, ofPoint p2, ofPoint p3, ofPoint p4){
	return 1/2*((p2.y-p4.y)*p1.x+(p3.y-p1.y)*p2.x+(p4.y-p2.y)*p3.x+(p1.y-p3.y)*p4.x);
}


static void makeMarker(ofPoint p1, ofPoint p2, ofPoint p3, ofPoint p4, ARMarkerInfo * marker){

		
		//area of the marker is
		marker->area = areaOfAQuad(p1, p2, p3, p4);	

		//id is one for now
		marker->id = 1;
		
		//direction is 0 for now
		//we give vertexes in clockwise order
		marker->dir	= 0;

	    //cf is one :)
		marker->cf = 1;
		
		//center of mass of the marker
		ofPoint center;
		center.x += p1.x;
		center.y += p1.y;
		center.x += p2.x;
		center.y += p2.y;
		center.x += p3.x;
		center.y += p3.y;
		center.x += p4.x;
		center.y += p4.y;	
		
		center.x /= 4;
		center.y /= 4;
	
		marker->pos[0] = center.x;
		marker->pos[1] = center.y;
		
		//now lets do the lines 
		float a, b, c;
		
		lineEquation(p1.x, p1.y, p2.x, p2.y, a, b, c);
		marker->line[0][0] = a; 
		marker->line[0][1] = b; 
		marker->line[0][2] = c; 

		lineEquation(p2.x, p2.y, p3.x, p3.y, a, b, c);
		marker->line[1][0] = a; 
		marker->line[1][1] = b; 
		marker->line[1][2] = c; 		

		lineEquation(p3.x, p3.y, p4.x, p4.y, a, b, c);
		marker->line[2][0] = a; 
		marker->line[2][1] = b; 
		marker->line[2][2] = c; 

		lineEquation(p4.x, p4.y, p3.x, p3.y, a, b, c);
		marker->line[3][0] = a; 
		marker->line[3][1] = b; 
		marker->line[3][2] = c; 
		
		for(int i = 0; i < 4; i++){
			float largest = MAX( fabs(marker->line[i][0]), fabs(marker->line[i][1]) );
			marker->line[i][0] /= largest;
			marker->line[i][1] /= largest;
			marker->line[i][2] /= largest;
			
		}
		
		//now lets do the vertexes
		marker->vertex[0][0] = p1.x;
		marker->vertex[0][1] = p1.y;

		marker->vertex[1][0] = p2.x;
		marker->vertex[1][1] = p2.y;
		
		marker->vertex[2][0] = p3.x;
		marker->vertex[2][1] = p3.y;

		marker->vertex[3][0] = p4.x;
		marker->vertex[3][1] = p4.y;								
	
}







