#include <stdio.h>
#include <stdlib.h>

#include "libmath/gsl_util.h"
#include "libmath/so3.h"

#define DTOR (M_PI / 180.)
#define RTOD (180. / M_PI)

int
main (int argc, char *argv[])
{
    // Rx using gsl_util matrix representation
    GSLU_MATRIX_VIEW (Rx, 3, 3);
    so3_rotx (Rx.data, 30*DTOR);
    gslu_matrix_printf (&Rx.matrix, "Rx", NULL);

    // Ry using double array representation
    double Ry[9];
    so3_roty (Ry, 30*DTOR);
    gslu_array_printf (Ry, 3, 3, "Ry", NULL);

    // Rz using gsl_matrix representation
    gsl_matrix *Rz = gsl_matrix_alloc (3, 3);
    so3_rotz (Rz->data, 30*DTOR);
    gslu_matrix_printf (Rz, "Rz", NULL);

    // convert Euler rph to SO3 rotation matrix R
    double R[3*3];
    double rph[] = {30*DTOR, 50*DTOR, 27*DTOR};
    so3_rotxyz (R, rph);
    gslu_array_printf (R, 3, 3, "R", NULL);


    // convert SO3 rotation matrix back to Euler rph
    so3_rot2rph (R, rph);
    gsl_vector_view rph_view = gsl_vector_view_array (rph, 3);
    gsl_vector_scale (&rph_view.vector, RTOD);
    gslu_vector_printf (&rph_view.vector, "rph", NULL);
}
