#ifndef _LASER_TRACKING_H
#define _LASER_TRACKING_H

#include "ofMain.h"
#include "ofAddons.h"
#include "guiQuad.h"

#include "laserUtils.h"
#include "baseGui.h"
#include "coordWarping.h"
#include "hitZone.h"

//our other app objects

class laserTracking : public baseGui{


	public:
	
		//---------------------------
		laserTracking(){
		
			bCameraSetup 	= false;
			bVideoSetup  	= false;
			newStroke		= true;
			shouldClear		= false;
			noLaserCounter 	= 0;
			distDifference  = 0.0;
			smoothPos.x 	= 0;
			smoothPos.y 	= 0;
			smoothVel.x 	= 0;
			smoothVel.y 	= 0;
			distX			= 0;
			distY			= 0;
			oldX			= 0;
			oldY			= 0;
			
			
			//we do all our tracking at 320 240 - regardless 
			//of camera size - larger cameras get scaled down to
			//these dimensions - otherwise 'shit would be slow'
			pre				= new unsigned char[320 * 240 * 3];
			
			
			//this should read the settings from a file!
			setupCalibrationPoints();
		}
		

		//these are the points
		//for the callibration quads
		//the dst points are projected
		//and then compared to the percieved 
		//src points from the camera
		//----------------------------		
		void setupCalibrationPoints(){
									
			calSrc[0].x = 0;
			calSrc[0].y = 0;
			
			calSrc[1].x = 1.0;
			calSrc[1].y = 0;
			
			calSrc[2].x = 1.0;
			calSrc[2].y = 1.0;

			calSrc[3].x = 0;
			calSrc[3].y = 1.0;
			
			
			//set to something a little spread out
			//but always likely to be seen on the surface
			
			calDst[0].x = 0.2;
			calDst[0].y = 0.2;
			
			calDst[1].x = 0.8;
			calDst[1].y = 0.2;
			
			calDst[2].x = 0.8;
			calDst[2].y = 0.8;

			calDst[3].x = 0.2;
			calDst[3].y = 0.8;
			
		}		
		
		//
		//---------------------------
		void setClearZone(float x, float y, float w, float h){
			clearZone.setPosition(x/320.0, y/240.0);
			clearZone.setDimensions(w/320.0, h/240.0);
		}
		
		//
		//---------------------------
		void setUseClearZone(bool useClearZone){
			clearZone.setActive(useClearZone);
		}
		
		//---------------------------
		bool isClearZoneHit(){
			if( shouldClear ){
				shouldClear = false;
				return true;
			}
			return false;
		}
				
		//changing cameras or switching from/to the camera mode
		//requires the app to be restarted - mabe we can change this?
		//---------------------------		
		void setupCamera(int deviceNumber, int width, int height){
			VG.setDeviceID(deviceNumber);
			VG.initGrabber(width, height);
			W = VG.width;
			H = VG.height;			
			bCameraSetup = true;
			noLaserCounter  = 0;
			distDifference  = 0.0;
		}
		
		//same as above - from/to using test movies
		//requires a restart
		//---------------------------		
		void setupVideo(string videoPath){
			VP.loadMovie(videoPath);
			VP.play(); 
			VP.setUseTexture(true);
			W = VP.width;
			H = VP.height;
			bVideoSetup = true;
			noLaserCounter = 0;
			distDifference  = 0.0;
		}
		
		//good for adjusting the color balance, brightness etc
		//---------------------------		
		void openCameraSettings(){
			VG.videoSettings();			
		}
		
		//hooked up to a key press when the laser pointer appears 
		//on one of the callibration points set the percieved
		//point to be at this location - not sure if this is working
		//100%, seems that the distortion goes wonky outside of the points
		//---------------------------
		void setCallibrationPoint(int which){
			
			/*
				1+++++++2
				+       +
				+       +
				+       +
				4+++++++3
			
			*/
			
			if(which >= 1 && which <= 4){
				calSrc[which-1].x = laserX;
				calSrc[which-1].y = laserY; 
			}
		}	
		
