#include "debug.h"
#include <algorithm>

#include "ConfigFile\ConfigFile.h"

#include "ARAnalyse.h"
extern char* narrow( const std::wstring& str );
extern ConfigFile *g_config;
extern std::wstring g_camera_name;
extern int g_camera_instance;
extern int g_ThresholdMin;
extern int g_ThresholdMax;
extern bool g_read_calibrationfile;

// Filtrage regexp IVY !!! a mettre en concordance avec les SendMsg effectué !!!
const char *ivy_filters[] = {"MarkerAdd", "MarkerMove", "MarkerMoveCalibrated", "MarkerRemove" };

//-------------------------------------------------------------------
//  constructor
//-------------------------------------------------------------------

ARAnalyse::ARAnalyse() :
	m_calibration(NULL),
	m_tracker(NULL)
{
	MarkerMoveThreshold = 0.001;
	MarkerAbsenceCount = 10;
	MarkerWidth = 20;
	m_calibrated = false;
	m_calibration_enabled = false;
	FrameCount = 0;
	bus = NULL;
	m_gray = NULL;
	m_blur0 = NULL;
	m_blur1 = NULL;
	DownSample = 8;
	BlurPass = 2;
	Display = ARAnalyse::Diff;
	filter = ARAnalyse::FMoyen;

	MarkerWidth = g_config->read<double>("MarkerWidth");
	MarkerMoveThreshold = g_config->read<double>("MarkerMoveThreshold");
	MarkerAbsenceCount = g_config->read<double>("MarkerAbsenceCount");
	MarkerSeekingThreshold = g_config->read<double>("MarkerSeekingThreshold");
	CameraParameterFile = g_config->read<string>("CameraParameterFile","data\\no_distortion.cal");
	MarkerSetConfigFile = g_config->read<string>("MarkerSetConfigFile", "data\\One.cfg");
	CalibrationFile = g_config->read<string>("CalibrationFile", "data/savedCalibration.dat");
	MarkerBorderPercentage = g_config->read<float>("MarkerBorderPercentage",0.125f);
    MarkerSeekingThreshold = g_config->read<int>("MarkerSeekingThreshold",160);
	MarkerType =  (ARToolKitPlus::MARKER_MODE) g_config->read<int>("MarkerTraditionalOrSimpleOrBCH",ARToolKitPlus::MARKER_ID_BCH);
	
	m_calibration = new Calibration(1.0, 1.0);
	// intialize Calibration
	int id;
	id = g_config->read<int>("CalibrationID_TopLeft");
    m_IdToCorner[id] = Calibration::TopLeft;
	m_calibration_ids.insert( id );
	id = g_config->read<int>("CalibrationID_TopRight");
    m_IdToCorner[id] = Calibration::TopRight;
	m_calibration_ids.insert( id );
	id = g_config->read<int>("CalibrationID_BottomLeft");
    m_IdToCorner[id] = Calibration::BottomLeft;
	m_calibration_ids.insert( id );
	id = g_config->read<int>("CalibrationID_BottomRight");
    m_IdToCorner[id] = Calibration::BottomRight;
	m_calibration_ids.insert( id );
	if ( g_read_calibrationfile )
	{
		m_calibrated = m_calibration->RestoreCalibration( CalibrationFile );
	}
}

//-------------------------------------------------------------------
//  destructor
//-------------------------------------------------------------------

ARAnalyse::~ARAnalyse()
{
	if ( m_calibration ) delete m_calibration;
	if ( bus )
	{
		bus->stop();
		delete bus;
	}
}
void ARAnalyse::IvyInit(const char *domain)
{
	// start Ivy
	bus = new Ivy( "ARIvy", NULL, NULL );
	bus->SetFilter( sizeof( ivy_filters ) /sizeof( char *), ivy_filters );
	bus->start(domain);
}
bool ARAnalyse::Calibrate( )
{
	if ( m_calibration )
		m_calibrated = m_calibration->Calibrate();
	return m_calibrated;
}
void ARAnalyse::SaveCalibration( )
{
	if ( m_calibration )
		m_calibration->SaveCalibration(CalibrationFile);
}
void ARAnalyse::RestoreCalibration( )
{
	if ( m_calibration )
		m_calibrated = m_calibration->RestoreCalibration(CalibrationFile);
}
bool ARAnalyse::ToggleAutoThreshold()
{
	if ( m_tracker->isAutoThresholdActivated() )
	{
		// try to set manually 
		if ( MarkerSeekingThreshold > 0 )
		{
			m_tracker->activateAutoThreshold( false );
			m_tracker->setThreshold( MarkerSeekingThreshold );
		}
	}
	else
	{
		m_tracker->activateAutoThreshold( true );
	}
	return m_tracker->isAutoThresholdActivated();
}

bool ARAnalyse::IsCalibrationID( int ident )
{
	return m_calibration_ids.find( ident ) != m_calibration_ids.end();
}
	
