/*! \file rand3rot.cpp
        \brief Generates random 3D rotation matrices in C++.

		Compile using g++ -O3 rand3rot.cpp -lm
		To use it, remove main() and rename it as rand3rot.hpp.
		To create a random 3D rotation matrix, use the function
		rand3rot(). It needs a 3x3 matrix as input and writes 
		the random rotation matrix into it. The CheckRotMat()
		function checks whether a returned rand3rot() matrix is
		indeed a rotation matrix.
 */
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <ctime>
#include <cmath>

namespace reviver{

class Random
{
    double low, high;

public:
    Random(double l, double h) : low(l), high(h)
    {
        srand(unsigned(time(NULL))); // seeding with time
    }

    inline double operator()()
    {
        return rand()*(high - low)/RAND_MAX + low;
    }

}; 

inline void rand3rot(double M[3][3]){
	const double pi = 3.141592654;
	Random r(0,1);
	double RotMatrix[3][3];
	double x1,x2,x3;
	x1 = r();
	x2 = r();
	x3 = r();

	RotMatrix[0][0] = cos(2.0*pi*x1);
	RotMatrix[0][1] = sin(2.0*pi*x1);
	RotMatrix[0][2] = 0.0;

	RotMatrix[1][0] = -1.0 * sin(2.0*pi*x1);
	RotMatrix[1][1] = cos(2.0*pi*x1);
	RotMatrix[1][2] = 0.0;

	RotMatrix[2][0] = 0.0;
	RotMatrix[2][1] = 0.0;
	RotMatrix[2][2] = 1.0;

	double v[3] = {   sqrt(x3)*cos(2.0*pi*x2),
		              sqrt(x3)*sin(2.0*pi*x2), 
                      sqrt(1.0 - x3) };

	double vv_[3][3];
	vv_[0][0] = v[0] * v[0];
	vv_[0][1] = v[0] * v[1];
	vv_[0][2] = v[0] * v[2];

	vv_[1][0] = v[1] * v[0];
	vv_[1][1] = v[1] * v[1];
	vv_[1][2] = v[1] * v[2];

	vv_[2][0] = v[2] * v[0];
	vv_[2][1] = v[2] * v[1];
	vv_[2][2] = v[2] * v[2];

	double H[3][3];

	H[0][0] = 1.0 - 2*vv_[0][0];
	H[0][1] = 0.0 - 2*vv_[0][1];
	H[0][2] = 0.0 - 2*vv_[0][2];
	
	H[1][0] = 0.0 - 2*vv_[1][0];
	H[1][1] = 1.0 - 2*vv_[1][1];
	H[1][2] = 0.0 - 2*vv_[1][2];
	
	H[2][0] = 0.0 - 2*vv_[2][0];
	H[2][1] = 0.0 - 2*vv_[2][1];
	H[2][2] = 1.0 - 2*vv_[2][2];

	for (int i = 0; i < 3; ++i)
		for (int j = 0; j < 3; ++j ){
			M[i][j] = 0.0;
			for(int k = 0; k < 3; ++k){
				M[i][j] = (M[i][j] + H[i][k] * RotMatrix[k][j]);
			}
			M[i][j] *= -1.0;
		}	
}	

inline bool CheckRotMat(double randrot[3][3]){
		double I[3][3];
		for (int i = 0; i < 3; ++i)
		for (int j = 0; j < 3; ++j ){
			I[i][j] = 0.0;
			for(int k = 0; k < 3; ++k){
				I[i][j] = (I[i][j] + randrot[i][k] * randrot[j][k]);
			}
			
			if( i == j ){
				if ((fabs(I[i][j] - 1.0) ) > 0.0001){
					std::cerr << "\nError at " << i << ", " << j << " : " << I[i][j] 
					<< std::endl;
						return false;
				}
			} else 
				if (fabs(I[i][j]) > 0.0001){
					std::cerr << "\nError at " << i << ", " << j << " : " << I[i][j] 
					<< std::endl;
					return false;
				}
				
		}
		return true;

}

}

using namespace std;
using namespace reviver;

int main(void){

	double M[3][3];
	rand3rot(M);

	cout << setprecision(5) << fixed << endl;
	cout.setf(ios::showpos);

	cout << M[0][0] <<"\t" << M[0][1] << "\t" << M[0][2] << endl;
	cout << M[1][0] <<"\t" << M[1][1] << "\t" << M[1][2] << endl;
	cout << M[2][0] <<"\t" << M[2][1] << "\t" << M[2][2] << endl;

	cout << "\nChecking Random rotation Matrix: Returns: " << CheckRotMat(M) << endl; 
	return 0;
}