/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <stdio.h>

#include "agent/Motion/Kinematics.h"

void usage()
{
  printf("usage: kin_test <ers7|ers210>\n");
}

int main(int argc, char *argv[])
{
  if(argc < 2){
    printf("not enough arguments\n");
    usage();
    exit(1);
  }

  Kinematics kin;

  char *robot_platform=argv[1];
  if(strcmp(robot_platform,"ers7")==0){
    kin = Kinematics(Kinematics::ERS7);
  }else if(strcmp(robot_platform,"ers210")==0){
    kin = Kinematics(Kinematics::ERS210);
  }else{
    printf("bad platform '%s'\n",robot_platform);
    usage();
    exit(2);
  }

  double body_height = 125.0;
  double body_angle  = 0.0;

  bool done=false;
  while(!done){
    char buf[1024];
    char cmd[1025];
    char *ch;

    printf("\n");
    printf("b <height> <angle>                     ) set body height and angle\n");
    printf("a <leg idx> <x> <y> <z>                ) compute the angles for the specified leg idx and angles\n");
    printf("p <leg idx> <shoulder> <rotator> <knee>) compute the position for the specified leg idx and angles\n");
    printf("la <x> <y> <z>                         ) compute head angles to look at specified point\n");
    //printf("fm <neck_tilt> <pan> <head_tilt/roll> <x> <y> <z>) convert camera coordinates to ego coordinates\n");
    printf("q) quit\n");
    if(fgets(buf,sizeof(buf),stdin)==NULL){
      done = true;
    }
    
    ch = buf + strcspn(buf," \n");
    strncpy(cmd,buf,ch-buf);
    cmd[ch-buf] = '\00';

    if(strcmp(cmd,"q")==0){
      done = true;
    }else if(strcmp(cmd,"b")==0){
      char *start_arg,*end_arg;
      char arg[1025];

      start_arg = ch+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      body_height = atof(arg);
      
      start_arg = end_arg+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      body_angle = atof(arg);

      printf("set body height=%8.2f angle=%8.2f\n",
             body_height,body_angle);
    }else if(strcmp(cmd,"a")==0){
      char *start_arg,*end_arg;
      char arg[1025];

      double angles[3];
      int leg;
      vector3d target;

      start_arg = ch+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      leg = atoi(arg);
      
      start_arg = end_arg+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      target.x = atof(arg);
      
      start_arg = end_arg+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      target.y = atof(arg);
      
      start_arg = end_arg+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      target.z = atof(arg);

      printf("inv kinematics converts leg=%1d target=(%8.2f, %8.2f, %8.2f) with body height=%8.2f angle=%8.2f to:\n",
             leg,V3COMP(target),body_height,body_angle);
      BodyPosition bp;
      bp.angle.set(0.0,body_angle,0.0);
      bp.loc.set(0.0,0.0,body_height);
      kin.getLegAngles(angles,target,bp,leg);
      printf("angles = [%8.4f, %8.4f, %8.4f]\n",angles[0],angles[1],angles[2]);

      printf("for kinematics converts leg=%1d angles=[%8.4f, %8.4f, %8.4f] with body height=%8.2f angle=%8.2f to:\n",
             leg,angles[0],angles[1],angles[2],body_height,body_angle);
      target = kin.getLegPos(angles,body_angle,body_height,leg);
      printf("pos=(%8.2f, %8.2f, %8.2f) to:\n",
             V3COMP(target));
    }else if(strcmp(cmd,"p")==0){
      char *start_arg,*end_arg;
      char arg[1025];

      double angles[3];
      int leg;
      vector3d target;

      start_arg = ch+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      leg = atoi(arg);
      
      start_arg = end_arg+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      angles[0] = atof(arg);
      
      start_arg = end_arg+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      angles[1] = atof(arg);
      
      start_arg = end_arg+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      angles[2] = atof(arg);

      printf("for kinematics converts leg=%1d angles=[%8.4f, %8.4f, %8.4f] with body height=%8.2f angle=%8.2f to:\n",
             leg,angles[0],angles[1],angles[2],body_height,body_angle);
      target = kin.getLegPos(angles,body_angle,body_height,leg);
      printf("pos=(%8.2f, %8.2f, %8.2f) to:\n",
             V3COMP(target));

      printf("inv kinematics converts leg=%1d target=(%8.2f, %8.2f, %8.2f) with body height=%8.2f angle=%8.2f to:\n",
             leg,V3COMP(target),body_height,body_angle);
      BodyPosition bp;
      bp.angle.set(0.0,0.0,body_angle);
      bp.loc.set(0.0,0.0,body_height);
      kin.getLegAngles(angles,target,bp,leg);
      printf("angles = [%8.4f, %8.4f, %8.4f]\n",angles[0],angles[1],angles[2]);
    }else if(strcmp(cmd,"la")==0){
      char *start_arg,*end_arg;
      char arg[1025];

      double angles[3];
      vector3d target;

      start_arg = ch+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      target.x = atof(arg);
      
      start_arg = end_arg+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      target.y = atof(arg);
      
      start_arg = end_arg+1;
      end_arg = start_arg + strcspn(start_arg," \n");
      strncpy(arg,start_arg,end_arg - start_arg);
      arg[end_arg - start_arg] = '\00';
      target.z = atof(arg);

      printf("inv head kinematics looks at target=(%8.2f, %8.2f, %8.2f) with body height=%8.2f angle=%8.2f with:\n",
             V3COMP(target),body_height,body_angle);
      kin.getHeadAngles(angles,target,body_angle,body_height);
      printf("angles=[%8.4f, %8.4f, %8.4f]:\n",
             angles[0],angles[1],angles[2]);

      printf("for head kinematics says angles=[%8.4f, %8.4f, %8.4f] points head at:\n",
             angles[0],angles[1],angles[2]);
      vector3d cam_pt,cam_x;//,cam_y,cam_z;
      cam_pt = kin.runForwardModel(angles,body_angle,body_height,vector3d(0.0,0.0,0.0));
      cam_x = kin.runForwardModel(angles,body_angle,body_height,vector3d(1.0,0.0,0.0)) - cam_pt;
      //cam_y = kin.runForwardModel(angles,body_angle,body_height,vector3d(0.0,1.0,0.0)) - cam_pt;
      //cam_z = kin.runForwardModel(angles,body_angle,body_height,vector3d(0.0,0.0,1.0)) - cam_pt;
      target = point_on_line(cam_pt,cam_pt+cam_x,target);
      printf("target=(%8.2f, %8.2f, %8.2f) cam_loc=(%8.2f, %8.2f, %8.2f) cam_dir=(%8.2f, %8.2f, %8.2f)\n",
             V3COMP(target),V3COMP(cam_pt),V3COMP(cam_x));
    }else{
      printf("unrecognized command '%s' in input '%s'\n",cmd,buf);
    }
  }

  return 0;
}