/* LaHaShem HaAretz U'Mloah */
/* Copyright 2012, 2013, 2014, and 2015 Purdue University. All rights reserved.
 */

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <stdio.h>
#include <vector>
#include <iostream>
#include <stdlib.h>
#include <string.h>
using namespace cv;
using namespace std;
#include "sftlib-c.h"

// needs work: to rename
//   imagePoints
//   boardSize
//   squareSize
//   pointbuf
//   cameraMatrix
//   distCoeffs
//   objectPoints
//   rvecs
//   tvecs
//   CV_64F

extern "C" int calibrate_undistort(void) {
  /*
  if (saving_frame_number<1) return FALSE;
  vector<vector<Point2f> > imagePoints;
  // hardcoded
  Size boardSize(7, 5);
  float squareSize = 3;
  for (unsigned int frame = 0; frame<saving_frame_number; frame++) {
    imlib_context_set_image(saved_frames[frame]);
    Mat image = Mat(imlib_image_get_height(), imlib_image_get_width(),
                    CV_8UC4,
                    imlib_image_get_data_for_reading_only());
    vector<Point2f> pointbuf;
    if (findChessboardCorners(image, boardSize, pointbuf,
			      CV_CALIB_CB_ADAPTIVE_THRESH|
			      CV_CALIB_CB_FAST_CHECK|
			      CV_CALIB_CB_NORMALIZE_IMAGE)) {
      Mat gray;
      cvtColor(image, gray, CV_BGRA2GRAY);
      cornerSubPix(gray, pointbuf, Size(11, 11), Size(-1, -1),
                   TermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));
      imagePoints.push_back(pointbuf);
    }
  }
  Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
  Mat distCoeffs = Mat::zeros(5, 1, CV_64F);
  vector<vector<Point3f> > objectPoints(1);
  for (int i = 0; i<boardSize.height; i++) {
    for (int j = 0; j<boardSize.width; j++) {
      objectPoints[0].push_back(Point3f((float)j*squareSize,
					(float)i*squareSize,
					0));
    }
  }
  objectPoints.resize(imagePoints.size(), objectPoints[0]);
  vector<Mat> rvecs, tvecs;
  printf("Number of images with points %ld\n", imagePoints.size());
  if (imagePoints.size()<1) return FALSE;
  double rms = calibrateCamera(objectPoints,
                               imagePoints,
                               Size(imlib_image_get_width(),
                                    imlib_image_get_height()),
                               cameraMatrix,
                               distCoeffs,
                               rvecs,
                               tvecs,
                               CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
  printf("rms %f\n", rms);
  printf("((");
  for (unsigned int x = 0; x<3; x++) {
    for (unsigned int y = 0; y<3; y++) {
      printf("%f ", cameraMatrix.at<double>(x, y));
    }
  }
  printf(")\n (");
  for (unsigned int i = 0; i<5; i++) {
    printf("%f ", distCoeffs.at<double>(0, i));
  }
  printf("))\n");
  memcpy(&calibration[0], cameraMatrix.data, 9*sizeof(double));
  memcpy(&distortion[0], distCoeffs.data, 5*sizeof(double));
  */
  return TRUE;
}

/* Tam V'Nishlam Shevah L'El Borei Olam */
