// OpenCV Face Detection Application: FaceDetect.h
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <limits.h>
#include <time.h>
#include <ctype.h>
#include <iostream>

// Include OpenCV header files
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>

#include "DualCoding/DualCoding.h"
#include "Behaviors/StateMachine.h"
#include "Events/EventRouter.h"

#include "Motion/MMAccessor.h"
#include "Motion/PIDMC.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/Kinematics.h"
#include "Motion/PostureMC.h" 
#include "IPC/SharedObject.h"
#include "Shared/Config.h"
#include "Shared/ERS7Info.h"
#include "Shared/fmat.h"
#include "Shared/WorldState.h"

// these are for drawing into the camera frame
#include "Shared/ProjectInterface.h"
#include "Vision/Graphics.h"
#include "Events/FilterBankEvent.h"
#include "Vision/RawCameraGenerator.h"
#include "Behaviors/Mon/RawCamBehavior.h"

using namespace DualCoding;

// Predefined Tekkotsu "Raw Cam" image width and height
#define imgHeight 320
#define imgWidth 240

// Create memory storage for Haar features used in calculations
static CvMemStorage* storage = 0;

// Create global Haar classifier
static CvHaarClassifierCascade* cascade = 0;

// File path for Haar cascade containing faces
const char* cascade_name = "/home/student/project/haarcascade_frontalface_alt.xml";

/* Demo program for face detection */
class FaceDetect : public StateNode, public VRmixin {
 public:  
  class FindFace : public StateNode, public VRmixin {
  public:
  FindFace() : StateNode("FindFace"), VRmixin() {}
    virtual void DoStart() {
      
      // Load the HaarClassifierCascade
      cascade = (CvHaarClassifierCascade*)cvLoad( cascade_name, 0, 0, 0 );
      
      // Check whether the cascade has loaded successfully. Else report an error and post completion.
      if( !cascade ) {
	fprintf( stderr, "ERROR: Could not load classifier cascade\n" );
	postStateCompletion();
      }
      
      // Allocate the memory storage
      storage = cvCreateMemStorage(0);
      
      // Listen to visRawCameraEGID to draw a box on the camera delimiting area of face
      erouter->addListener(this,EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::statusETID);  
    }
    
    void processEvent(const EventBase& e){
      if(e.getGeneratorID() == EventBase::visRawCameraEGID) {
	//*****************************************************//
	//*** Draw into camera frame to put box around face ***//
	//*****************************************************// 
	
	/* Image stored for processing.  The openCV image    *
	 * corresponds to camera's width and height.  Image  *
	 * must be released after being processed.           */
	IplImage *image = cvCreateImage(cvSize(imgHeight, imgWidth), IPL_DEPTH_8U, 3);
	
	// Clear the memory storage which was used before
	cvClearMemStorage( storage );
	
	// Create the YUV image from camera frame
	Sketch<yuv> yuvImg(sketchFromYUV());
	
	IplImage *temp = cvCreateImage(cvSize(imgHeight, imgWidth), IPL_DEPTH_8U, 3);

	// Convert YUV image to IplImage
	memcpy(temp->imageData, yuvImg->getRawPixels(), imgHeight * imgWidth * 3);

	cvCvtColor(temp, image, CV_YCrCb2RGB);
	
	/*****************************************************/
	/*** Detect faces in image and draw bounding boxes ***/
	/*****************************************************/
	
	// Check whether the cascade is loaded
	if( cascade ){
	  // Detect the faces and store them in the sequence
	  CvSeq* faces = cvHaarDetectObjects( image, cascade, storage,
					      1.1, 2, CV_HAAR_DO_CANNY_PRUNING,
					      cvSize(40, 40) );
	  if(faces->total == 0)
	    cout << "Did not find any face.\n" << endl;
	  else {
	    // Loop the number of faces found.
	    for(int i = 0; i < (faces ? faces->total : 0); i++ ) {
	      cout << "Found a face in the image." << endl;
	      // Create a new rectangle for drawing the face
	      CvRect* r = (CvRect*)cvGetSeqElem( faces, i );
	      
	      cout << "Printing coordinates of the rectangle: " << endl;
	      cout << "X coord: " << r->x << " Y coord: " << r->y << endl;
	      cout << " width: " << r->width << " height: " << r->height << endl;
	      
	      const FilterBankEvent& fbe = dynamic_cast<const FilterBankEvent&>(e);
	      unsigned chan=RawCameraGenerator::CHAN_Y;
	      unsigned int layer=RawCamBehavior::getSourceLayer(chan,fbe.getNumLayers());
	      Graphics g(*fbe.getSource(), layer, chan);
	      g.setColor(255);
	      g.drawRect(r->x,r->y,r->width,r->height);
	    }
	  }
	}
	
	// Release image memory
	cvReleaseImage( &image );
      }
    }
  };

 FaceDetect() : StateNode("FaceDetect"), VRmixin() {}
  virtual void setup() {
    
#statemachine
    
  startnode: FindFace()
      
#endstatemachine
      
      }
};