bool ARAnalyse::InitialiseTracker( int width, int height, int sample_size)
{
	first_calibration_done = false;
	camera = narrow( g_camera_name );
	int pixel_size = sample_size / width /height;
	m_width = width;
	m_height = height;
	nb_pixel = width * height;
	m_gray = new pixel_type[nb_pixel];
	// on alloue au max pur eviter de se prendre la tete avec le malloc free , et le changement de DownSample
	m_blur0 = new pixel_type[nb_pixel /*width /DownSample * height /DownSample*/];
	m_blur1 = new pixel_type[nb_pixel /*width /DownSample * height /DownSample*/];
	m_filter_h = new pixel_type[nb_pixel /*width /DownSample * height /DownSample*/];
	m_filter_v = new pixel_type[nb_pixel /*width /DownSample * height /DownSample*/];

	nb_images = 5;
	m_sum = new float[nb_pixel];
	for ( int i = 0; i < nb_pixel ; i++ )
	{
		m_sum[i] = 0.0;
	}
	m_images = new float*[nb_images];
	for ( int i = 0 ; i < nb_images; i++ )
		m_images[i] = new float[nb_pixel];
	m_first_image = 0;
	m_last_image = 0;

	
	if ( m_tracker ) delete m_tracker;
    // create a tracker that does:
    //  - 6x6 sized marker images (required for binary markers)
    //  - samples at a maximum of 6x6
    //  - works with luminance (gray) images
    //  - can load a maximum of 0 non-binary pattern
    //  - can detect a maximum of 8 patterns in one image
    m_tracker = new TrackerMultiMarker(width, height, 4096, 6, 6, 6, 0);
	// poor man pixel format chooser
	switch ( pixel_size )
	{
	case 1:
	    m_tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);
		break;
	case 2:
		m_tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_YUY2);
		break;
	case 3:
		m_tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_RGB);
		break;
	}
    // load a camera file.
	if (!m_tracker->init(CameraParameterFile.c_str(),
						MarkerSetConfigFile.c_str(),
						0.0f, 3000.0f)) 
		{
        DBGMSG(TEXT("ERROR: InitialiseTracker() failed\n"));
		delete m_tracker;
		m_tracker = NULL;
		return false;
		}

    //m_tracker->getCamera()->printSettings();
	m_tracker->setNumAutoThresholdRetries(2);
	//if ( MarkerSeekingThreshold > 0 )
	//{
	//	m_tracker->activateAutoThreshold( false );
	//	m_tracker->setThreshold( MarkerSeekingThreshold );
	//}
	//else
	//{
		m_tracker->activateAutoThreshold( true );
	//}
    m_tracker->setImageProcessingMode(ARToolKitPlus::IMAGE_FULL_RES);
    m_tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP);
	//m_tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL_CONT);
    // the marker in the BCH test image has a thiner border...
    m_tracker->setBorderWidth(MarkerBorderPercentage);

    // set a threshold. we could also activate automatic thresholding
    //m_tracker->setThreshold(MarkerSeekingThreshold);

    // let's use lookup-table undistortion for high-speed
    // note: LUT only works with images up to 1024x1024
    //m_tracker->setUndistortionMode(ARToolKitPlus::UNDIST_LUT);
	m_tracker->setUndistortionMode(ARToolKitPlus::UNDIST_NONE);
	m_tracker->setUseDetectLite(false);
    // switch to simple ID based markers
    // use the tool in tools/IdPatGen to generate markers
    m_tracker->setMarkerMode(MarkerType);
	// HULL mode
    m_tracker->setHullMode(ARToolKitPlus::HULL_FULL);
            
    // do the OpenGL camera setup
    //glMatrixMode(GL_PROJECTION)
    //glLoadMatrixf(m_tracker->getProjectionMatrix());

    return true;
}

