//#include <stdio.h>
//#include <sstream>
//#include <iostream>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

const float SquareDimention = 1.0f;
const Size ChessboardSize = Size(7,7);
const int NumOfImages = 14;
const string Images[NumOfImages] = {
    "gals/1.jpg", "gals/2.jpg", "gals/3.jpg", "gals/4.jpg", "gals/5.jpg",
    "gals/6.jpg", "gals/7.jpg", "gals/8.jpg", "gals/9.jpg", "gals/10.jpg",
    "gals/11.jpg", "gals/12.jpg", "gals/13.jpg", "gals/14.jpg"
};

int main(int argc, char** argv)
{
    vector<Mat> openedImages;

    for (int i=0; i<NumOfImages; i++)
    {
	openedImages.push_back(imread(Images[i], CV_LOAD_IMAGE_COLOR));
    }

    vector<vector<Point2f> > imageSpacePoints;

    for (vector<Mat>::iterator i = openedImages.begin(); i != openedImages.end(); i++)
    {
	vector<Point2f> spacePointBuffer;

	bool found = findChessboardCorners(*i, ChessboardSize, spacePointBuffer,
	    CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);

	if (found)
	{
	    imageSpacePoints.push_back(spacePointBuffer);
	}
    }

    vector<vector<Point3f> > worldSpacePoints(1);
    for (int i=0; i<ChessboardSize.height; i++)
	for (int j=0; j<ChessboardSize.width; j++)
	    worldSpacePoints[0].push_back(Point3f(float(j*SquareDimention),
		float(i*SquareDimention), 0.0f));

    worldSpacePoints.resize(imageSpacePoints.size(), worldSpacePoints[0]);

    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
    Mat distanceCoefficients = Mat::zeros(8, 1, CV_64F);
    vector<Mat> rVectors, tVectors;

    calibrateCamera(worldSpacePoints, imageSpacePoints, ChessboardSize, cameraMatrix,
	distanceCoefficients, rVectors, tVectors);

    cout << "Camera Matrix: " << endl << cameraMatrix << endl;
    cout << "Cooef Matrix: " << endl <<  distanceCoefficients << endl;

    /*
    // Just a helper
    int ctr = 0;
    for (vector<vector<Point2f> >::iterator i = imageSpacePoints.begin(); i != imageSpacePoints.end(); i++)
    {
	vector<Point2f> pointBuffer = *i;
	for (vector<Point2f>::iterator j = pointBuffer.begin(); j != pointBuffer.end(); j++)
	{
	    cout << *j << endl;
	}
	++ctr;
	cout << "--------------------[ " << ctr << " ]-----------------------" << endl;
    }

    for (int i=0; i<rVectors.size(); i++)
    {
	cout << "RVect: " << rVectors[i] << endl;
    }
    */

    return 0;
}