		//do all our openCV allocation
		//and loads our quad settings from xml
		//---------------------------		
		void setupCV(string filePath){
						
			//our openCV inits
			VideoFrame.allocate(W, H);
			WarpedFrame.allocate(320, 240);
			PresenceFrame.allocate(320, 240);
			
			//this is so we can select a sub region of the 
			//camera image and warp it to full 320 by 240
			warpSrc[0].x = 0;
			warpSrc[0].y = 0;
			
			warpSrc[1].x = 1.0;
			warpSrc[1].y = 0;
			
			warpSrc[2].x = 1.0;
			warpSrc[2].y = 1.0;

			warpSrc[3].x = 0;
			warpSrc[3].y = 1.0;
			
			//Now we update out quad with the default
			//values and if then if the file is found
			//we overwrite with values from xml file
			QUAD.setup("QUAD_SRC_");
			QUAD.setQuadPoints(warpSrc);
			QUAD.readFromFile(filePath);
			
			//warp dst will always be to 320 by 240
			//unless you want shit slow!
			warpDst[0].x = 0;
			warpDst[0].y = 0;
			
			warpDst[1].x = 320;
			warpDst[1].y = 0;
			
			warpDst[2].x = 320;
			warpDst[2].y = 240;

			warpDst[3].x = 0;
			warpDst[3].y = 240;	
		}
		