void ARAnalyse::Analyse( double SampleTime, const unsigned char * cameraBuffer, int size )
{
	static double StartTime = -1;
	static double LastFrameTime = 0;
	

	extern HWND g_hTrack;  // Quick & Dirty !!!!

	double delay = 0;
	if ( StartTime < 0 )
	{
		StartTime = SampleTime;
		LastFrameTime = SampleTime;
	}
	else
	{
	delay = SampleTime - LastFrameTime;
	}
	FrameCount++;	
	if ( FrameCount > 30 ) // skip first frames in calc avg Wait first Seconds video
	{
		frame_per_seconds.SetValue( FrameCount / ( SampleTime - StartTime ) );
		frame_delay.SetValue( delay );
	}
	LastFrameTime = SampleTime;
	
	if ( !m_tracker) return;
	// here we go, just one call to find the camera pose
    int numDetected = m_tracker->calc(cameraBuffer);

	//DBGMSG(TEXT("\n tracker Threshold %d \n"), m_tracker->getThreshold());
	if ( m_tracker->isAutoThresholdActivated() && FrameCount %10 == 0  )
	{
	SendMessage(g_hTrack, TBM_SETPOS, 
        (WPARAM) TRUE,                   // redraw flag 
        (LPARAM) m_tracker->getThreshold());
	}
    // use the result of calc() to setup the OpenGL transformation
    //glMatrixMode(GL_MODELVIEW)
    //glLoadMatrixf(m_tracker->getModelViewMatrix());
	//if ( numDetected )
	//    DBGMSG(TEXT("\n%d good Markers found and used for pose estimation.\n"), numDetected);
#ifdef DUMP_MATRIX
	if ( numDetected )
	{
	DBGMSG(TEXT("Pose-Matrix:\n  "));
	const ARFloat*matrix = m_tracker->getModelViewMatrix();
	if ( matrix )
    for (int i = 0; i < 16; i++)
        DBGMSG(TEXT("%.2f %s"), matrix[i], (i % 4 == 3) ? TEXT("\n  ") : TEXT(" "));
	}
#endif
	            std::set<int> new_ids;
                confidence.Raz(); 
	
                for (int i = 0; i < numDetected; i++) {
                    ARToolKitPlus::ARMarkerInfo markerInfo = m_tracker->getDetectedMarker(i);
                   
                    if (new_ids.find(markerInfo.id) != new_ids.end()) {
                        DBGMSG(TEXT("One id for one marker please!!\n"));
                        continue;
                    }
                    // calculate min max confidence
					confidence.SetValue( markerInfo.cf );
					
					new_ids.insert(markerInfo.id);

                    if (markers.find(markerInfo.id) == markers.end() )
                    {
                        // Adding new marker
                        markers[markerInfo.id].id = markerInfo.id;
                        DBGMSG(TEXT("MarkerAdd cam=%s_%d id=%d\n"), g_camera_name.c_str(), g_camera_instance, markerInfo.id);
						// filtrage des ID de calibration
                        if ( bus && !IsCalibrationID(markerInfo.id) ) bus->SendMsg("MarkerAdd cam=%s_%d id=%d", camera, g_camera_instance, markerInfo.id);
                    }
                    MyMakerInfo & info = markers[markerInfo.id];
					info.id = markerInfo.id;
                    info.absent = 0;
                    info.confidence = markerInfo.cf;
                    info.area = markerInfo.area;
                    info.dir = markerInfo.dir;
					
                    info.time = SampleTime - StartTime;
                    ARFloat center[2] ;
                    ARFloat  matrix[3][4];
                    center[0] = 0.0f;
                    center[1] = 0.0f;
                    //ARFloat retTransMat = m_tracker->arGetTransMat(tracker, markerInfoPtr, center, (ARFloat)Properties.Settings.Default.MarkerWidth, matrix);
                    ARFloat retTransMat =  m_tracker->arGetTransMatCont( &markerInfo, info.prev_matrix, center, (ARFloat)MarkerWidth, matrix);
                    //ARFloat retTransMat = m_tracker->rppGetTransMat(tracker, markerInfoPtr, center, (ARFloat)Properties.Settings.Default.MarkerWidth, matrix);
                    if (retTransMat < 0)
                    {
                        DBGMSG(TEXT("Marker Skip id={0} bad transfrm{1}\n"), info.id, retTransMat);
                        continue;
                    }
                    memcpy(info.prev_matrix, matrix, sizeof(matrix));
                    double dx = info.x  - markerInfo.pos[0];
					double dy = info.y  - markerInfo.pos[1];
					double dist = dx*dx + dy*dy; // Square Length distance
					
                    info.x = markerInfo.pos[0];
					info.y = markerInfo.pos[1];
					if ( dist >= MarkerMoveThreshold)
                    {
                        DBGMSG(TEXT("MarkerMove cam=%s_%d id=%d time=%.03f confidence=%.03f area=%d dir=%d x=%.03f y=%.03f distance=%.03f \n"),
                            g_camera_name.c_str(), g_camera_instance, info.id, info.time, info.confidence, info.area, info.dir, info.x, info.y, dist);
                        if ( bus && !IsCalibrationID(markerInfo.id)) 
							bus->SendMsg("MarkerMove cam=%s_%d id=%d time=%.03f confidence=%.03f area=%d dir=%d x=%.03f y=%.03f distance=%.03f",
								camera, g_camera_instance, info.id, info.time, info.confidence, info.area, info.dir, info.x, info.y, dist);
                        if (m_calibrated)
                        {
							double cal_x, cal_y;
                            m_calibration->translateCoords(info.x, info.y, &cal_x, &cal_y);
                            DBGMSG(TEXT("MarkerMoveCalibrated cam=%s_%d id=%d time=%.03f confidence=%.03f area=%d dir=%d x=%.03f y=%.03f\n"), g_camera_name.c_str(), g_camera_instance, info.id, info.time, info.confidence, info.area, info.dir, cal_x, cal_y);
                            if ( bus && !IsCalibrationID(markerInfo.id)) 
								bus->SendMsg("MarkerMoveCalibrated cam=%s_%d id=%d time=%.03f confidence=%.03f area=%d dir=%d x=%.03f y=%.03f", camera, g_camera_instance, info.id, info.time, info.confidence, info.area, info.dir, cal_x, cal_y);
                        
                        }
                    }
                    
                }
                // remove OLD markers
                std::set<int> old_ids;
				for (MarkersMap::iterator iter = markers.begin(); iter != markers.end();  iter++)
				{
					int id = iter->first;
					if ( new_ids.find( id ) == new_ids.end() )
						old_ids.insert( id );
				}
				for (std::set<int>::iterator iter = old_ids.begin(); iter != old_ids.end();  iter++)
                {
					int id = *iter;
                    markers[id].absent += 1;
                    if ( markers[id].absent > MarkerAbsenceCount)
                    {
                        markers.erase(id);
                        DBGMSG(TEXT("Marker Remove cam=%s_%d id=%d\n"), g_camera_name.c_str(), g_camera_instance, id);
                        if ( bus && !IsCalibrationID(id)) bus->SendMsg("MarkerRemove cam=%s_%d id=%d", camera, g_camera_instance, id);
                    }
                }
                /*if (titleText.Equals("")) {
                    titleText ="Markers with ID \""
                        +String.Join(", ", new_ids.Select(id => id.ToString("000")).ToArray())
                        +"\" are found!!";
                }*/
 
                
                    
                // calibration point
                m_calibration_enabled = false;
                if (includes(new_ids.begin(), new_ids.end(), m_calibration_ids.begin(), m_calibration_ids.end() ))
				{
                    for (std::set<int>::iterator iter = m_calibration_ids.begin(); iter != m_calibration_ids.end() ; iter++)
                    {
						int id = *iter;
                        MyMakerInfo marker = markers[id];
                        m_calibration->SetCalibrationPoint(m_IdToCorner[id], marker.x, marker.y);
                    }
                    //all calibration points OK
					m_calibration_enabled = true;
					if ( !first_calibration_done )
					{
						first_calibration_done = Calibrate();
						m_calibrated = first_calibration_done;
					}
                    
                }
					
           
    bool showConfig = false;

    if (showConfig) {
        const ARToolKitPlus::ARMultiMarkerInfoT *artkpConfig = m_tracker->getMultiMarkerConfig();
        DBGMSG(TEXT("%d markers defined in multi marker cfg\n"), artkpConfig->marker_num);

        printf("marker matrices:\n");
        for (int multiMarkerCounter = 0; multiMarkerCounter < artkpConfig->marker_num; multiMarkerCounter++) {
            DBGMSG(TEXT("marker %d, id %d:\n"), multiMarkerCounter, artkpConfig->marker[multiMarkerCounter].patt_id);
            for (int row = 0; row < 3; row++) {
                for (int column = 0; column < 4; column++)
                    DBGMSG(TEXT("%.2f  "), artkpConfig->marker[multiMarkerCounter].trans[row][column]);
                DBGMSG(TEXT("\n"));
            }
        }
    }
}

