/**
 * @file    timeCalibratedCamera.cpp
 * @brief   time CalibratedCamera derivatives
 * @author  Frank Dellaert
 */

#include <time.h>
#include <iostream>

#include <gtsam/geometry/CalibratedCamera.h>

using namespace std;
using namespace gtsam;

int main()
{
  int n = 100000;

  const Pose3 pose1(Matrix_(3,3,
  				      1., 0., 0.,
  				      0.,-1., 0.,
  				      0., 0.,-1.
  				      ),
  			      Point3(0,0,0.5));

  const CalibratedCamera camera(pose1);
  const Point3 point1(-0.08,-0.08, 0.0);

  // Aug 8, iMac 3.06GHz Core i3
  //  378870 calls/second
  //  2.63943 musecs/call
  // AFTER collapse:
	//  1.57399e+06 calls/second
	//  0.63533 musecs/call
  {
	Matrix computed;
  long timeLog = clock();
  for(int i = 0; i < n; i++)
  	computed = Dproject_pose(camera, point1);
  long timeLog2 = clock();
  double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
  cout << ((double)n/seconds) << " calls/second" << endl;
  cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
  }

  // Aug 8, iMac 3.06GHz Core i3
  // AFTER collapse:
	//  1.55383e+06 calls/second
	//  0.64357 musecs/call
  {
  	Matrix computed;
  long timeLog = clock();
  for(int i = 0; i < n; i++)
  	computed = Dproject_point(camera, point1);
  long timeLog2 = clock();
  double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
  cout << ((double)n/seconds) << " calls/second" << endl;
  cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
  }

  // Aug 8, iMac 3.06GHz Core i3
	//  371153 calls/second
	//  2.69431 musecs/call
  // AFTER collapse:
	//  1.10733e+06 calls/second
	//  0.90307 musecs/call
  {
	Matrix computed1, computed2;
  long timeLog = clock();
  for(int i = 0; i < n; i++)
  	Dproject_pose_point(camera, point1, computed1, computed2);
  long timeLog2 = clock();
  double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
  cout << ((double)n/seconds) << " calls/second" << endl;
  cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
  }

  return 0;
}