/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2010 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

You may alternatively use this source under the terms of a specific version of
the OpenSpace3D Unrestricted License provided you have obtained such a license from
I-maginer.
-----------------------------------------------------------------------------
*/

#include "Prerequisites.h"
#include "ArCameraParam.h"

ArCameraParam::ArCameraParam(int width, int height, float nearclip, float farclip, std::string yamlfile)
{
	SetBitmapSize(width, height);
	SetClipping(nearclip, farclip);
	SetCameraParameter(yamlfile);
}

ArCameraParam::~ArCameraParam()
{
}

aruco::CameraParameters ArCameraParam::GetCameraParameter()
{
	return camParam;
}

void ArCameraParam::SetCameraParameter(std::string yamlfile)
{
  // load yaml calibration file or default values
  try
  {
	  camParam.readFromXMLFile(yamlfile.c_str());
  }
  catch (cv::Exception e)
  {
	  cv::Mat Distorsion=cv::Mat::zeros(4,1,CV_32FC1);
    cv::Mat CameraMatrix=cv::Mat::eye(3,3,CV_32FC1);

    //set default camera values
    CameraMatrix.at<float>(0,0)= 757.36307741305950f;
    CameraMatrix.at<float>(0,2)= 323.15565634777062f;
    CameraMatrix.at<float>(1,1)= 757.05029823649602f;
    CameraMatrix.at<float>(1,2)= 239.85178811870270f;
    Distorsion.at<float>(0,0)= 0.058322574816106650f;
    Distorsion.at<float>(1,0)= -0.17482068549377444f;
    Distorsion.at<float>(2,0)= -0.0031083477039117124f;
    Distorsion.at<float>(3,0)= -0.0043781939644044129f;

	  camParam = aruco::CameraParameters(CameraMatrix, Distorsion, bitmapSize);
  }
}

cv::Size ArCameraParam::GetBitmapSize()
{
	return bitmapSize;
}

void ArCameraParam::SetBitmapSize(int width, int height)
{
	bitmapSize.height = height;
	bitmapSize.width = width;
}

cv::Point2f ArCameraParam::GetClipping()
{
	return clipping;
}

void ArCameraParam::SetClipping(float nearclip, float farclip)
{
	clipping.x = nearclip;
	clipping.y = farclip;
}

void ArCameraParam::GetProjectionMatrix(double m_proj[16], cv::Size screensize)
{
	try
	{
		aruco::MarkerDetector::glGetProjectionMatrix(camParam, bitmapSize, screensize, m_proj, clipping.x, clipping.y, true);
	}
	catch(cv::Exception e)
	{
		MMechostr(MSKDEBUG,"error : %s \n", e.what());
	}
}