#define MAX(a,b) ((a) > (b) ? (a) : (b))
#define MIN(a,b) ((a) < (b) ? (a) : (b))
static void RGB2HSL(unsigned char * rgb,  double& h, double& s,  double& l)
        {

            double r = (double)rgb[0] / 255.0;
            double g = (double)rgb[1] / 255.0;
            double b = (double)rgb[2] / 255.0;
            double v;
            double m;
            double vm;
            double r2, g2, b2;

            h = 0; // default to black
            s = 0;
            l = 0;

            v = MAX(r, g);
            v = MAX(v, b);
            m = MIN(r, g);
            m = MIN(m, b);
            l = (m + v) / 2.0;
            if (l <= 0.0)
            {
                return;
            }

            vm = v - m;
            s = vm;
            if (s > 0.0)
            {
                s /= (l <= 0.5) ? (v + m) : (2.0 - v - m);
            }
            else
            {
                return;
            }

            r2 = (v - r) / vm;
            g2 = (v - g) / vm;
            b2 = (v - b) / vm;
            if (r == v)
            {
                h = (g == m ? 5.0 + b2 : 1.0 - g2);
            }

            else if (g == v)
            {
                h = (b == m ? 1.0 + r2 : 3.0 - b2);
            }
            else
            {
                h = (r == m ? 3.0 + g2 : 5.0 - r2);
            }
            h /= 6.0;
        }
int CircularDistance(int index1, int index2, int steps)
        {            //circular distance
            int dist = abs(index1 - index2);
            return MIN(dist, abs(dist - steps));
        }
