#include "Controller.h"
#include "cv.h"
#include "highgui.h"
#include "stdio.h"
#include <SerialStream.h>
#include <iostream>
#include <string>
#include <sstream>
#include <ostream>
#include <streambuf>
#include <sstream>
#include <iostream>



#define PORT "/dev/ttyUSB0"



using namespace std;
using namespace LibSerial;

SerialStream ardu;				//SerialStream from libSerial for transmission of data over serial
int thresh=0;					//level of thresholding to be applied to the image, controlled by slider


Controller::Controller(void)
{

}

/***
 *
 *  Callback method used to allow slider to change threshold
 *
 ***/

void switch_callback( int position ){
	thresh = position;
}

/***
 *
 *  Method which controls main loop for video capture and processing
 *
 ***/

void Controller::start()
{
	CvCapture * pCapture;                       //new OpenCV capture stream
	IplImage * pVideoFrame;                     //new OpenCV image
        IplImage * pVideoFrameBW;                   //new OpenCV image for color conversion
        
	pCapture = cvCaptureFromCAM(0);             //choose camera for capture
        
	cvNamedWindow( "video", CV_WINDOW_AUTOSIZE );//Declare window
        cvCreateTrackbar("track","video",&thresh,255,switch_callback);//Add slider to control thresholding

	int key = -1;                               //key to tke keyboard input

        ardu.Open(PORT);                            //Open Serial Port ardu

        /*setup serial connection*/
        ardu.SetBaudRate(SerialStreamBuf::BAUD_115200);
        ardu.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);

        while(key == -1)                            //while no key has been pressed
	{
            try {
                stringstream str;		     //Stream to build string for output

                pVideoFrame = cvQueryFrame(pCapture);//capture frame

                cvThreshold(pVideoFrame, pVideoFrame, thresh, 255, CV_THRESH_BINARY); //Threshold image based on slider value

                pVideoFrameBW=cvCreateImage(cvGetSize(pVideoFrame),8,1);//allocate memory for new image
                
                cvCvtColor(pVideoFrame, pVideoFrameBW, CV_BGR2GRAY);//convert color to greyscale
                
                /*create small image*/
                IplImage* out = cvCreateImage( cvSize(pVideoFrameBW->width/60,
                        pVideoFrameBW->height/60), pVideoFrameBW->depth,
                        pVideoFrameBW->nChannels );
                
                cvResize(pVideoFrameBW,out,CV_INTER_NN);            //resize image
                
                cvResize(out,pVideoFrameBW,CV_INTER_NN);            //resize image for display

                cvThreshold(out,out,120,255,CV_THRESH_BINARY);      //threshold image
                
                cvShowImage( "video", pVideoFrameBW);		    //renders image to window

                int ledCount=0;			//Int to keep track of current led

		/*loop over all 64 leds for every frame captured*/

                for(int y=0; y<8; y++)		//loop over rows
                {
                    
                    for(int x=0;x<8;x++)	//loop over columns
                    {
                        CvScalar s;		//allocate space to store image scalar

                        s=cvGet2D(out,y,x); 	//get the (i,j) pixel value
                        int p = s.val[0];	//save pixel value (blue)
                        printf("%i",p);		//print value to terminal for debugging
                        
			if(p==255)		//if the pixel is black
                        {
                            p=1;		//turn on led
                        }
                        
                        str<<p;			//add current pixel to stream
                        
                        ledCount++;		//move to next pixel
                    }
                    
                }
                printf("\n");			//add a new line to the terminal display to separate frames
                printf("\n");

                ardu<<str.str();		//send frame over serial

                usleep(6000);			//wait to allow the arduino to catch up (vary this value if you get nonsence displayed)
                key = cvWaitKey(20);		//wait for keypress to quit

		/*release memory*/
                cvReleaseImage(&pVideoFrameBW);
                cvReleaseImage(&out);
            }

            catch(exception)
            {
                 cvReleaseImage( &pVideoFrame );//release frame

                 cvReleaseCapture ( &pCapture );// release pointer copture stream

                cvDestroyWindow( "video" );     // Destroy window
                exit(0);
            }
                               
	}

        cvReleaseImage( &pVideoFrame );		//release frame
        cvReleaseCapture ( &pCapture ); 	// release pointer copture stream
        
        cvDestroyWindow( "video" );     	// Destroy window
        exit(0);                        	// Exit

}


Controller::~Controller(void)
{
}