#include "testApp.h"

using namespace ofxCv;
using namespace cv;

//-----------------------INITIALISING--------------//
void testApp::setup(){
//	ofSetVerticalSync(true);

	camWidth 		= 640;	// try to grab at this size.
	camHeight 		= 480;

    Adam = pic = cCirc = false;

	vidGrabber.setVerbose(true);
	vidGrabber.initGrabber(camWidth,camHeight);

    colorImg        = new unsigned char[camWidth*camHeight*3];
	videoTexture.allocate(camWidth,camHeight, GL_RGB);

    origImg.loadImage("display.jpg");
    //warpImg.loadImage("display.jpg");
    origPoints.push_back(ofVec2f(0,0));                                     // blue
    origPoints.push_back(ofVec2f(origImg.getWidth(),0));                    // red
    origPoints.push_back(ofVec2f(0,origImg.getHeight()));                   // yellow
    origPoints.push_back(ofVec2f(origImg.getWidth(),origImg.getHeight()));  // green


    imitate(warpImg, origImg); // this isn't really doing anything as of right now, I believe.
}
//--------------------UPDATE FUNCTION------------------//
void testApp::update(){

	vidGrabber.grabFrame();


	if (vidGrabber.isFrameNew()){

		//int totalPixels = camWidth*camHeight*3;
		unsigned char * pixels = vidGrabber.getPixels();

        for (int y = 0; y < camHeight; y++)
            for (int x = 0; x < camWidth; x++)
                for (int c = 0; c < 3; c++)
                    colorImg[(y*camWidth + x)*3+c] = pixels[(y*camWidth + x)*3+c];

                // this code is enabling our webcam feed on the OF canvas


        videoTexture.loadData(colorImg, camWidth,camHeight, GL_RGB);
    }
}
//------------------------Draw function-----------------------------//
void testApp::draw(){
    ofSetColor(255,255,255);
    videoTexture.draw(0,0,camWidth,camHeight);

    if (Adam) {
        trackRect(); // runs the function that tracks the colors
        ofNoFill();
        ofSetLineWidth(4);

    if (cCirc) { //if statement that controls whether or not the circles and the white shape should be drawn
        ofSetColor(0, 0, 200);
        ofCircle(pos.x, pos.y, 40);

        ofSetColor(200, 0, 0);
        ofCircle(pos2.x, pos2.y, 40);

        ofSetColor(200,200,0);
        ofCircle(pos3.x, pos3.y, 40);

        ofSetColor(0,200,0);
        ofCircle(pos4.x, pos4.y, 40);

//
//        //_---------------------//
        ofSetColor(255);
        ofSetPolyMode(OF_POLY_WINDING_NONZERO);
        ofBeginShape();
            ofVertex(pos.x, pos.y);   // blue
            ofVertex(pos2.x, pos2.y); // red
            ofVertex(pos4.x, pos4.y); // yellow
            ofVertex(pos3.x, pos3.y); // green
        ofEndShape();
        } // end of cCirc
//
//        //--------------------//
        ofSetColor(255);
        if( pos.x>10 && pos.y > 10 && pic)
        //ofEnableBlendMode(OF_BLENDMODE_MULTIPLY);
       warpImg.draw(pos.x, pos.y, 250, 250);


        //--------------------//


    }
}

void testApp::keyPressed(int key) // keypressed function that allows us to show different states, such as the circles, picture and rect.
{
     if ( key == 'g' || key == 'G'){
         ValleRect = Adam = true;
         }
    if ( key == 'p' || key == 'P'){
         pic = Adam = true;
         }
    if ( key == 'v' || key == 'V'){
         cCirc = Adam = true;
         }

    if ( key == 'c' || key == 'C'){
         pic = Adam = cCirc = false;
         }

}



void testApp::trackRect() { //the function that tracks the colors

    pos=ofPoint(0,0);
    int n=0;
    pos2=ofPoint(0,0);
    int n2=0;
    pos3=ofPoint(0,0);
    int n3=0;
    pos4=ofPoint(0,0);
    int n4=0;
    //the above are resetting the points and counters for each color every time it runs through.


    unsigned char * pixels = vidGrabber.getPixels();

    for (int y= 0; y < camHeight; y++) {
        for (int x= 0; x < camWidth; x++) {

            r = pixels[(y*camWidth+x)*3 + 0];
            g = pixels[(y*camWidth+x)*3 + 1];
            b = pixels[(y*camWidth+x)*3 + 2];

            result =    (b> g+30 && b> r+50 )? 255: 0; // blue
            result2 =   (r> g+50 && r> b+30 )? 255: 0; // red
            result3 =   (r> b+50 && g> b+50 )? 255: 0; // yellow
            result4 =   (g> r+20 && g> b+40 )? 255: 0; // green

            if (result==255){
                pos.x+=x; // adds the x position to the pos.x
                pos.y+=y; // adds the y position to the pos.y
                n++; // increases the counter by 1 for each runthrough
            }
            if (result2==255){
                pos2.x+=x;
                pos2.y+=y;
                n2++;
            }
            if (result3==255){
                pos3.x+=x;
                pos3.y+=y;
                n3++;
            }
            if (result4==255){
                pos4.x+=x;
                pos4.y+=y;
                n4++;
            }
        }
    }
    if (n>0)
        {
            pos.x/=n; // divides the pos.x with our counter in order to find the middle of the color field
            pos.y/=n; // divides the pos.y with our counter in order to find the middle of the color field
        }
    if (n2>0)
        {
            pos2.x/=n2;
            pos2.y/=n2;
        }
    if (n3>0)
        {
            pos3.x/=n3;
            pos3.y/=n3;
        }
    if (n4>0)
        {
            pos4.x/=n4;
            pos4.y/=n4;
        }


//	if(leftPoints.size() >= 4) {
//		vector<Point2f> srcPoints, dstPoints;
//		for(int i = 0; i < leftPoints.size(); i++) {
//			srcPoints.push_back(Point2f(rightPoints[i].x - left.getWidth(), rightPoints[i].y));
//			dstPoints.push_back(Point2f(leftPoints[i].x, leftPoints[i].y));
//		}
//----------------------------------------------//
    if (!(origPoints.size() < 4))
    {
        warpPoints.push_back(ofVec2f(pos.x,pos.y)); // blue
        warpPoints.push_back(ofVec2f(pos2.x,pos2.y)); // red
        warpPoints.push_back(ofVec2f(pos3.x,pos3.y)); // yellow
        warpPoints.push_back(ofVec2f(pos4.x,pos4.y)); // green
        vector<Point2f>     srcPoints, dstPoints;

        for(n = 0; n < 4; n++)
        {
            srcPoints.push_back(Point2f(origPoints[n].x,origPoints[n].y));
            dstPoints.push_back(Point2f(warpPoints[n].x,warpPoints[n].y));
        }

        homography = findHomography(Mat(dstPoints), Mat(srcPoints));
  		homographyReady = true;

    }

    if (homographyReady) {
    // this is how you warp one ofImage into another ofImage given the homography matrix
		// CV INTER NN is 113 fps, CV_INTER_LINEAR is 93 fps
//    warpPerspective(right, warpedColor, homography, CV_INTER_LINEAR);
    warpPerspective(origImg, warpImg, homography, CV_INTER_LINEAR);
	warpImg.update();
    }
    videoTexture.loadData(pixels, camWidth,camHeight, GL_RGB); // loads the pixels (original input) in to our texture
}