#ifdef TRANSFORM_COLOR
 void ARAnalyse::Transform( unsigned char * cameraBuffer, int size, bool moyen)
        {
			float min = FLT_MAX;
			float max = FLT_MIN;
			pixel_type *dst = m_blur0;
			// Gray Image
			{
				for ( int i = 0; i < nb_pixel ; i++ )
				{

					unsigned char  * pixel = &cameraBuffer[i*3];
					value_type val = pixel[0] * 0.3 + pixel[1] * 0.59 + pixel[2] * 0.11;
					m_gray[i] = val;
					{
						if ( val > max ) 
							max = val;
						else if ( val < min ) 
							min = val;
					}
					if ( !moyen ) 
						m_sum[i] = 0;
				}
				float a = 1.0;
				float b = 0.0;
				float c =  (max - min);
				if ( c != 0.0)
				{
					a = 1.0 / c;
					b = - min  / c;
				}
				a *= 255.0;
				b *= 255.0;
				#pragma omp parallel for
				for ( int i = 0; i < nb_pixel ; i++ )
				{
					m_gray[i] = m_gray[i] * a + b;
				}
			// Image moyenne
				if ( moyen )
					CalcMoyen();
			}

			const int steps = 10;
			const int chromaHistoLength = steps +1;
            
            int chromaHisto[chromaHistoLength];
            double hue[1920];  
            
            // chroma histogram 10 steps
            // find the 2 max
            //if the 2 max> 1 the threashold = min (2 max) + (max2 - max1)/2

            //Horizontal line 
            for (int y = 0; y < m_height; y++)
            {
                //Find min max mean median
				for (int i = 0; i < chromaHistoLength; i++)
					chromaHisto[i] = 0;
                for (int x = 0; x < m_width; x++)
                {
					long index = (x + y * m_width);
                    unsigned char  * pixel = &cameraBuffer[index*3];
					double h,s,l;
					RGB2HSL( pixel, h,s,l );
					hue[x] = h;
                    int histIndexChroma = (int)(h * steps);
                    chromaHisto[histIndexChroma]++;
                }

                //Find the two max
                int max1 = -1;
                int chroma1 = -1;
                int max2 = -1;
                int chroma2 = -1;
        
                //Find max1
                for (int i = 0; i < chromaHistoLength; i++)                   
                {
                    if ((chromaHisto[i] > max1) && (chromaHisto[i] != 0))
                    {
                        max1 = chromaHisto[i];
                        chroma1 = i;
                    }
                }
               //find max2
                for (int i = 0; i < chromaHistoLength; i++)                   
                {
                    if (CircularDistance(i, chroma1, steps) > (steps / 3))
                        if ((chromaHisto[i] > max2)  && (chromaHisto[i] != 0))
                        {
                            max2 = chromaHisto[i];
                            chroma2 = i;
                        }
                }

                int peak = chroma1;
                if ((max1 != -1) && (max2 != -1) &&  (chromaHisto[chroma2] > chromaHisto[chroma1])) peak = chroma2;
                double delta = 3;

				//binaries
				if ((max1 != -1) && (max2 != -1))  //found a max1 and max 2 ?
				{
					for (int x = 0; x < m_width; x++)
					{
						int index = (x + y * m_width);
						int histIndexChroma = (int)(hue[x] * steps);
						if (  CircularDistance(histIndexChroma, peak, steps) < delta)
							dst[index] = 255;  //White
						else
							dst[index] = 0;  //Black   
					}

				}
				else
					for (int x = 0; x < m_width; x++)
					{
						int index = (x + y * m_width);
						dst[index] = 127.0f;  //no relevant histogram
					}
            }

			switch ( Display )
			{
			case ARAnalyse::GrayScale:
				DisplayGrayScale(cameraBuffer, m_gray );
				break;
			case ARAnalyse::Blur:
				DisplayBlur(cameraBuffer, dst );
				break;
			case ARAnalyse::Diff:
				DisplayDiff(cameraBuffer, dst );
				break;
			case ARAnalyse::Threshold:
				DisplayThreshold(cameraBuffer, dst );
				break;
			case ARAnalyse::Moyen:
				DisplayMoyen(cameraBuffer, NULL );
				break;
			}
         
        }

 
