// test file for HSO3.hpp and HSO4.hpp

//  (C) Copyright Hubert Holin 2001. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.


#include <iostream>

#include <boost/math/quaternion.hpp>

#include "HSO3.hpp"
#include "HSO4.hpp"


const int   number_of_intervals = 5;

const float   pi = ::std::atan(1.0f)*4;



void   test_SO3();
   
void   test_SO4();


int   main()

{
   test_SO3();
   
   test_SO4();
   
   ::std::cout << "That's all folks!" << ::std::endl;
}


//
//   Test of quaternion and R^3 rotation relationship
//

void   test_SO3_spherical()
{
   ::std::cout << "Testing spherical:" << ::std::endl;
   ::std::cout << ::std::endl;
   
   const float   rho = 1.0f;
   
   float      theta;
   float      phi1;
   float      phi2;
   
   for   (int idxphi2 = 0; idxphi2 <= number_of_intervals; idxphi2++)
   {
      phi2 = (-pi/2)+(idxphi2*pi)/number_of_intervals;
      
      for   (int idxphi1 = 0; idxphi1 <= number_of_intervals; idxphi1++)
      {
         phi1 = (-pi/2)+(idxphi1*pi)/number_of_intervals;
         
         for   (int idxtheta = 0; idxtheta <= number_of_intervals; idxtheta++)
         {
            theta = -pi+(idxtheta*(2*pi))/number_of_intervals;
            
            //::std::cout << "theta = " << theta << " ; ";
            //::std::cout << "phi1 = " << phi1 << " ; ";
            //::std::cout << "phi2 = " << phi2;
            //::std::cout << ::std::endl;
            
            ::boost::quaternion<float>   q = ::boost::spherical(rho, theta, phi1, phi2);
            
            //::std::cout << "q = " << q << ::std::endl;
            
            R3_matrix<float>         rot = quaternion_to_R3_rotation(q);
            
            //::std::cout << "rot = ";
            //::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;
            
            ::boost::quaternion<float>   p = R3_rotation_to_quaternion(rot, &q);
            
            //::std::cout << "p = " << p << ::std::endl;
            
            //::std::cout << "round trip discrepancy: " << ::boost::abs(q-p) << ::std::endl;
            
            //::std::cout << ::std::endl;
         }
      }
   }
   
   ::std::cout << ::std::endl;
}

   
void   test_SO3_semipolar()
{
   ::std::cout << "Testing semipolar:" << ::std::endl;
   ::std::cout << ::std::endl;
   
   const float   rho = 1.0f;
   
   float      alpha;
   float      theta1;
   float      theta2;
   
   for   (int idxalpha = 0; idxalpha <= number_of_intervals; idxalpha++)
   {
      alpha = (idxalpha*(pi/2))/number_of_intervals;
      
      for   (int idxtheta1 = 0; idxtheta1 <= number_of_intervals; idxtheta1++)
      {
         theta1 = -pi+(idxtheta1*(2*pi))/number_of_intervals;
         
         for   (int idxtheta2 = 0; idxtheta2 <= number_of_intervals; idxtheta2++)
         {
            theta2 = -pi+(idxtheta2*(2*pi))/number_of_intervals;
            
            //::std::cout << "alpha = " << alpha << " ; ";
            //::std::cout << "theta1 = " << theta1 << " ; ";
            //::std::cout << "theta2 = " << theta2;
            //::std::cout << ::std::endl;
            
            ::boost::quaternion<float>   q = ::boost::semipolar(rho, alpha, theta1, theta2);
            
            //::std::cout << "q = " << q << ::std::endl;
            
            R3_matrix<float>         rot = quaternion_to_R3_rotation(q);
            
            //::std::cout << "rot = ";
            //::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;
            
            ::boost::quaternion<float>   p = R3_rotation_to_quaternion(rot, &q);
            
            //::std::cout << "p = " << p << ::std::endl;
            
            //::std::cout << "round trip discrepancy: " << ::boost::abs(q-p) << ::std::endl;
            
            //::std::cout << ::std::endl;
         }
      }
   }
   
   ::std::cout << ::std::endl;
}

   
void   test_SO3_multipolar()
{
   ::std::cout << "Testing multipolar:" << ::std::endl;
   ::std::cout << ::std::endl;
   
   float   rho1;
   float   rho2;
   
   float   theta1;
   float   theta2;
   
   for   (int idxrho = 0; idxrho <= number_of_intervals; idxrho++)
   {
      rho1 = (idxrho*1.0f)/number_of_intervals;
      rho2 = ::std::sqrt(1.0f-rho1*rho1);
      
      for   (int idxtheta1 = 0; idxtheta1 <= number_of_intervals; idxtheta1++)
      {
         theta1 = -pi+(idxtheta1*(2*pi))/number_of_intervals;
         
         for   (int idxtheta2 = 0; idxtheta2 <= number_of_intervals; idxtheta2++)
         {
            theta2 = -pi+(idxtheta2*(2*pi))/number_of_intervals;
            
            //::std::cout << "rho1 = " << rho1 << " ; ";
            //::std::cout << "theta1 = " << theta1 << " ; ";
            //::std::cout << "theta2 = " << theta2;
            //::std::cout << ::std::endl;
            
            ::boost::quaternion<float>   q = ::boost::multipolar(rho1, theta1, rho2, theta2);
            
            //::std::cout << "q = " << q << ::std::endl;
            
            R3_matrix<float>         rot = quaternion_to_R3_rotation(q);
            
            //::std::cout << "rot = ";
            //::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;
            
            ::boost::quaternion<float>   p = R3_rotation_to_quaternion(rot, &q);
            
            //::std::cout << "p = " << p << ::std::endl;
            
            //::std::cout << "round trip discrepancy: " << ::boost::abs(q-p) << ::std::endl;
            
            //::std::cout << ::std::endl;
         }
      }
   }
   
   ::std::cout << ::std::endl;
}

   
void   test_SO3_cylindrospherical()
{
   ::std::cout << "Testing cylindrospherical:" << ::std::endl;
   ::std::cout << ::std::endl;
   
   float   t;
   
   float   radius;
   float   longitude;
   float   latitude;
   
   for   (int idxt = 0; idxt <= number_of_intervals; idxt++)
   {
      t = -1.0f+(idxt*2.0f)/number_of_intervals;
      radius = ::std::sqrt(1.0f-t*t);
      
      for   (int idxlatitude = 0; idxlatitude <= number_of_intervals; idxlatitude++)
      {
         latitude = (-pi/2)+(idxlatitude*pi)/number_of_intervals;
         
         for   (int idxlongitude = 0; idxlongitude <= number_of_intervals; idxlongitude++)
         {
            longitude = -pi+(idxlongitude*(2*pi))/number_of_intervals;
            
            //::std::cout << "t = " << t << " ; ";
            //::std::cout << "longitude = " << longitude;
            //::std::cout << "latitude = " << latitude;
            //::std::cout << ::std::endl;
            
            ::boost::quaternion<float>   q = ::boost::cylindrospherical(t, radius, longitude, latitude);
            
            //::std::cout << "q = " << q << ::std::endl;
            
            R3_matrix<float>         rot = quaternion_to_R3_rotation(q);
            
            //::std::cout << "rot = ";
            //::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;
            
            ::boost::quaternion<float>   p = R3_rotation_to_quaternion(rot, &q);
            
            //::std::cout << "p = " << p << ::std::endl;
            
            //::std::cout << "round trip discrepancy: " << ::boost::abs(q-p) << ::std::endl;
            
            //::std::cout << ::std::endl;
         }
      }
   }
   
   ::std::cout << ::std::endl;
}

   
void   test_SO3_cylindrical()
{
   ::std::cout << "Testing cylindrical:" << ::std::endl;
   ::std::cout << ::std::endl;
   
   float   r;
   float   angle;
   
   float   h1;
   float   h2;
   
   for   (int idxh2 = 0; idxh2 <= number_of_intervals; idxh2++)
   {
      h2 = -1.0f+(idxh2*2.0f)/number_of_intervals;
      
      for   (int idxh1 = 0; idxh1 <= number_of_intervals; idxh1++)
      {
         h1 = ::std::sqrt(1.0f-h2*h2)*(-1.0f+(idxh2*2.0f)/number_of_intervals);
         r = ::std::sqrt(1.0f-h1*h1-h2*h2);
         
         for   (int idxangle = 0; idxangle <= number_of_intervals; idxangle++)
         {
            angle = -pi+(idxangle*(2*pi))/number_of_intervals;
            
            //::std::cout << "angle = " << angle << " ; ";
            //::std::cout << "h1 = " << h1;
            //::std::cout << "h2 = " << h2;
            //::std::cout << ::std::endl;
            
            ::boost::quaternion<float>   q = ::boost::cylindrical(r, angle, h1, h2);
            
            //::std::cout << "q = " << q << ::std::endl;
            
            R3_matrix<float>         rot = quaternion_to_R3_rotation(q);
            
            //::std::cout << "rot = ";
            //::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << ::std::endl;
            //::std::cout << "\t";
            //::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << ::std::endl;
            
            ::boost::quaternion<float>   p = R3_rotation_to_quaternion(rot, &q);
            
            //::std::cout << "p = " << p << ::std::endl;
            
            //::std::cout << "round trip discrepancy: " << ::boost::abs(q-p) << ::std::endl;
            
            //::std::cout << ::std::endl;
         }
      }
   }
   
   ::std::cout << ::std::endl;
}


