#include <assert.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <math.h>

#include "linalg.h"
#include "licks_message.h"

licks_message message;

int
main(int argc, char **argv) {
   if(argc==1) {
      printf("%s <command> <args...>\n",argv[0]);
      exit(0);
   }

   if(!strcmp(argv[1],"power")) {
      if(argc!=3) {
         printf("%s power {1|0}\n",argv[0]);
         exit(0);
      };
      message.type=MSG_POWER;
      message.i=atoi(argv[2]);
   } else if(!strcmp(argv[1],"quit")) {
      message.type=MSG_QUIT;
   } else if(!strcmp(argv[1],"move")) {
      if(argc!=6) { 
         printf("%s move x y z duration \n",argv[0]);
         exit(0);
      };
      message.type=MSG_MOVE_BODY;
      message.move_parameters.v.x=atoi(argv[2]);
      message.move_parameters.v.y=atoi(argv[3]);
      message.move_parameters.v.z=atoi(argv[4]);
      message.move_parameters.duration=atoi(argv[5]);
   } else if(!strcmp(argv[1],"walk")) {
      if(argc!=7) { 
         printf("%s walk x y a h d\n",argv[0]);
         exit(0);
      };
      message.type=MSG_WALK;
      message.walk_parameters.step_direction.x=atoi(argv[2]);
      message.walk_parameters.step_direction.y=atoi(argv[3]);
      message.walk_parameters.step_rotation=(M_PI/1800.0)*atoi(argv[4]);
      message.walk_parameters.step_height=atoi(argv[5]);
      message.walk_parameters.step_duration=atoi(argv[6]);
   }


   licks_socket_open("/tmp/ikcontrol");
   send_message(&message,ikd_path);
   licks_socket_close();
   return 0;
}