#endif
#ifdef TRANSFORM_HUE
void ARAnalyse::Transform( unsigned char * cameraBuffer, int size, bool moyen)
{
	pixel_type *src;
	pixel_type *dst;
	float min = FLT_MAX;
	float max = FLT_MIN;
	//#pragma omp parallel //default(none) shared (cameraBuffer,pixel_size, max, min )
	{
	//#pragma omp parallel for
	for ( int i = 0; i < nb_pixel ; i++ )
	{
		unsigned char  * pixel = &cameraBuffer[i*3];
		value_type  rgb_min = MIN(pixel[0], pixel[1], pixel[2]);
		value_type  rgb_max = MAX(pixel[0], pixel[1], pixel[2]);

		value_type val = pixel[0] * 0.3 + pixel[1] * 0.59 + pixel[2] * 0.11;
		//
		//if ( pixel[0] > pixel[1]  && pixel[0] > pixel[2] )
		//	val = 255;
		//if ( pixel[2] > pixel[0] && pixel[2] > pixel[1]  )
		//	val = 0;
		//m_gray[i] = val;
		
		/* Compute hue */
		//value_type hue;
		//if (rgb_max == pixel[0]) {
		//	hue = 0 + 43*(pixel[1] - pixel[2])/(rgb_max - rgb_min);
		//} else if (rgb_max == pixel[1]) {
		//	hue = 85 + 43*(pixel[2] - pixel[0])/(rgb_max - rgb_min);
		//} else /* rgb_max == rgb.b */ {
		//	hue = 171 + 43*(pixel[0] - pixel[1])/(rgb_max - rgb_min);
		//}
		double h,s,l;
		RGB2HSL( pixel, h,s,l );
		m_gray[i] = h * 255.0;
		//#pragma omp critical
		{
			if ( val > max ) 
				max = val;
			else 
				if ( val < min ) 
					min = val;
		}
		if ( !moyen )
			m_sum[i] = 0;
	}

	float a = 1.0;
	float b = 0.0;
	float c =  (max - min);
	if ( c != 0.0)
	{
		a = 1.0 / c;
		b = - min  / c;
	}
	a *= 255.0;
	b *= 255.0;
	#pragma omp parallel for
	for ( int i = 0; i < nb_pixel ; i++ )
	{
		m_gray[i] = m_gray[i] * a + b;
	}
	}
	if ( moyen )
		CalcMoyen();
	// Down sample
	int width = m_width / DownSample;
	int height = m_height / DownSample;
	
	#pragma omp parallel //default(none) shared (width,height)
	{
	#pragma omp parallel for
	for ( int y = 0; y < height ; y++ )
	{
			//#pragma omp for
			for ( int x = 0; x < width ; x++ )
			{
				long yy = y * width + x;
				value_type val = 0;
				for ( int i = 0 ; i < DownSample ; i++ )
					for ( int j = 0; j < DownSample ; j++ )
					{
						int index = (y*DownSample+i) * m_width + x*DownSample+j;
						val += m_gray[index];
					}
				val /= DownSample*DownSample;
				//assert( val <= 255 );
				m_blur0[yy] = val;
				m_blur1[yy] = val;
			}
	}
	}
	// blur
	dst = m_blur0;
#define BLUR
#ifdef BLUR		
	#pragma omp parallel  //default(none) shared (width,height,src,dst)
	{
		
		static const float moyen[] = {
			1/9.0,1/9.0,1/9.0,
			1/9.0,1/9.0,1/9.0,
			1/9.0,1/9.0,1/9.0
		};
		static const float laplacien4[] = {
			0,-1,0,
			-1,4,-1,
			0,-1,0
		};
		
		static const float laplacien8[] = {
			-1,-1,-1,
			-1,8,-1,
			-1,-1,-1
		};
		
		static const float laplaciend[] = {
			1,-2,1,
			-2,4,-2,
			1,-2,1
		};
		
		static const float sobelv[] = {
			1,0,-1,
			2,0,-2,
			1,0,-1
		};
		static const float sobelh[] = {
			1,2,1,
			0,0,0,
			-1,2,-1
		};
		
		static const float prewittv[] = {
			1,0,-1,
			1,0,-1,
			1,0,-1
		};
		static const float prewitth[] = {
			1,1,1,
			0,0,0,
			-1,-1,-1
		};
		//5x5 Gaussian filter
		static const float gaussian[] = {
			2/159.0,	4/159.0,	5/159.0,	4/159.0,	2/159.0,
			4/159.0,	9/159.0,	12/159.0,	9/159.0,	4/159.0,
			5/159.0,	12/159.0,	15/159.0,	12/159.0,	5/159.0,
			4/159.0,	9/159.0,	12/159.0,	9/159.0,	4/159.0,
			2/159.0,	4/159.0,	5/159.0,	4/159.0,	2/159.0	
		};
	for ( int pass = 0 ; pass < BlurPass ; pass++ )
	{
		if ( (pass & 1) ==0 ) 
		{
		src = m_blur0;
		dst = m_blur1;
		}
		else
		{
		src = m_blur1;
		dst = m_blur0;
		}
		switch ( filter )
		{
		case FMoyen:
			Filter(  moyen,3, src, dst );
			break; 
		case FLaplacien4:
			Filter( laplacien4,3, src, dst );
			break;
		case FLaplacien8:
			Filter( laplacien8, 3, src, dst );
			break;
		case FLaplaciend:
			Filter( laplaciend, 3, src, dst );
			break;
		case FSobel:
			Filter( sobelv, 3, src, m_filter_v );
			Filter( sobelh, 3, src, m_filter_h );
			CombineFilter( m_filter_v, m_filter_h, dst );
			break;
		case FPrewitt:
			Filter( prewittv, 3, src, m_filter_v );
			Filter( prewitth, 3, src, m_filter_h );
			CombineFilter( m_filter_v, m_filter_h, dst );
		case FGaussian:
			Filter( gaussian, 5, src, dst );
			break;
		}
		
	}
	}
#endif	
	switch ( Display )
	{
	case ARAnalyse::GrayScale:
		DisplayGrayScale(cameraBuffer, dst );
		break;
	case ARAnalyse::Blur:
		DisplayBlur(cameraBuffer, dst );
		break;
	case ARAnalyse::Diff:
		DisplayDiff(cameraBuffer, dst );
		break;
	case ARAnalyse::Threshold:
		DisplayThreshold(cameraBuffer, dst );
		break;
	case ARAnalyse::Moyen:
		DisplayMoyen(cameraBuffer, dst );
		break;
	}
		
}
#endif
#define GRAY
#ifdef GRAY 
// Gray scale & threshold
void ARAnalyse::Transform( unsigned char * cameraBuffer, int size, bool moyen)
{
	pixel_type *src;
	pixel_type *dst;
	float min = FLT_MAX;
	float max = FLT_MIN;
	//#pragma omp parallel //default(none) shared (cameraBuffer,pixel_size, max, min )
	{
	//#pragma omp parallel for
	for ( int i = 0; i < nb_pixel ; i++ )
	{
		unsigned char  * pixel = &cameraBuffer[i*3];
		value_type val = pixel[0] * 0.3 + pixel[1] * 0.59 + pixel[2] * 0.11;
		m_gray[i] = val ;
		//#pragma omp critical
		{
			if ( val > max ) 
				max = val;
			else 
				if ( val < min ) 
					min = val;
		}
		if ( !moyen )
			m_sum[i] = 0;
	}

	float a = 1.0;
	float b = 0.0;
	float c =  (max - min);
	if ( c != 0.0)
	{
		a = 1.0 / c;
		b = - min  / c;
	}
	a *= 255.0;
	b *= 255.0;
	#pragma omp parallel for
	for ( int i = 0; i < nb_pixel ; i++ )
	{
		m_gray[i] = m_gray[i] * a + b;
	}
	}
	if ( moyen )
		CalcMoyen();
	// Down sample
	int width = m_width / DownSample;
	int height = m_height / DownSample;
	
	#pragma omp parallel //default(none) shared (width,height)
	{
	#pragma omp parallel for
	for ( int y = 0; y < height ; y++ )
	{
			//#pragma omp for
			for ( int x = 0; x < width ; x++ )
			{
				long yy = y * width + x;
				value_type val = 0;
				for ( int i = 0 ; i < DownSample ; i++ )
					for ( int j = 0; j < DownSample ; j++ )
					{
						int index = (y*DownSample+i) * m_width + x*DownSample+j;
						val += m_gray[index];
					}
				val /= DownSample*DownSample;
				//assert( val <= 255 );
				m_blur0[yy] = val;
				m_blur1[yy] = val;
			}
	}
	}
	// blur
	dst = m_blur0;
#define BLUR
#ifdef BLUR		
	#pragma omp parallel  //default(none) shared (width,height,src,dst)
	{
		
		static const float moyen[] = {
			1/9.0,1/9.0,1/9.0,
			1/9.0,1/9.0,1/9.0,
			1/9.0,1/9.0,1/9.0
		};
		static const float laplacien4[] = {
			0,-1,0,
			-1,4,-1,
			0,-1,0
		};
		
		static const float laplacien8[] = {
			-1,-1,-1,
			-1,8,-1,
			-1,-1,-1
		};
		
		static const float laplaciend[] = {
			1,-2,1,
			-2,4,-2,
			1,-2,1
		};
		
		static const float sobelv[] = {
			1,0,-1,
			2,0,-2,
			1,0,-1
		};
		static const float sobelh[] = {
			1,2,1,
			0,0,0,
			-1,2,-1
		};
		
		static const float prewittv[] = {
			1,0,-1,
			1,0,-1,
			1,0,-1
		};
		static const float prewitth[] = {
			1,1,1,
			0,0,0,
			-1,-1,-1
		};
		//5x5 Gaussian filter
		static const float gaussian[] = {
			2/159.0,	4/159.0,	5/159.0,	4/159.0,	2/159.0,
			4/159.0,	9/159.0,	12/159.0,	9/159.0,	4/159.0,
			5/159.0,	12/159.0,	15/159.0,	12/159.0,	5/159.0,
			4/159.0,	9/159.0,	12/159.0,	9/159.0,	4/159.0,
			2/159.0,	4/159.0,	5/159.0,	4/159.0,	2/159.0	
		};
	for ( int pass = 0 ; pass < BlurPass ; pass++ )
	{
		if ( (pass & 1) ==0 ) 
		{
		src = m_blur0;
		dst = m_blur1;
		}
		else
		{
		src = m_blur1;
		dst = m_blur0;
		}
		switch ( filter )
		{
		case FMoyen:
			Filter(  moyen,3, src, dst );
			break; 
		case FLaplacien4:
			Filter( laplacien4,3, src, dst );
			break;
		case FLaplacien8:
			Filter( laplacien8, 3, src, dst );
			break;
		case FLaplaciend:
			Filter( laplaciend, 3, src, dst );
			break;
		case FSobel:
			Filter( sobelv, 3, src, m_filter_v );
			Filter( sobelh, 3, src, m_filter_h );
			CombineFilter( m_filter_v, m_filter_h, dst );
			break;
		case FPrewitt:
			Filter( prewittv, 3, src, m_filter_v );
			Filter( prewitth, 3, src, m_filter_h );
			CombineFilter( m_filter_v, m_filter_h, dst );
		case FGaussian:
			Filter( gaussian, 5, src, dst );
			break;
		}
		
	}
	}
#endif	
	switch ( Display )
	{
	case ARAnalyse::GrayScale:
		DisplayGrayScale(cameraBuffer, dst );
		break;
	case ARAnalyse::Blur:
		DisplayBlur(cameraBuffer, dst );
		break;
	case ARAnalyse::Diff:
		DisplayDiff(cameraBuffer, dst );
		break;
	case ARAnalyse::Threshold:
		DisplayThreshold(cameraBuffer, dst );
		break;
	case ARAnalyse::Moyen:
		DisplayMoyen(cameraBuffer, dst );
		break;
	}
		
}
#endif
void ARAnalyse::CombineFilter ( pixel_type *src_h, pixel_type *src_v, pixel_type *dst )
{
	int width = m_width / DownSample;
	int height = m_height / DownSample;
	#pragma omp for
		for ( int y = height-2 ; y > 0  ; y-- )
		{
			//#pragma omp for
			for ( int x = width-2; x > 0  ; x-- )
			{
				long yy;
				value_type val = 0;
				// kernel sum
				yy = y * width + x;
				
				val += src_h[ yy ]	* src_h[ yy ];
				val += src_v[ yy ]	* src_v[ yy ];
				
				dst[yy] = sqrt(val);
				
			}
		}
}
void ARAnalyse::Filter ( const float *krn, int k_size, pixel_type *src, pixel_type *dst )
{
	int width = m_width / DownSample;
	int height = m_height / DownSample;
	const float *krn_ptr;
	int half_kernel = k_size / 2;
	#pragma omp for
		for ( int y = height-1-half_kernel ; y >= half_kernel  ; y-- )
		{
			//#pragma omp for
			for ( int x = width-1-half_kernel; x >= half_kernel  ; x-- )
			{
				long yy;
				value_type val = 0;
				// kernel sum
				krn_ptr = krn;
				yy = (y - half_kernel) * width + x ;
				for ( int j = 0; j < k_size ; j++ )
				{
					for ( int i = 0; i < k_size ; i++ )
						val += src[ yy - half_kernel + i ] * *krn_ptr++;
				yy +=  width ;
				}
				
				dst[y * width + x] = val;
				
			}
		}
}
	