		//the heart of the beast - where we process 
		//the incoming frame and look for a laser
		//---------------------------		
		void processFrame(float hue, float hueThresh, float sat, float value, int minSize, int deadCount, float jumpDist){
						
			///////////////////////////////////////////////////////////
			// Part 1 - get the video data
			///////////////////////////////////////////////////////////
			
			//pointer to our incoming video pixels			
			unsigned char * pixCam;
			
			//either grab pixels from video or grab from camera
			if(bVideoSetup){
				VP.idleMovie();	
				pixCam 	= VP.getPixels();
			}else{
				VG.grabFrame();
				pixCam 	= VG.getPixels();		
			}
			
			///////////////////////////////////////////////////////////
			// Part 2 - warp the video based on our quad
			///////////////////////////////////////////////////////////
			
			//add to openCV and warp to our dst image
			VideoFrame.setFromPixels(pixCam, W, H);
			WarpedFrame.warpIntoMe(VideoFrame, QUAD.getScaledQuadPoints(W,H), warpDst);

			
			///////////////////////////////////////////////////////////
			// Part 3 - convert to hsv and search for matching pixels
			////////////////////////////////////////////////////////////
						
			//Get pixels and convert to hue sat and value;
			hsvFrame = WarpedFrame;
			hsvFrame.rgbToHsv();
			
			//okay time to look for our laser!
			//based on hue sat and val
			int totalPixels = 320*240*3;
			
			unsigned char * pix = hsvFrame.getPixels();
						
			float h 		= hue 		* 255;
			float ht		= hueThresh * 255; 
			float s 		= sat 		* 255;
			float v 		= value 	* 255;
			
			//here we figure out what is the max allowed hue 
			//and the minimum allowed hue. because hue is 
			//continious we have to make sure we handle what happens 
			//if hueMax goes over 255
			//or hueMin goes less than 0
							
			float hueMax	= h + ht*0.5;
			float hueMin    = h - ht*0.5;
				
			int k = 0;
								
			for(int i = 0; i < totalPixels; i+=3){
			 
			  float pixHue = pix[i];
			           
			  if( pix[i+1] >= s  && pix[i+2] >= v){
			 
			     //we do this to check the cases when the
			     //hue min could have wrapped
			     //or the hue max could have wrapped
			     //also if saturation is zero
			     //then hue doesn't matter hence (s == 0)               
			     if( (s == 0) || (pixHue >= hueMin && pixHue <= hueMax) ||
			        (pixHue -255 >= hueMin && pixHue -255 <= hueMax) ||
			        (pixHue +255 >= hueMin && pixHue +255 <= hueMax) ){
			       
			        //we have a white pixel
			        pre[k] = 255;
			     }
			     else pre[k] = 0;
			  }else pre[k] = 0;
			           
			  k++;
			} 			
			
			///////////////////////////////////////////////////////////
			// Part 4 - find the largest blob of possible candidates 
			////////////////////////////////////////////////////////////
							
			//lets find blobs!
			PresenceFrame.setFromPixels(pre, 320, 240);
			
			//max blob size - our laser shouldn't be bigger
			//than 20 pixels by 20 pixels right?
			//so lets make our max blob size 400
	
			int maxSize = 400;		
			Contour.findContours(PresenceFrame, minSize, maxSize, 50, false);  
			
			///////////////////////////////////////////////////////////
			// Part 5 - finally calculate our laser coordinates
			////////////////////////////////////////////////////////////
										
			//okay so we found some blobs that matched our criteria
			//we are only really interested in the largest one
			if(Contour.nBlobs > 0){
				
				//get the center pos of the largest blob in 0 - 1 range
				float tmpX		= Contour.blobs[0].centroid.x/320;
				float tmpY		= Contour.blobs[0].centroid.y/240;
				
				//calculate the horizontal and vertical distance 
				//between the last point
				oldX			= laserX;
				oldY			= laserY;
				
				distX			= tmpX - laserX;
				distY			= tmpY - laserY;
				
				distDifference  = sqrt( pow(distX, 2) + pow(distY, 2) );
				
				//now update our laser position with this new position
				laserX 			= tmpX;
				laserY			= tmpY;
				newPos 			= true;
				noLaserCounter 	= 0;
				
			}else{
				//we are waiting for a laser!
				newPos = false;
				noLaserCounter++;
				
				distX			= 0;
				distY			= 0;			
			}
			
			///////////////////////////////////////////////////////////
			// Part 6 - calculate related variables, smoothing etc
			////////////////////////////////////////////////////////////

			// we need to store if a new stroke has occured
			// this is so we can no when to start a new line 
			if(!newStroke) newStroke = (noLaserCounter >= deadCount) ||  (distDifference >= jumpDist);

			if(newStroke){
			
				if(clearZone.getActive() && clearZone.isHit(laserX, laserY)){
					newPos 		= false;
					shouldClear = true;
				}
			
				oldX		= laserX;
				oldY		= laserY;
				smoothPos.x = laserX;
				smoothPos.y = laserY;
				distX		= 0;
				distY		= 0;
				smoothVel.x = 0;
				smoothVel.y = 0;
			}else{
				
				//here is some code that attempts to smooth the 
				//path a little using the new point as an attractor
				
				//so lets calculate for the smoothed brush
				//the distance between the laser and the 
				//last smoothed position
				float dx 	= laserX - smoothPos.x;
				float dy 	= laserY - smoothPos.y;
				float dz 	= sqrt( pow(dx, 2) + pow(dy, 2) );
				float total =  abs(dx) + abs(dy) + dz;
				
				//here we normalise the value
				float nx = 0;
				float ny = 0;
				
				//no divde by zero :)
				if(total > 0){
					nx = dx / total;
					ny = dy / total;
				}
				
				//friction / drag -- need to hookup to variables
				smoothVel.x *= 0.2;
				smoothVel.y *= 0.2;
				
				//make sure it is not too close otherwise we get an orbit situation	
				if(dz > 0.04){
					smoothVel.x += nx * 0.13;
					smoothVel.y += ny * 0.13;
					
					smoothPos.x += smoothVel.x;
					smoothPos.y += smoothVel.y;	
				}
				
				//finally we don't want it going outside of the image!!!
				if(smoothPos.x < 0) smoothPos.x = 0;
				if(smoothPos.y < 0) smoothPos.y = 0;
				if(smoothPos.x > 1) smoothPos.x = 1;
				if(smoothPos.y > 1) smoothPos.x = 1;
				
			}
			
			///////////////////////////////////////////////////////////
			// Part 7 - calculate our distorted laser coordinates
			////////////////////////////////////////////////////////////
			
			//Here is where we do our coordinate transformation
			//using a bit of openCv and a bit of myler & weeks
			//see the coordWarping class for more info
			CW.calculateMatrix(calSrc, calDst);
			outPoints = CW.transform(laserX, laserY);
			
		} 
		
		
		//---------------------------
		bool newData(){
			return newPos;
		}
		