void   test_SO3()
{
   ::std::cout << "Testing SO3:" << ::std::endl;
   ::std::cout << ::std::endl;
   
   test_SO3_spherical();
   
   test_SO3_semipolar();
   
   test_SO3_multipolar();
   
   test_SO3_cylindrospherical();
   
   test_SO3_cylindrical();
}


//
//   Test of quaternion and R^4 rotation relationship
//

void   test_SO4_spherical()
{
   ::std::cout << "Testing spherical:" << ::std::endl;
   ::std::cout << ::std::endl;
   
   const float   rho1 = 1.0f;
   const float   rho2 = 1.0f;
   
   float      theta1;
   float      phi11;
   float      phi21;
   
   float      theta2;
   float      phi12;
   float      phi22;
   
   for   (int idxphi21 = 0; idxphi21 <= number_of_intervals; idxphi21++)
   {
      phi21 = (-pi/2)+(idxphi21*pi)/number_of_intervals;
      
      for   (int idxphi22 = 0; idxphi22 <= number_of_intervals; idxphi22++)
      {
         phi22 = (-pi/2)+(idxphi22*pi)/number_of_intervals;
         
         for   (int idxphi11 = 0; idxphi11 <= number_of_intervals; idxphi11++)
         {
            phi11 = (-pi/2)+(idxphi11*pi)/number_of_intervals;
            
            for   (int idxphi12 = 0; idxphi12 <= number_of_intervals; idxphi12++)
            {
               phi12 = (-pi/2)+(idxphi12*pi)/number_of_intervals;
               
               for   (int idxtheta1 = 0; idxtheta1 <= number_of_intervals; idxtheta1++)
               {
                  theta1 = -pi+(idxtheta1*(2*pi))/number_of_intervals;
                  
                  for   (int idxtheta2 = 0; idxtheta2 <= number_of_intervals; idxtheta2++)
                  {
                     theta2 = -pi+(idxtheta2*(2*pi))/number_of_intervals;
                     
                     //::std::cout << "theta1 = " << theta1 << " ; ";
                     //::std::cout << "phi11 = " << phi11 << " ; ";
                     //::std::cout << "phi21 = " << phi21;
                     //::std::cout << "theta2 = " << theta2 << " ; ";
                     //::std::cout << "phi12 = " << phi12 << " ; ";
                     //::std::cout << "phi22 = " << phi22;
                     //::std::cout << ::std::endl;
                     
                     ::boost::quaternion<float>   p1 = ::boost::spherical(rho1, theta1, phi11, phi21);
                     
                     //::std::cout << "p1 = " << p1 << ::std::endl;
                     
                     ::boost::quaternion<float>   q1 = ::boost::spherical(rho2, theta2, phi12, phi22);
                     
                     //::std::cout << "q1 = " << q1 << ::std::endl;
                     
                     ::std::pair< ::boost::quaternion<float> , ::boost::quaternion<float> >   pq1 =
                        ::std::make_pair(p1,q1);
                     
                     R4_matrix<float>         rot = quaternions_to_R4_rotation(pq1);
                     
                     //::std::cout << "rot = ";
                     //::std::cout << "\t" << rot.a11 << "\t" << rot.a12 << "\t" << rot.a13 << "\t" << rot.a14 << ::std::endl;
                     //::std::cout << "\t";
                     //::std::cout << "\t" << rot.a21 << "\t" << rot.a22 << "\t" << rot.a23 << "\t" << rot.a24 << ::std::endl;
                     //::std::cout << "\t";
                     //::std::cout << "\t" << rot.a31 << "\t" << rot.a32 << "\t" << rot.a33 << "\t" << rot.a34 << ::std::endl;
                     //::std::cout << "\t";
                     //::std::cout << "\t" << rot.a41 << "\t" << rot.a42 << "\t" << rot.a43 << "\t" << rot.a44 << ::std::endl;
                     
                     ::std::pair< ::boost::quaternion<float> , ::boost::quaternion<float> >   pq2 =
                        R4_rotation_to_quaternions(rot, &pq1);
                     
                     //::std::cout << "p1 = " << pq.first << ::std::endl;
                     //::std::cout << "p2 = " << pq.second << ::std::endl;
                     
                     //::std::cout << "round trip discrepancy: " << ::std::sqrt(::boost::norm(pq1.first-pq2.first)+::boost::norm(pq1.second-pq2.second)) << ::std::endl;
                     
                     //::std::cout << ::std::endl;
                  }
               }
            }
         }
      }
   }
   
   ::std::cout << ::std::endl;
}


void   test_SO4()
{
   ::std::cout << "Testing SO4:" << ::std::endl;
   ::std::cout << ::std::endl;
   
   test_SO4_spherical();
}

