#include "stdafx.h"

#define MARKER_BINS 7	// [-]
#define BIN_SIZE 8	// [px]
#define MARKER_SIZE ( MARKER_BINS * BIN_SIZE ) // [px]

const std::vector<cv::Point2f> MBAR::corners_ = { cv::Point( 0, 0 ),
	cv::Point( MARKER_SIZE - 1, 0 ), cv::Point( MARKER_SIZE - 1, MARKER_SIZE - 1 ),
	cv::Point( 0, MARKER_SIZE - 1 ) };

const std::vector<cv::Point3f> MBAR::corners_ws_ = { cv::Point3f( -3.0f, 3.0f, 0.0f ),
cv::Point3f( 3.0f, 3.0f, 0.0f ), cv::Point3f( 3.0f, -3.0f, 0.0f ), cv::Point3f( -3.0f, -3.0f, 0.0f ) };

const std::vector<cv::Point3f> MBAR::axis_ws_ = { cv::Point3f( 0.0f, 0.0f, 0.0f ),
cv::Point3f( 1.0f, 0.0f, 0.0f ), cv::Point3f( 0.0f, 1.0f, 0.0f ), cv::Point3f( 0.0f, 0.0f, 1.0f ) };

//http://www.mathaddict.net/hamming.htm
//http://homepages.bw.edu/~rmolmen/multimedia/hammingcode.swf
unsigned char encode_2bits( const unsigned char data2 )
{
	return 0;
}

inline unsigned char get_bit( const unsigned char byte, const char bit )
{
	assert( bit >= 0 && bit < 8 );

	return ( ( byte >> bit ) & 1 )/* == 1*/;
}

unsigned char decode_2bits( const unsigned char codeword5, bool & error )
{
	return 0;
}

MBAR::MBAR( const std::string & file_name )
{
	image_ = cv::imread( file_name );
}

void MBAR::start( const std::string & file_name )
{
	image_ = cv::imread( file_name );
	start();
}

template<typename type>
std::string to_binary( const type & value )
{
	std::bitset<sizeof( type ) * 8> bs( value );

	return bs.to_string();
}

void MBAR::start()
{	
	contours_.clear();
	markers_.clear();

	preprocess_image();
	find_contours();
	crop_markers();
	read_markers();
	refine_corners();
	solve_pnp();
}

void MBAR::preprocess_image()
{	

}

void MBAR::find_contours()
{

}

void MBAR::crop_markers()
{

}

void MBAR::read_markers()
{

}

void MBAR::refine_corners()
{

}

void MBAR::solve_pnp()
{
	cv::Mat m_camera = camera_matrix();
	cv::Mat distortion_coefficients = cv::Mat::zeros( 4, 1, CV_64FC1 );
	double & k1 = distortion_coefficients.at<double>( 0, 0 );
	double & k2 = distortion_coefficients.at<double>( 1, 0 );
	double & p1 = distortion_coefficients.at<double>( 2, 0 );
	double & p2 = distortion_coefficients.at<double>( 3, 0 );
	k1 = 0; // radial distortion
	k2 = 0;
	p1 = 0; // tangential distortion
	p2 = 0;
}

cv::Mat MBAR::camera_matrix() const
{
	cv::Mat camera_matrix = cv::Mat::zeros( 3, 3, CV_64FC1 );
	double & fx = camera_matrix.ptr<double>( 0 )[0];
	double & fy = camera_matrix.ptr<double>( 1 )[1];
	double & cx = camera_matrix.ptr<double>( 0 )[2];
	double & cy = camera_matrix.ptr<double>( 1 )[2];	
	camera_matrix.ptr<double>( 2 )[2] = 1.0;
	const cv::Size size = image_.size();
	const double fov_y = DEG2RAD( 42.185 );
	fx = ( size.height * 0.5 ) / tan( fov_y * 0.5 );
	fy = fx;
	cx = size.width * 0.5;
	cy = size.height * 0.5;

	return camera_matrix;
}