		//---------------------------
		void clearNewStroke(){
			newStroke = false;
		}
		
		//---------------------------
		bool isStrokeNew(){
			return newStroke;
		}
	
		
		//this formats the x and y coordinates
		//to a string that can be sent to other apps
		//over tcp and udp we will see both regular
		//and transformed coordinates
		//---------------------------		
		string getLaserPointString(){
			
			if( newData() ){
				sendStr = ofToString(laserX);
				sendStr += "!"+ ofToString(laserY);
				sendStr += "@"+ ofToString(outPoints.x);
				sendStr += "!"+ ofToString(outPoints.y); 
			}
			
			return sendStr;
		}


		//---------------------------		
		void calcColorRange(float hue, float hueThresh, float sat, float value){
			
			//ehh this is kind of ugly and needs to be fixed I think
			//Lets calc the rgb values for the min and max hue range			
			float rgb[3];
			float hsb[3];
			
			hsb[0] = hue-hueThresh*0.5;
			hsb[1] = sat;
			hsb[2] = value;
			
			LU.hsbToRgb(hsb, rgb);
			
			r0Min = (int)(rgb[0]*255);
			g0Min = (int)(rgb[1]*255);
			b0Min = (int)(rgb[2]*255);
			
			hsb[0] = hue+hueThresh*0.5;
			hsb[1] = sat;
			hsb[2] = value;
					
			LU.hsbToRgb(hsb, rgb);
								
			r0Max = (int)(rgb[0]*255);
			g0Max = (int)(rgb[1]*255);
			b0Max = (int)(rgb[2]*255);
			
			hsb[0] = hue-hueThresh*0.5;
			if(sat == 0) hsb[1] = 0.0;
			else hsb[1] = 1.0;			
			hsb[2] = 1.0;
			
			LU.hsbToRgb(hsb, rgb);
			
			r1Min = (int)(rgb[0]*255);
			g1Min = (int)(rgb[1]*255);
			b1Min = (int)(rgb[2]*255);
			
			hsb[0] = hue+hueThresh*0.5;
			if(sat == 0) hsb[1] = 0.0;
			else hsb[1] = 1.0;	
			hsb[2] = 1.0;
					
			LU.hsbToRgb(hsb, rgb);
								
			r1Max = (int)(rgb[0]*255);
			g1Max = (int)(rgb[1]*255);
			b1Max = (int)(rgb[2]*255);
			
		}
		
		
		//---------------------------				
		void draw(float x, float y){
			
			//lets draw our openCV frames
			//with a nice border around them
			ofNoFill();
			ofSetColor(0xFFFFFF);	
			VideoFrame.draw(0, 0, 320, 240);
			ofRect(0,0,320,240);
			WarpedFrame.draw(320, 0, 320, 240);
			ofRect(320,0,320,240);
			PresenceFrame.draw(640,0, 320, 240);
			ofRect(640,0,320,240);
			ofFill();
			
			//lets draw the contours to show our blobs!
			glPushMatrix();
				glTranslatef(320, 0, 0);
				ofSetColor(0xFF00FF);
				Contour.draw();
			glPopMatrix();
			
			if(clearZone.getActive())drawClearZone(320, 0, 320, 240);
			
			//lets dtaw our camera quad
			QUAD.draw(0,0,320,240);
			
			//lets show what our transformed laser pos is
			ofSetColor(0xFF00FF);
			ofRect(320 + outPoints.x*320.0, outPoints.y*240.0, 8, 8);
			
			//lets draw the computed callibration points
			ofSetColor(0x00FFFF);
			for(int i = 0; i < 4; i ++){
				ofRect(320+calSrc[i].x*320, calSrc[i].y*240, 3, 3);
			}
			
			//and the projected callibration points
			//ofSetColor(0xFFFF00);
			//for(int i = 0; i < 4; i ++){
			//	ofRect(320+calDst[i].x*320, calDst[i].y*240, 3, 3);
			//}		
			
		}


