#include <iostream>
#include "NewGon.h"

int main ()
{
    NewGon myGon;
//    myGon.readParamFile("params.txt");
//    myGon.printParams();

    double mcs[9];
    //               SHx,SHy,SHz,  CHI,PHI,OMEGA,    Ox,Oy,Oz
    double ucs1[] = {27.35e-3,0,0,  0,0,    0, 188e-3,     0,     0};
    double ucs2[] = {27.35e-3,0,0,  0,0,    0, 188e-3, 10e-3,     0};
    double ucs3[] = {27.35e-3,0,0,  0,0,    0, 188e-3, 10e-3, 10e-3};
    double ucs4[] = {27.35e-3,0,0,  0,0,    0, 188e-3,-10e-3, 10e-3};
    double ucs5[] = {27.35e-3,0,0,  0,0,    0, 188e-3,-10e-3,-10e-3};
    double ucs6[] = {27.35e-3,0,0,  0,0,10e-3, 188e-3, 10e-3,     0};
    double ucs7[] = {27.35e-3,0,0, 20,0,    0, 188e-3,     0,     0};
    double ucs8[] = {27.35e-3,0,0, 40,0,    0, 188e-3,     0,     0};
    double ucs9[] = {27.35e-3,0,0, 50,0,    0, 188e-3,     0,     0};
    double ucs10[]= {27.35e-3,0,0, 60,0,    0, 188e-3,     0,     0};
    double ucs11[]= {27.35e-3,0,0, 70,0,    0, 188e-3,     0,     0};
    double ucs12[]= {27.35e-3,0,0, 80,90,    0, 188e-3,     0,     0};
    double ucs13[]= {27.35e-3,0,0, 90,0,    0, 188e-3,     0,     0};
 


    // motor values: q = (q1, q2,q3,q(theta), q5,q6)
    myGon.IK(ucs1, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs2, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs3, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs4, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs5, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs6, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs7, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs8, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs9, mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs10,mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs11,mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs12,mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);
    myGon.IK(ucs13,mcs); for (int i=0;i<6;i++) printf("%lf ", mcs[i]);



    return 0;
}