/* 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 <getopt.h>

#include "genmot.h"

char *model="ERS7";

// Helper functions
void BodyPos(Motion::BodyState &b,double height,double angle)
{
  b.pos.loc.set(0,0,height);
  b.pos.angle.set(0,angle,0);
}

void LegPos(Motion::BodyState &b,int leg,double x,double y,double z)
{
  b.leg[leg].attr = ATTR_POSITION;
  b.leg[leg].pos.set(x,y,z);
}

void LegAng(Motion::BodyState &b,int leg,double r,double s,double k)
{
  b.leg[leg].attr = ATTR_ANGLES;
  set3(b.leg[leg].angles,r,s,k);
}

void HeadPos(Motion::BodyState &b,double x,double y,double z)
{
  b.head.attr = ATTR_POSITION;
  b.head.target.set(x,y,z);
}

void HeadAng(Motion::BodyState &b,double t,double p,double r)
{
  b.head.attr = ATTR_ANGLES;
  set3(b.head.angles,t,p,r);
}

void MouthAng(Motion::BodyState &b,double a)
{
  b.mouth.attr = ATTR_ANGLES;
  b.mouth.angle = a;
}

//==== MotionMaker Class Implementation ==============================//

void MotionMaker::start(const char *_filename,double _speed)
{
  states.clear();
  mzero(bsm);
  bsm.time = (int)(speed*500 + 0.5);
  filename = _filename;
  speed = _speed;
}

void MotionMaker::bodyPos(double height,double angle)
{
  bsm.body.pos.loc.set(0,0,height);
  bsm.body.pos.angle.set(0,angle,0);
}

void MotionMaker::legPos(int leg,double x,double y,double z)
{
  bsm.body.leg[leg].attr = ATTR_POSITION;
  bsm.body.leg[leg].pos.set(x,y,z);
}

void MotionMaker::legAng(int leg,double r,double s,double k)
{
  bsm.body.leg[leg].attr = ATTR_ANGLES;
  set3(bsm.body.leg[leg].angles,r,s,k);
}

void MotionMaker::headPos(double x,double y,double z)
{
  bsm.body.head.attr = ATTR_POSITION;
  bsm.body.head.target.set(x,y,z);
}

void MotionMaker::headAng(double t,double p,double r)
{
  bsm.body.head.attr = ATTR_ANGLES;
  set3(bsm.body.head.angles,t,p,r);
}

void MotionMaker::mouthAng(double a)
{
  bsm.body.mouth.attr = ATTR_ANGLES;
  bsm.body.mouth.angle = a;
}

void MotionMaker::trans(double ms)
{
  bsm.time = (int)(speed*ms + 0.5);
  states.push_back(bsm);
}

void MotionMaker::end()
{
  // finish off last motion
  bsm.time = 0; // final time is ignored by motplayer
  states.push_back(bsm);

  // save file
  FILE *out = fopen(filename,"wb");
  int num = states.size();

  if(out != NULL){
    fwrite(&num,sizeof(num),1,out);
    fwrite(&(states[0]),sizeof(Motion::BodyStateMotion),num,out);
    if(fclose(out)){
      printf("Error: Could not close '%s'.\n",filename);
    }
  }else{
    printf("Error: Could not open '%s' for saving.\n",filename);
  }
}


//==== Globals =======================================================//

Motion::Motion m;
Motion::MotionCommand cmd;
Kinematics k;
double angles[NumPIDJoints];


void make_motions()
{
  make_side_kicks();
  // make_dive_kicks();
  make_kicks();

  MakeKicksERS7();

  // make_kicks_de();
  // make_kicks_unsw();

  make_blocks();

  make_getups();
  make_fun_motions();
  make_turn_with_ball();
  // make_skateboard_motions();
}

void test_head()
{
  vector3d target,tdir;
  vector3d location,dir,up,right;
  double body_height = 115;
  double body_angle  = RAD(0.0);

  target.set(200,0,0);

  k.getHeadAngles(angles,target,body_angle,body_height);
  k.getHeadPosition(location,dir,up,right,
                  angles,body_angle,body_height);

  tdir = (target - location).norm();

}

void WalkSwitchTest()
{
  int i,n;
  printf("Walk switch test\n");

  cmd.head_cmd = Motion::HEAD_SCAN_BALL;
  cmd.va = 0.0;
  cmd.vy = 0.0;

  cmd.motion_cmd = Motion::MOTION_WALK_TROT;
  cmd.vx = m.getTrot().wp_xy.max_vel.max_dx;
  m.setCommand(cmd);
  n = 200+rand()%1000;
  for(i=0; i<n; i++) m.getAngles(angles);

  printf("<<====<< Switch <<====<<\n");

  cmd.motion_cmd = Motion::MOTION_WALK_TROT_FAST;
  cmd.vx = m.getTrot().wp_xyf.max_vel.max_dx;
  m.setCommand(cmd);
  for(i=0; i<1000; i++) m.getAngles(angles);
}

void WalkTest(Motion::WalkParam wp,double x,double y,double a)
{
  int i;
  int *err;

  cmd.head_cmd = Motion::HEAD_SCAN_BALL;
  cmd.motion_cmd = Motion::MOTION_WALK_TROT;
  cmd.vx = x * wp.max_vel.max_dx;
  cmd.vy = y * wp.max_vel.max_dy;
  cmd.va = a * wp.max_vel.max_da;

  m.setCommand(cmd);
  for(i=0; i<10*1000/8; i++) m.getAngles(angles);
  Motion::kin.clearErrors();

  m.setCommand(cmd);
  for(i=0; i<20*1000/8; i++) m.getAngles(angles);

  err = Motion::kin.getErrors();
  printf("Errors=[%d] [%d]\n",
         err[0],err[1]);
  printf("       [%d] [%d]\n",
         err[2],err[3]);
}

int main(int argc,char **argv)
{
  int *err;
  int i;
  int s;
  Motion::WalkParam *wp_xy, *wp_a;
  int c;

  make_walk_param();
  make_motions();

  mset(angles,0.0,NumPIDJoints);
  mzero(cmd);
  m.init();
  m.init(angles);

  wp_xy = m.getXYParams();
  wp_a = m.getAParams();

  while((c = getopt(argc,argv,"w")) != EOF){
    switch(c){
      case 'w':
	WalkSwitchTest();
	return(0);
    }
  }

  WalkTest(m.getTrot().wp_a,0,0,1);

  switch(4){
    case 0:
      cmd.motion_cmd = Motion::MOTION_KICK_BUMP;
      cmd.head_cmd   = Motion::HEAD_SCAN_BALL;
      break;

    case 1:
      //cmd.bound_mode = Motion::BOUND_NONE;
      cmd.motion_cmd = Motion::MOTION_WALK_TROT;
      cmd.head_cmd = Motion::HEAD_SCAN_BALL;
      cmd.va =            0.0;
      cmd.vx =            wp_xy->max_vel.max_dx;
      cmd.vy =            0.0;
      break;

    case 2:
      cmd.motion_cmd = Motion::MOTION_WALK_TROT;
      cmd.head_cmd = Motion::HEAD_SCAN_BALL;
      cmd.va =   0.0;
      cmd.vx = wp_xy->max_vel.max_dx;
      cmd.vy =   0.0;
      break;

    case 3:
      cmd.bound_mode = Motion::BOUND_NONE;
      cmd.motion_cmd = Motion::MOTION_WALK_TROT;
      cmd.head_cmd = Motion::HEAD_SCAN_BALL;
      cmd.va =            wp_a->max_vel.max_da;
      cmd.vx =            0.0;
      cmd.vy =            0.0;
      break;

    case 4:
      cmd.motion_cmd = Motion::MOTION_WALK_TROT;
      cmd.head_cmd = Motion::HEAD_SCAN_BALL;
      cmd.va =   wp_a->max_vel.max_da;
      cmd.vx =   0.0;
      cmd.vy =   0.0;
      break;
  }

  return(0);
}