#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <inttypes.h>

#include "libcommon/timestamp.h"
#include "libcommon/units.h"

#include "libmath/fasttrig.h"
#include "libmath/gsl_util.h"
#include "libmath/ssc.h"

int
main (int argc, char *argv[])
{
    fasttrig_init ();

    GSLU_VECTOR_VIEW (x_ij, 6);
    ssc_pose_set_i (x_ij.data, 1, 2, 3, 4, 5, 6);
    gslu_vector_printf_row (&x_ij.vector, "x_ij", NULL);

    GSLU_VECTOR_VIEW (x_jk, 6);
    ssc_pose_set_i (x_jk.data, 6, 5, 4, 3, 2, 1);
    gslu_vector_printf_row (&x_jk.vector, "x_jk", NULL);

    // ssc_inverse
    printf ("\nssc_inverse: x_ji = (-)x_ij\n");
    GSLU_VECTOR_VIEW (x_ji, 6);
    GSLU_MATRIX_VIEW (Jminus, 6, 6);
    ssc_inverse (x_ji.data, Jminus.data, x_ij.data);
    gslu_vector_printf_row (&x_ji.vector, "x_ji", NULL);
    gslu_matrix_printf (&Jminus.matrix, "Jminus", NULL);


    // ssc_head2tail
    printf ("\nssc_head2tail: x_ik = x_ij (+) x_jk\n");
    GSLU_VECTOR_VIEW (x_ik, 6);
    GSLU_MATRIX_VIEW (Jplus, 6, 12);
    ssc_head2tail (x_ik.data, Jplus.data, x_ij.data, x_jk.data);
    gslu_vector_printf_row (&x_ik.vector, "x_ik", NULL);
    gslu_matrix_printf (&Jplus.matrix, "Jplus", NULL);


    // ssc_tail2tail
    printf ("\nssc_tail2tail: x_ik = (-)x_ji (+) x_jk\n");
    GSLU_VECTOR_VIEW (x_ik2, 6);
    GSLU_MATRIX_VIEW (Jtail, 6, 12);
    ssc_tail2tail (x_ik2.data, Jtail.data, x_ji.data, x_jk.data);
    gslu_vector_printf_row (&x_ik2.vector, "x_ik2", NULL);
    gslu_matrix_printf (&Jtail.matrix, "Jtail", NULL);

}