		//---------------------------		
		void drawColorRange(float x, float y, float w, float h){

			//this is not super accurate
			//but good for an indication
			glBegin(GL_QUADS);
				ofSetColor(r1Min, g1Min, b1Min);
				glVertex2f(x,  y);
				
				ofSetColor(r1Max, g1Max, b1Max);				
				glVertex2f(x+w, y);
				
				ofSetColor(r0Max, g0Max, b0Max);
				glVertex2f(x+w, y + h );

				ofSetColor(r0Min, g0Min, b0Min);
				glVertex2f(x, y + h);
			glEnd();
		
			
			glBegin(GL_LINE_LOOP);
			ofSetColor(0xFFFFFF);
				glVertex2f(x, y);
				glVertex2f(x + w, y);
				glVertex2f(x + w, y + h);
				glVertex2f(x  ,   y + h);
			glEnd();

		}
		
		//---------------------------		
		void drawCallibrationMarkers(float x, float y, float w, float h){
			ofSetColor(0x0000FF);
			for(int i = 0; i < 4; i ++){
				ofCircle(x + w*calDst[i].x, y + h*calDst[i].y, 10);
			}	
		}
		
		//---------------------------
		void drawClearZone(float x, float y, float w, float h){
			ofNoFill();
			ofSetColor(0xFF0000);
			
			float xdraw = x + (clearZone.zoneDimensions[0].x * w);
			float ydraw = y + (clearZone.zoneDimensions[0].y * h);
			float wdraw =      clearZone.zoneDimensions[1].x * w;
			float hdraw =      clearZone.zoneDimensions[1].y * h;

			ofRect(xdraw, ydraw, wdraw, hdraw); 				
			ofFill();
		}
		
		
		
		//---------------------------		
		void drawQuadSetupImage(float x, float y, float w, float h){
			ofSetColor(0xFFFFFF);	
			ofRect(0,0,w, h);
			VideoFrame.draw(16, 12, w-32, h-24);
			ofSetColor(0xFFFF00);	
			QUAD.draw(16, 12, w-32, h-24);
		}
		
		//---------------------------		
		//---------------------------		
		
		ofVideoPlayer  	VP;
		ofVideoGrabber 	VG;
		guiQuad			QUAD;
		laserUtils		LU;
		coordWarping	CW;
		
		ofPoint2f 			warpSrc[4];
		ofPoint2f 			warpDst[4];
		
		ofPoint2f 			calSrc[4];
		ofPoint2f 			calDst[4];		
		
		ofPoint2f			smoothPos;
		ofPoint2f			smoothVel;
		ofPoint2f 			outPoints;
		
		ofCvColorImage  	VideoFrame;
		ofCvColorImage 		WarpedFrame;
		ofCvColorImage 		hsvFrame;
		ofCvGrayscaleImage 	PresenceFrame;
		ofCvContourFinder	Contour;
		
		unsigned char * pre;
		
		string sendStr;
		
		bool bCameraSetup, bVideoSetup, newPos, newStroke, shouldClear;
		
		int W;
		int H;
		int noLaserCounter;
		float laserX;
		float laserY;
		float oldX;
		float oldY;
		float distX;
		float distY;
		float distDifference;
		
		hitZone clearZone;
		hitZone brushZone;
		
		int r0Min, r0Max, g0Min, g0Max, b0Min, b0Max, r1Min, r1Max, g1Min, g1Max, b1Min, b1Max;
};

#endif