void ARAnalyse::DisplayGrayScale (unsigned char * cameraBuffer, pixel_type * dest) 
{
	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 
	{
	#pragma omp for
	for ( int y = 0; y < m_height ; y++ )
	{
			//#pragma omp for
			for ( int x = 0; x < m_width ; x++ )
			{
				long index = (y * m_width + x);
				unsigned char  * pixel = &cameraBuffer[index*3];
				value_type val = m_gray[index]; //display gray scale				
				pixel[0] = (unsigned char)(int)(val);
				pixel[1] = (unsigned char)(int)(val);
				pixel[2] = (unsigned char)(int)(val);
			}
	}
	}
}
void ARAnalyse::DisplayBlur (unsigned char * cameraBuffer, pixel_type * dest) 
{
	// Down sample
	int width = m_width / DownSample;
	int height = m_height / DownSample;
	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 
	{
	#pragma omp for
	for ( int y = 0; y < m_height ; y++ )
	{
			//#pragma omp for
			for ( int x = 0; x < m_width ; x++ )
			{
				long index = (y * m_width + x);
				unsigned char  * pixel = &cameraBuffer[index*3];
				value_type blur =  dest[((y/DownSample) * width + (x/DownSample))];
				value_type val=0;
				val =  blur; // display BLUR
				
				pixel[0] = (unsigned char)(int)(val);
				pixel[1] = (unsigned char)(int)(val);
				pixel[2] = (unsigned char)(int)(val);
			}
	}
	}
}
void ARAnalyse::DisplayDiff (unsigned char * cameraBuffer, pixel_type * dest) 
{
	// Down sample
	int width = m_width / DownSample;
	int height = m_height / DownSample;
	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 
	{
	#pragma omp for
	for ( int y = 0; y < m_height ; y++ )
	{
			//#pragma omp for
			for ( int x = 0; x < m_width ; x++ )
			{
				long index = (y * m_width + x);
				unsigned char  * pixel = &cameraBuffer[index*3];
				value_type blur =  dest[((y/DownSample) * width + (x/DownSample))];
				value_type val=0;
				val = m_gray[index] - blur;
				val += 127;
				if ( val < 0 ) val = 0;
				else if ( val > 255 ) val = 255;
				
				pixel[0] = (unsigned char)(int)(val);
				pixel[1] = (unsigned char)(int)(val);
				pixel[2] = (unsigned char)(int)(val);
			}
	}
	}
}
void ARAnalyse::DisplayThreshold (unsigned char * cameraBuffer, pixel_type * dest) 
{
	// Down sample
	int width = m_width / DownSample;
	int height = m_height / DownSample;
	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 
	{
	#pragma omp for
	for ( int y = 0; y < m_height ; y++ )
	{
			//#pragma omp for
			for ( int x = 0; x < m_width ; x++ )
			{
				long index = (y * m_width + x);
				unsigned char  * pixel = &cameraBuffer[index*3];
				value_type blur =  dest[((y/DownSample) * width + (x/DownSample))];
				value_type val=0;
				val = m_gray[index] - blur * 0.9;
						//val =  val  < 0 ? 0 : 255;
						val += 127;
						if ( val < g_ThresholdMin  ) val = 0;
						else if ( val > g_ThresholdMax ) val = 255;
						else val = 127;
				
				pixel[0] = (unsigned char)(int)(val);
				pixel[1] = (unsigned char)(int)(val);
				pixel[2] = (unsigned char)(int)(val);
			}
	}
	}
}
void ARAnalyse::DisplayMoyen (unsigned char * cameraBuffer, pixel_type * dest) 
{
	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 
	{
	#pragma omp for
	for ( int y = 0; y < m_height ; y++ )
	{
			//#pragma omp for
			for ( int x = 0; x < m_width ; x++ )
			{
				long index = (y * m_width + x);
				unsigned char  * pixel = &cameraBuffer[index*3];
				value_type val = (m_sum[index] / nb_images);
				pixel[0] = (unsigned char)(int)(val);
				pixel[1] = (unsigned char)(int)(val);
				pixel[2] = (unsigned char)(int)(val);
			}
	}
	}
}

void ARAnalyse::CalcMoyen()
{
	//DBGMSG(TEXT("Before CalcMoyen Frame %d first %d, last %d:\n"), FrameCount, m_first_image, m_last_image);
            
	// calcul sum - first 
	if ( FrameCount > (nb_images-1) ) 
	{
		pixel_type *first_image = m_images[m_first_image];
		for ( int i = 0; i < nb_pixel ; i++ )
		{
			m_sum[i] -= first_image[i];
		}
		m_first_image++;
		if ( m_first_image >= nb_images ) m_first_image = 0;
	}
	// calcul sum + last = m_gray !
	for ( int i = 0; i < nb_pixel ; i++ )
	{
		m_sum[i] += m_gray[i];
		 m_images[m_last_image][i] = m_gray[i]; // copy input to last image
		m_gray[i] = m_sum[i]/nb_images;			// copy and calculate moyen 
	}
	m_last_image++;
	if ( m_last_image >= nb_images ) m_last_image = 0;
	//DBGMSG(TEXT("After CalcMoyen first %d, last %d:\n"), m_first_image, m_last_image);
    
}