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

#include <stdio.h>

#include "Kinematics.h"

bool
equal_until(char *str1, char *str2, char stop) {
  while(*str1 && *str2 && *str1==*str2 && *str1!=stop) {
    str1++; str2++;
  }

  return ((!*str1 || *str1==stop) &&
          (!*str2 || *str2==stop));
}

int
display_menu(char *response,int num_options,char *options[]) {
  bool valid_option=false;
  int option=0;
  char value[256];

  while(!valid_option) {
    for(int opt_idx=0; opt_idx<num_options; opt_idx++)
      printf("%s\n",options[opt_idx]);
    
    fgets(value,sizeof(value),stdin);
    if(value[strlen(value)-1]=='\n')
      value[strlen(value)-1]='\00';

    for(int opt_idx=0; opt_idx<num_options && !valid_option; opt_idx++) {
      if(equal_until(options[opt_idx],value,' ')) {
        option = opt_idx;
        valid_option = true;
      }
    }
  }

  strcpy(response,value);

  return option;
}

char *test_head_menu[]={
  "tar %g %g %g) set target x,y,z",
  "ba  %g      ) set body_angle",
  "bh  %g      ) set body_height",
  "q           ) quit"
};

void
test_head_angles() {
  double head_angles[3];
  double body_angle;
  double body_height;
  vector3d target(1000.0,0.0,100.0);
  Kinematics k(Kinematics::ERS7);

  bool keep_testing=true;

  head_angles[0] = head_angles[1] = head_angles[2] = 0.0;
  body_angle  =   0.0;
  body_height = 110.0;

  while(keep_testing) {
    vector3d loc,dir,up,right,lookat;
    double d;
    int option;
    char params[256];

    option=display_menu(params,sizeof(test_head_menu)/sizeof(char *),test_head_menu);
    switch(option) {
    case 0: { // set target
      sscanf(params," %*s %lg %lg %lg",&target.x,&target.y,&target.z);
    } break;
    case 1: { // set body_angle
      sscanf(params," %*s %lg",&body_angle);
    } break;
    case 2: { // set body_height
      sscanf(params," %*s %lg",&body_height);
    } break;
    case 3: {
      keep_testing=false;
    } break;
    }

    if(keep_testing) {
      k.getHeadAngles(head_angles,target,body_angle,body_height);
      k.getHeadPosition(loc,dir,up,right,head_angles,body_angle,body_height);
      
      d = (target - loc).length();
      lookat = loc + dir * d;
      
      printf("in : target (%10g,%10g,%10g) body_angle %10g body_height %10g\n",
             target.x,target.y,target.z,body_angle,body_height);
      printf("  computed: head ang t:%10g p:%10g r:%10g\n",
             head_angles[0],head_angles[1],head_angles[2]);
      printf("  computed: loc (%10g,%10g,%10g) dir (%10g,%10g,%10g)\n",
             loc.x,loc.y,loc.z,
             dir.x,dir.y,dir.z);
      printf("in : lookat (%10g,%10g,%10g) mismatch: %g\n",
             lookat.x,lookat.y,lookat.z,
	     distance(target,lookat));
    }
  }
}

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