#include "opencv/cv.h"
#include "opencv2/highgui/highgui.hpp"
#include <stdio.h>

using namespace cv;
//using namespace std;




int main(int argc, char* argv){

    #define MARKER_SIZE (50)
    /* size of marker (Millimeter)*/
    CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml");
    CvMat *distortion = (CvMat*)cvLoad("Distortion.xml");

//----------------------------------------//
     CvMat *intrinsic_matrix = cvCreateMat( 3, 3, CV_32FC1 );
 /* double fu = 794.54;
    double fv = 774.31;
    double cx = 320.0;
    double cy = 240.0;

    cvZero(intrinsic_matrix);
    cvSetReal2D(intrinsic_matrix,0,0,fu);
    cvSetReal2D(intrinsic_matrix,1,1,fv);
    cvSetReal2D(intrinsic_matrix,0,2,cx);
    cvSetReal2D(intrinsic_matrix,1,2,cy);
    cvSetReal2D(intrinsic_matrix,2,2, 1);*/
 CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f;
 CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f;
    //----------------------------------------//

    int i;

    CvMat object_points;
    CvMat image_points;

    CvMat *rotation = cvCreateMat (1, 3, CV_32FC1); // [ X X X ] (1 row, 3 columns), 32bit-float, 1-channel
    CvMat *translation = cvCreateMat (1 , 3, CV_32FC1);

    //Axis coordinate generation
    CvMat *srcPoints3D = cvCreateMat (4, 1, CV_32FC3);//The original three-dimensional coordinates
    CvMat *dstPoints2D = cvCreateMat (4, 1, CV_32FC3);//Two-dimensional coordinates on the screen when the projection

    CvPoint3D32f baseMarkerPoints[4];

    //In the physical space that specifies whether a box which will coordinate.
    baseMarkerPoints[0].x =(float) 0 * MARKER_SIZE;
    baseMarkerPoints[0].y =(float) 0 * MARKER_SIZE;
    baseMarkerPoints[0].z = 0.0;

    baseMarkerPoints[1].x =(float) 0 * MARKER_SIZE;
    baseMarkerPoints[1].y =(float) 1 * MARKER_SIZE;
    baseMarkerPoints[1].z = 0.0;

    baseMarkerPoints[2].x =(float) 1 * MARKER_SIZE;
    baseMarkerPoints[2].y =(float) 1 * MARKER_SIZE;
    baseMarkerPoints[2].z = 0.0;

    baseMarkerPoints[3].x =(float) 1 * MARKER_SIZE;
    baseMarkerPoints[3].y =(float) 0 * MARKER_SIZE;
    baseMarkerPoints[3].z = 0.0;

     char text[255] = "Test";

    CvFont axisfont;
    float axhscale      = 0.8f;
    float axvscale      = 0.8f;
    float italicscale = 0.0f;
    int  thickness    = 1;
    cvInitFont (&axisfont, CV_FONT_HERSHEY_SIMPLEX , axhscale, axvscale, italicscale, thickness, CV_AA);

  //Determine the coordinates of the axis base.
    for ( i=0;i<4;i++)
    {
        switch (i)
        {
            case 0: srcPoints3D->data.fl[0]     =0;
                    srcPoints3D->data.fl[1]     =0;
                    srcPoints3D->data.fl[2]     =0;
                    break;
            case 1: srcPoints3D->data.fl[0+i*3] =(float)MARKER_SIZE;
                    srcPoints3D->data.fl[1+i*3] =0;
                    srcPoints3D->data.fl[2+i*3] =0;
                    break;
            case 2: srcPoints3D->data.fl[0+i*3] =0;
                    srcPoints3D->data.fl[1+i*3] =(float)MARKER_SIZE;
                    srcPoints3D->data.fl[2+i*3] =0;
                    break;
            case 3: srcPoints3D->data.fl[0+i*3] =0;
                    srcPoints3D->data.fl[1+i*3] =0;
                    srcPoints3D->data.fl[2+i*3] =-(float)MARKER_SIZE;;
                    break;

        }
    }


    CvCapture* capture = cvCaptureFromCAM(1);
    IplImage* frame = cvQueryFrame(capture);

    IplImage* image = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U,3);
    IplImage* gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U,1);
    IplImage* grayContour = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U,1);

    CvMemStorage *storage = cvCreateMemStorage (0);
    CvMemStorage *storagepoly = cvCreateMemStorage (0);

    CvSeq *firstcontour=NULL; //first contour found
    CvSeq *polycontour=NULL;

    int contourCount;

    IplImage *marker_inside=cvCreateImage(cvSize(50,50),IPL_DEPTH_8U,1);

    CvMat *map_matrix;
    CvPoint2D32f src_pnt[4], dst_pnt[4], tmp_pnt[4];

    dst_pnt[0] = cvPoint2D32f (0, 0);
    dst_pnt[1] = cvPoint2D32f (marker_inside->width, 0);
    dst_pnt[2] = cvPoint2D32f (marker_inside->width, marker_inside->height);
    dst_pnt[3] = cvPoint2D32f (0, marker_inside->height);
    map_matrix = cvCreateMat (3, 3, CV_32FC1);

    cvNamedWindow ("marker_inside", CV_WINDOW_AUTOSIZE);
    //cvNamedWindow ("capture_image", CV_WINDOW_AUTOSIZE);


    char chr;

    cvNamedWindow("Camera");
    cvNamedWindow("grayscale-threshold");
    cvNamedWindow("grayscale-inverse");
    cvNamedWindow("grayscale-smooth");

    while(1){
        frame = cvQueryFrame(capture);
        cvCopy(frame,image);



        cvCvtColor(image,gray,CV_BGR2GRAY);

        cvSmooth(gray,gray,CV_GAUSSIAN,3);
        cvShowImage("grayscale-smooth", gray);

        cvThreshold (gray, gray, 0, 255, CV_THRESH_BINARY | CV_THRESH_OTSU);
        cvShowImage("grayscale-threshold", gray);

        cvNot(gray,gray);
        cvShowImage("grayscale-inverse", gray);

        cvCopy(gray,grayContour);

        contourCount=cvFindContours (grayContour, storage, &firstcontour, sizeof (CvContour), CV_RETR_CCOMP);
        //printf("\nContours: %d", contourCount);

        polycontour=cvApproxPoly(firstcontour,sizeof(CvContour),storagepoly,CV_POLY_APPROX_DP,3,1);


        for(CvSeq* c=polycontour;c!=NULL;c=c->h_next) // loop till end of Seq (last found contour)
        {

            if(c->total==4)
            {

                    int nearestindex=0;


                    //Draw marker's contour
                    cvDrawContours(image,c,CV_RGB(255,255,0),CV_RGB(200,255,255),0);

                    float xlist[4];
                        float ylist[4];
                        for(int n=0;n<4;n++) //get point by point (total 4 points since we're detecting quadrilaterals) in Sequence
                        {
                            CvPoint* p=CV_GET_SEQ_ELEM(CvPoint,c,n);
                            tmp_pnt[n].x=(float)p->x;
                            tmp_pnt[n].y=(float)p->y;
                        /*  xlist[n]=(float)p->x;
                            ylist[n]=(float)p->y;*/
                        }

                            src_pnt[0].x=tmp_pnt[0].x;
                            src_pnt[0].y=tmp_pnt[0].y;
                            src_pnt[1].x=tmp_pnt[3].x;
                            src_pnt[1].y=tmp_pnt[3].y;
                            src_pnt[2].x=tmp_pnt[2].x;
                            src_pnt[2].y=tmp_pnt[2].y;
                            src_pnt[3].x=tmp_pnt[1].x;
                            src_pnt[3].y=tmp_pnt[1].y;

                        //Only pass the box information. I still do not know that either way
                        cvGetPerspectiveTransform (tmp_pnt, dst_pnt, map_matrix);

                        //Deforming the inner square of the marker
                        cvWarpPerspective (gray, marker_inside, map_matrix, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS, cvScalarAll (0));


                        //Set the coordinates of the markers on the image.
                        //create a Mat with points in src_pnt (perspective points)
                        cvInitMatHeader (&image_points, 4, 1, CV_32FC2, src_pnt);

                        //Underlying set of marker coordinates
                        // create Mat with real points with actual distances) (unwarped real 50*50 square)
                        cvInitMatHeader (&object_points, 4, 3, CV_32FC1, baseMarkerPoints);

                        //cvFindExtrinsicCameraParams2(&object_points,&image_points,intrinsic,distortion,rotation,translation);
cvFindExtrinsicCameraParams2(&object_points,&image_points,intrinsic_matrix,NULL,rotation,translation);

                        //Those obtained by using calculations that do come to any location on the screen coordinates of real space
                        //cvProjectPoints2(srcPoints3D,rotation,translation,intrinsic,distortion,dstPoints2D);

 int total = srcPoints3D->rows * srcPoints3D->cols * CV_MAT_CN(srcPoints3D->type);

 printf("%d",total);

cvProjectPoints2(srcPoints3D,rotation,translation,intrinsic_matrix,NULL,dstPoints2D);
                        //Draw a number of markers
                        //cvPutText(image, text,cvPoint((int)(dstPoints2D->data.fl[3]+dstPoints2D->data.fl[6])/2,(int)(dstPoints2D->data.fl[4]+dstPoints2D->data.fl[7])/2), &axisfont, CV_RGB(255, 255, 100));
                        //cvPutText(image, text,cvPoint(20,20), &axisfont, CV_RGB(255, 255, 100));
            }
        }

        cvShowImage("Camera", image);

        cvShowImage("marker_inside",marker_inside);

        chr = cvWaitKey(50);
        if(chr ==27)
            break;

    }

    return 0;
}