/* 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 "../headers/system_config.h"

#include <stdlib.h>
#include <stdio.h>
//#include <strings.h>
#include <iostream>

#include "../headers/Config.h"
#include "../headers/DogTypes.h"
#include "../headers/CircBufPacket.h"
#include "../headers/FileSystem.h"
#include "../headers/Geometry.h"
#include "../headers/Sensors.h"
#include "../Vision/VisionInterface.h"

#ifdef PLATFORM_APERIOS
#include "../headers/Reporting.h"
#include "../headers/Utility.h"
#endif

#include "Kinematics.h"
#include "Spline.h"
// #include "Path.h"
#include "MotionFile.h"

#include "Motion.h"


#ifndef PLATFORM_LINUX
extern SensorData sensor;
#endif

static const bool PrintChecksums=false;

// #define BOUND_MOTION

namespace Motion{

static const bool ApplyOdometryCorrection=true;

Kinematics kin;
double camera_vert_fov;
double camera_horz_fov;

int debug_sound_frequency;
int debug_sound_duration;

void DebugPlaySound(int frequency,int ms_duration)
{
  debug_sound_frequency = frequency;
  debug_sound_duration  = ms_duration * 1000;
}

void DebugGetCommand(MotionCommand &cmd)
{
  if(debug_sound_frequency){
    cmd.sound_cmd = SOUND_NOTE;
    cmd.sound_frequency = debug_sound_frequency;
    cmd.sound_duration  = debug_sound_duration;
    cmd.sound_file = 0;

    debug_sound_frequency = 0;
  }
}

void Complete(BodyState &body)
/*
  You fill in either angles or a target (kinematic) position for the
  joints, and this function does the rest so that it can be sent to
  the robot's joint controller.
*/
{
  int attr,i;

  // legs
  for(i=0; i<4; i++){
    attr = body.leg[i].attr;
    if(!(attr & ATTR_ANGLES) && (attr & ATTR_POSITION)){
      kin.getLegAngles(body.leg[i].angles,body.leg[i].pos,body.pos,i);
      body.leg[i].attr |= ATTR_ANGLES;

      /*
      printf("%d (%8.2f %8.2f %8.2f)\n",i,
             body.leg[i].pos.x,
             body.leg[i].pos.y,
             body.leg[i].pos.z);
      */
    }
  }

  // head
  attr = body.head.attr;
  if(!(attr & ATTR_ANGLES) && (attr & ATTR_POSITION)){
    kin.getHeadAngles(body.head.angles,body.head.target,
		      body.pos.angle.y,body.pos.loc.z);
    body.head.attr |= ATTR_ANGLES;
  }
}

void Flip(BodyState &body)
/*
  This mirrors a body position right to left, which is useful for
  motion scripts that aren't symmetric since you don't have to make a
  left and right version of each one.
*/
{
  int i;

  // Flip legs
  swap(body.leg[0],body.leg[1]);
  swap(body.leg[2],body.leg[3]);
  for(i=0; i<4; i++){
    body.leg[i].pos.y = -body.leg[i].pos.y;
  }

  // Flip head
  body.head.target.y = -body.head.target.y;
  body.head.angles[1] = -body.head.angles[1]; // flip pan
  if(kin.model==Kinematics::ERS210){
    body.head.angles[2] = -body.head.angles[2]; // flip roll
  }
}

void Interpolate(BodyState &body,BodyState &start,BodyState &target,double t)
/*
  This is the core of what the motion files use; it lets you
  interpolate with t=0..1 between two other positions.  If can be used
  for time interplolation (between keyframes like the motion files),
  or could be used to do weighted averages of multiple actions that
  requested a particular body state.
*/
{
  int attr,i,j;

  if(t <= 0.0){
    body = start;
    return;
  }

  if(t >= 1.0){
    body = target;
    return;
  }

  body.pos.loc   = start.pos.loc  *(1-t) + target.pos.loc  *t;
  body.pos.angle = start.pos.angle*(1-t) + target.pos.angle*t;

  // printf(".\n");

  // legs
  for(i=0; i<4; i++){
    attr = start.leg[i].attr & target.leg[i].attr;

    if(attr){
      if(attr & ATTR_POSITION){
        body.leg[i].pos = start.leg[i].pos*(1-t) + target.leg[i].pos*t;
        kin.getLegAngles(body.leg[i].angles,body.leg[i].pos,body.pos,i);
        attr |= ATTR_ANGLES;
      }else{
        for(j=0; j<3; j++){
          body.leg[i].angles[j] = start.leg[i].angles[j]*(1-t) +
                                  target.leg[i].angles[j]*t;
        }
      }

      body.leg[i].attr = attr;
    }else{
      pprintf(TextOutputStream,"Can't interpolate attr (%ld->%ld)\n",
              start.leg[i].attr,target.leg[i].attr);
    }
  }

  // head
  attr = start.head.attr & target.head.attr;
  // printf("interp attr=%d %ld %ld\n",attr,start.head.attr,target.head.attr);

  if(attr){
    if(attr & ATTR_POSITION){
      body.head.target = start.head.target*(1-t) + target.head.target*t;
      kin.getHeadAngles(body.head.angles,body.head.target,
                    body.pos.angle.y,body.pos.loc.z);
      attr |= ATTR_ANGLES;
    }else{
      for(j=0; j<3; j++){
        body.head.angles[j] = start.head.angles[j]*(1-t) +
                              target.head.angles[j]*t;
      }
    }

    body.head.attr = attr;
  } else {
    body.head.attr = 0;
  }

  // mouth
  attr = start.mouth.attr & target.mouth.attr;
  if(attr){
    body.mouth.angle =
      start. mouth.angle*(1-t) +
      target.mouth.angle* t;

    body.mouth.attr = attr;
  } else {
    body.mouth.attr = 0;
  }
}

#ifndef PLATFORM_APERIOS
template <class data>
void DeleteRegion(data *arr)
{
  delete[] arr;
}

#define sSUCCESS 1
#define sERROR   0
typedef int sError;

sError NewRegion(int size,void **ptr)
{
  void *mem = new char[size];
  if(mem){
    *ptr = mem;
    return(sSUCCESS);
  }else{
    return(sERROR);
  }
}
#endif

unsigned long checksum(char *data,int num)
{
  unsigned long c;
  int i;

  c = 0;
  for(i=0; i<num; i++){
    c = c ^ (data[i]*13 + 37);
    c = (c << 13) | (c >> 19);
  }

  return(c);
}

template <class data>
int read_file(data *arr,int num,char *filename)
{
  char name[256];
  int fd;
  int n;

  mzero(arr,num);

  if(linux){
    sprintf(name,"%s",filename);
  }else{
    if(kin.model==Kinematics::ERS210){
      sprintf(name,"/MS/motion/ers210/%s",filename);
    }else{
      sprintf(name,"/MS/motion/ers7/%s",filename);
    }
  }

  fd = BFS::open(name,BFS_O_RDONLY);
  if(fd < 0) return(0);
  n = BFS::read(fd,arr,sizeof(data)*num);
  // pprintf(TextOutputStream,"read_file: %d of %d\n",n,sizeof(data)*num);
  n /= sizeof(data);
  BFS::close(fd);

  return(n);
}

//====================================================================//
//  Motion Player Class
//====================================================================//

bool MotPlayer::load(char *filename)
{
  char name[256];
  HFS::FILE *in;
  int i,num;

  if(linux){
    sprintf(name,"%s",filename);
  }else{    
    if(kin.model==Kinematics::ERS210){
      sprintf(name,"/MS/motion/ers210/%s",filename);
    }else{
      sprintf(name,"/MS/motion/ers7/%s",filename);
    }
  }

  in = HFS::fopen(name,"rb");
  if(!in) return(false);

  num = 0;
  HFS::fread(&num,sizeof(int),1,in);
  if(num<=0 || num>=256) return(false);

  if(false){
    if(frame) delete(frame);
    frame = new BodyStateMotion[num];
  }else{
    if(frame) DeleteRegion(frame);
    frame = NULL;
    NewRegion(sizeof(BodyStateMotion)*num,reinterpret_cast<void**>(&frame));
  }
  if(!frame) return(false);

  HFS::fread(frame,sizeof(BodyStateMotion),num,in);
  HFS::fclose(in);

  num_frames = num;
  current_frame = 0;
  base_time = 0;

  for(i=0; i<num; i++){
    Complete(frame[i].body);
  }

  speed = 0;
  flip = false;

  if(PrintChecksums) {
    pprintf(TextOutputStream,"Loaded Motion '%s'\t frames=%d checksum=0x%X\n",
	    filename,num_frames,
	    checksum((char*)frame,sizeof(BodyStateMotion)*num));
  }

  return(true);
}

void MotPlayer::close()
{
  if(frame) DeleteRegion(frame);

  frame = NULL;
  num_frames = 0;
  current_frame = 0;
  base_time = 0;
}

void MotPlayer::play(BodyState &body,int time,double nspeed,bool flip_lr)
{
  current_frame = 0;
  base_time = time;
  speed = nspeed;
  flip = flip_lr;
  if(frame){
    frame[0].body = body;
    Complete(frame[0].body);
    if(flip) Flip(frame[0].body);
  }
}

int MotPlayer::get(BodyState &body,int time)
{
  int dt,len;
  int busy;

  if(time<base_time || !frame) return(0);

  dt  = time - base_time;
  len = frame[current_frame].time;

  while(current_frame<num_frames-1 && dt>len){
    current_frame++;
    base_time += len;
    dt = time - base_time;
    len = frame[current_frame].time;
  }

  if(current_frame < num_frames-1){
    Interpolate(body,
                frame[current_frame].body,
                frame[current_frame + 1].body,
                ((double)dt) / len);
    //printf("frame=%d t=%f\n",current_frame,((double)dt) / len);
    if(flip) Flip(body);
    busy = LEGS_BUSY;
    if(body.head.attr)  busy |= HEAD_BUSY;
    if(body.mouth.attr) busy |= MOUTH_BUSY;
    return(busy);
  }else{
    body = frame[num_frames - 1].body;
    if(flip) Flip(body);
    return(0);
  }
}


//====================================================================//
//  Stand Class
//====================================================================//

Stand::Stand()
{
  time = 0;
  length = TimeStep;
}

void Stand::init()
{
  BodyState b;
  int i;

  time = length = 0;

  // Neutral
  // Note this default gets overwritten later by the trot neutral position
  mzero(b);
  for(i=0; i<4; i++) b.leg[i].attr = ATTR_POSITION;
  b.head.attr = 0;
  b.mouth.attr = 0;

  b.leg[0].pos.set( 65, 65,0);
  b.leg[1].pos.set( 65,-65,0);
  b.leg[2].pos.set(-65, 65,0);
  b.leg[3].pos.set(-65,-65,0);

  set3(b.head.angles,0.0,0.0,0.0);
  b.mouth.angle = 0.0;
  b.pos.loc.set(0,0,140);
  b.pos.angle.set(0,RAD(0.0),0);

  neutral = b;
  left    = b;
  right   = b;

  // Crouch
  for(i=0; i<4; i++) b.leg[i].attr = ATTR_POSITION;
  b.head.attr = 0;
  b.mouth.attr = 0;

  b.leg[0].pos.set(105, 130,0);
  b.leg[1].pos.set(105,-130,0);
  b.leg[2].pos.set(-70,  75,0);
  b.leg[3].pos.set(-70, -75,0);

  set3(b.head.angles,0.0,0.0,0.0);
  b.mouth.angle = 0.0;
  b.pos.loc.set(0,0,100.0);
  b.pos.angle.set(0,RAD(17.5),0);

  crouch = b;

  // Skate stand
  for(i=0; i<4; i++) b.leg[i].attr = ATTR_POSITION;
  b.head.attr = 0;
  b.mouth.attr = 0;

  set3(b.leg[0].angles,0.37,.7,0.6);
  set3(b.leg[1].angles,0.37,.7,0.6);
  b.leg[2].pos.set(-90, 77,0);
  b.leg[3].pos.set(-90,-77,0);

  set3(b.head.angles,0.0,0.0,0.0);
  b.mouth.angle = 0.0;
  b.pos.loc.set(0,0,105.0);
  b.pos.angle.set(0,RAD(12.0),0);

  skate_neutral = b;

  // BackWall Turn Neutral
  for(i=0; i<4; i++) b.leg[i].attr = ATTR_POSITION;
  b.head.attr = 0;
  b.mouth.attr = 0;

  b.leg[0].pos.set( 130, 90,0);
  b.leg[1].pos.set( 135,-120,0);
  b.leg[2].pos.set(-100, 80,0);
  b.leg[3].pos.set(-100,-80,0);

  set3(b.head.angles,0.0,0.0,0.0);
  b.mouth.angle = 0.0;
  b.pos.loc.set(0,0,109);
  b.pos.angle.set(0,RAD(8.0),0);

  wall_turn_neutral = b;
}

void Stand::setNeutral(const BodyState &b)
{
  neutral = b;
  left    = b;
  right   = b;

  left.pos.loc.y += 30.0;
  right.pos.loc.y -= 30.0;
}

void Stand::setTarget(const BodyState &body,const BodyState &ntarget,int ntime)
{
  start  = body;
  target = ntarget;
  time   = 0;
  length = max(ntime,TimeStep);

  Complete(start);
  Complete(target);
}

void Stand::setTarget(const BodyState &body,int target,int ntime)
{
  BodyState b;

  switch(target){
    case MOTION_STAND_NEUTRAL:     b = neutral;           break;
    case MOTION_STAND_LEFT:        b = left;              break;
    case MOTION_STAND_RIGHT:       b = right;             break;
    case MOTION_STAND_CROUCH:      b = crouch;            break;
    case MOTION_STAND_SKATEBOARD:  b = skate_neutral;     break;
    case MOTION_STAND_WALL_TURN_L: b = wall_turn_neutral; break;
    case MOTION_STAND_WALL_TURN_R:  
      b = wall_turn_neutral; 
      Flip(b);
      break;
    default: b = neutral;
  }

  setTarget(body,b,ntime);
}

int Stand::getAngles(const BodyState &current,BodyState &next)
{
  double t;
  int busy;

  if(time < length){
    // Calculate current position
    t = min((double)time / length,1.0);
    Interpolate(next,start,target,t);

    busy = LEGS_BUSY;
    if(next.head.attr)  busy |= HEAD_BUSY;
    if(next.mouth.attr) busy |= MOUTH_BUSY;
  }else{
    next = target;
    busy = 0;
  }

  // move time ahead
  time += TimeStep;

  return(busy);
}

void Stand::getMotionUpdate(MotionLocalizationUpdate &update)
{
}

int Stand::areBusy()
{
  int busy = 0;

  if(time < length){
    busy |= LEGS_BUSY;
    if(start.head.attr  & target.head.attr ) busy |= HEAD_BUSY;
    if(start.mouth.attr & target.mouth.attr) busy |= MOUTH_BUSY;
  }

  return(busy);
}

//====================================================================//
//  Trotting Class
//====================================================================//

const vector3d zero(0,0,0);
const vector3d one(1,0,0);


Trot::Trot()
{
  memset(this,0,sizeof(*this));

  body_loc.init(zero,zero);
  body_angle.init(zero,zero);

  cycle = 1.0;

  new_param = false;
}

void Trot::load()
{
  int n;

  n = read_file(&wp_xy,1,"walk_xy.prm");
  if(PrintChecksums) {
    pprintf(TextOutputStream,"Loaded XY Trot Paramaters n=%d checksum=0x%X\n",n,
	    checksum((char*)&wp_xy,sizeof(wp_xy)));
  }

  n = read_file(&wp_xyf,1,"walk_xyf.prm");
  if(PrintChecksums) {
    pprintf(TextOutputStream,"Loaded Fast XY Trot Paramaters n=%d checksum=0x%X\n",n,
	    checksum((char*)&wp_xy,sizeof(wp_xy)));
  }

  n = read_file(&wp_a,1,"walk_a.prm");
  if(PrintChecksums) {
    pprintf(TextOutputStream,"Loaded A Trot Paramaters n=%d checksum=0x%X\n",n,
	    checksum((char*)&wp_a,sizeof(wp_a)));
  }

  n = read_file(&wp_dxy,1,"walk_dxy.prm");
  if(PrintChecksums) {
    pprintf(TextOutputStream,"Loaded D Trot Paramaters n=%d checksum=0x%X\n",n,
	    checksum((char*)&wp_dxy,sizeof(wp_dxy)));
  }

  n = read_file(&wp_da,1,"walk_da.prm");
  if(PrintChecksums) {
    pprintf(TextOutputStream,"Loaded D Trot Paramaters n=%d checksum=0x%X\n",n,
	    checksum((char*)&wp_da,sizeof(wp_da)));
  }

  n = read_file(&odom_param,1,"odom.prm");
  if(n < 1){
    // don't crash if can't read file on Linux so that genmot doesn't require this file
#ifdef PLATFORM_APERIOS
    if(ApplyOdometryCorrection){
      // abort if odometry parameters are missing
      *((ulong *)0x0) = 0x0DEAD0D0UL;
    }else{
      mzero(odom_param);
    }
#endif
  }

  // start with walk-xy
  wp = wp_xy;
  new_param = true;

  // this is normally overridden by behaviors, but set up a sane default
  wp_beh = wp_xy;
}

void Trot::getNeutral(BodyState &b)
{
  int i;

  mzero(b);
  for(i=0; i<4; i++) b.leg[i].attr = ATTR_POSITION;
  b.head.attr = 0;
  b.mouth.attr = 0;

  for(i=0; i<4; i++)
    b.leg[i].pos = wp.leg[i].neutral;

  set3(b.head.angles,0.0,0.0,0.0);
  b.mouth.angle = 0.0;
  b.pos.loc.set(0,0,wp.body_height);
  b.pos.angle.set(0,wp.body_angle,0);
}

void Trot::setParams(WalkParam &params)
{
  wp_beh = params;
  pprintf(TextOutputStream,"Motion got new beh_wp, period = %d, height = %f, angle = %f\n",
	  wp_beh.period, wp_beh.body_height, wp_beh.body_angle);
}

void Trot::evalPosition(BodyPosition &bp,int time)
{
  double t;

  t = time / 1000.0;

  bp.loc = body_loc.eval(t);
  bp.angle = body_angle.eval(t);

  bp.angle.y = wp.body_angle;

#ifdef PLATFORM_LINUX
  /*
  printf("Body: l:(%f,%f,%f) a:(%f,%f,%f) t=%d\n",
         bp.loc.x,bp.loc.y,bp.loc.z,
         bp.angle.x,bp.angle.y,bp.angle.z,
         time);
  */
#endif
}

vector3d Trot::projectPosition(BodyPosition &bp,vector3d pos)
{
  vector3d p;
  p = pos.rotate_z(bp.angle.z).rotate_y(bp.angle.y) + bp.loc;
  return(p);
}

void Trot::init(double dx,double dy,double da,int walk_type)
{
  vector3d body,angle;
  int i;

  type = walk_type;

  time = 0;
  chooseParam(dx,dy,da);

  body.set(0,0,wp.body_height);
  angle.set(0,wp.body_angle,0);

  body_loc.init(body,zero);
  body_angle.init(angle,zero);

  for(i=0; i<4; i++){
    legw[i].air = false;
  }

  target_vel_xya.set(0,0,0);
  vel_xya.set(0,0,0);

  pos_delta.set(0,0,0);
  angle_delta = 0.0;

  cycle = 1.0;
}

void Trot::mirrorParam(WalkParam &wp)
{
  // swap left/right legs
  swap(wp.leg[0],wp.leg[1]);
  swap(wp.leg[2],wp.leg[3]);

  // negate y values
  for(int i=0; i<4; i++){
    wp.leg[i].neutral.y = -wp.leg[i].neutral.y;
  }
  wp.sway = -wp.sway;
}

void Trot::chooseParam(double dx,double dy,double da)
{
  //pprintf(TextOutputStream,"Chooseparam(%f,%f,%f)\n\n",dx, dy, da );

  // double new_cycle;
  // int sgn = (int)sign_nz(da);
  WalkParam wp_old = wp;

  // maximum speed along any dimension
  double max_dim_dist = max(fabs(dx),fabs(dy));

  // we're turning if dx,dy are zero and da is larger, or
  // if we're turning significantly more than we are translating
  bool turning = max_dim_dist/50.0 < fabs(da);
  // bool turning = (fabs(da)>0.0 && max_dim_dist<= 0.0) ||
  //               (fabs(da)>1.0 && max_dim_dist<=50.0);

  switch(type){
    case MOTION_WALK_TROT:
      if(turning){
	wp = wp_a;
	if(da < 0.0) mirrorParam(wp);
      }else{
	wp = wp_xy;
      }
      break;

    case MOTION_WALK_TROT_FAST:
      if(turning){
	wp = wp_a;
	if(da < 0.0) mirrorParam(wp);
      }else{
	wp = wp_xyf;
      }
      break;

    case MOTION_WALK_DRIBBLE:
      if(turning){
	wp = wp_da;
	if(da < 0.0) mirrorParam(wp);
      }else{
	wp = wp_dxy;
      }
      break;

    case MOTION_WALK_BEH:
      wp = wp_beh;
      break;
  }

  // set flag on whether these are new parameters
  new_param = (memcmp(&wp,&wp_old,sizeof(wp)) != 0);
  //if(new_param) DebugPlaySound(2500,250);
  // if(new_param) printf("New param\n");
}

void Trot::setTarget(double dx,double dy,double da,
                     double accel_time,int bound_mode)
{
  double fa,f,l;
  vector2d v;

  if(bound_mode == BOUND_GOTO_EGO){
    da = bound(da, -wp.max_vel.max_da, wp.max_vel.max_da);
    fa = 1.0 - fabs(da / wp.max_vel.max_da);
    v.set(dx / wp.max_vel.max_dx,dy / wp.max_vel.max_dy);

    l = v.length();
    f = (l < fa)? 1.0 : fa / (l + 1E-10);

    // printf("f=%f l=%f fa=%f v(%f,%f)\n",f,l,fa,v.x,v.y);

    dx = wp.max_vel.max_dx * f * v.x;
    dy = wp.max_vel.max_dy * f * v.y;
  }else if(bound_mode == BOUND_SPEED){
    double total;
    total = 0.0;

    total += fabs(da / wp.max_vel.max_da);
    total += fabs(dx / wp.max_vel.max_dx);
    total += fabs(dy / wp.max_vel.max_dy);
    
    if(total > 1.0){
      da = da / total;
      dx = dx / total;
      dy = dy / total;
    }
  }

  target_vel_xya.set(dx,dy,da);
  //printf("xya(%f,%f,%f)\n",dx,dy,da);
  //pprintf(TextOutputStream,"xya(%f,%f,%f) mode %d\n",dx,dy,da, bound_mode);
}

int Trot::getAngles(BodyState &current,BodyState &next)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Trot::getAngles",SecToTime(5.0),SecToTime(100.0),10000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  int i, start_leg_id;
  bool air;
  double cycle_step;

  BodyPosition delta;
  vector3d target,vfp;
  LegWalkState *l;
  double t,air_f,b,vfa;
  double height,hop,sway;
  double accel_mult;

  vector3d max_accel_xya(wp.max_vel.max_dx*2.0,
			 wp.max_vel.max_dy*2.0,
			 wp.max_vel.max_da*8.0); // * 2

  t = TimeStep / 1000.0;

  /*
  if( fabs(target_vel_xya.x - vel_xya.x) > 150 ){
    accel_mult = .8;
  }else{
    accel_mult = (cos(cycle*4*M_PI)+1);
  }
  */

  // find distance of legs from neutral
  /*
  max_leg_d.x = wp.max_vel.max_dx * wp.period/1000.0;
  max_leg_d.y = wp.max_vel.max_dy * wp.period/1000.0;
  max_leg_d.z = 0.0;
  */

  vector3d p;
  double dist = 0.0;
  for(int i=0; i<4; i++){
    p = current.leg[i].pos - wp.leg[i].neutral;
    p.z = 0.0;
    dist += p.length();
  }

  // figure out acceleration multiplier
  accel_mult = bound(1.0 - dist/120.0, 0.0, 1.0);
  max_accel_xya *= accel_mult; // (cos(cycle*4*M_PI)+1);

  /*
  printf("v=<%f,%f,%f> d=%f ",V3COMP(vel_xya),dist);
  for(int i=0; i<4; i++){
    vector3d p = current.leg[i].pos - wp.leg[i].neutral;
    printf("%d<%f,%f,%f> ",i,V3COMP(p));
  }
  printf("\n");
  */

  vel_xya = GVector::bound(target_vel_xya,
			   vel_xya-max_accel_xya*t,
			   vel_xya+max_accel_xya*t);
  delta.loc.set(vel_xya.x*t,vel_xya.y*t,0);


  if(type != MOTION_SKATE){
    start_leg_id = 0;
    delta.angle.set(0,0,vel_xya.z*t);
  }else{
    //values for moving front legs depending on angular vel.
    //skate_turn_offset.set(0,0,target_vel_xya.z*0.5);
    set3(next.leg[0].angles, 0.1, -0.1,	1.9);
    set3(next.leg[1].angles, 0.1, -0.1, 1.9);

    //calculate trot motion only for back legs
    start_leg_id = 2;
    delta.angle.set(0,0,0 );
  }

  //FIXME: this does nothing since we dont get the return value.
  //target_vel_xya.rotate_z(-delta.angle.z);

  // incorporate movement delta
  angle_delta += delta.angle.z;
  pos_delta += delta.loc.rotate_z(angle_delta);

  // if a new cycle is beginning, can choose new walk parameters
  cycle_step = (double)TimeStep/wp.period;
  if(fmod(cycle + cycle_step,1.0) < fmod(cycle,0.5)){
    chooseParam(vel_xya.x,vel_xya.y,vel_xya.z);
    cycle_step = (double)TimeStep/wp.period;
  }
  // printf("%f\n",cycle);

  for(i=start_leg_id; i<4; i++){
    l = &legw[i];
    air = (cycle >= wp.leg[i].lift_time) && (cycle < wp.leg[i].down_time);
    air_f = wp.leg[i].down_time - wp.leg[i].lift_time;

    if(air != legw[i].air){
      if(air){
        t = wp.period/1000.0 * 0.75; // FIXME: 0.75 should probably be air_f+(1-air_f)/2
        vfp.x = bound(target_vel_xya.x,
                      vel_xya.x-max_accel_xya.x*t,
                      vel_xya.x+max_accel_xya.x*t);
        vfp.y = bound(target_vel_xya.y,
                      vel_xya.y-max_accel_xya.y*t,
                      vel_xya.y+max_accel_xya.y*t);
        vfp.z = 0.0;

	if(type == MOTION_SKATE){
	  vfa = 0.0;
	}else{
	  vfa   = bound(target_vel_xya.z,
			vel_xya.z-max_accel_xya.z*t,
			vel_xya.z+max_accel_xya.z*t);
	}

        b = (wp.period/1000.0) * (1.0 - air_f) / 2.0;
        target = wp.leg[i].neutral + vfp*b;
        target = target.rotate_z(vfa*b);

        /*
        printf("targetf=(%f,%f,%f) vfp(%f,%f,%f) vfa=%f t=%f b=%f\n",
               V3COMP(target),V3COMP(vfp),vfa,t,b);
        */

        /*
        t = ((wp.period/2)/TimeStep * (1.0-air_f));
        target = wp.leg[i].neutral + delta.loc*t;
        target = target.rotate_z(delta.angle.z*t);
        printf("targetd=(%f,%f,%f)\n",V3COMP(target));
        */

        l->airpath.create(current.leg[i].pos,target,
                          wp.leg[i].lift_vel,wp.leg[i].down_vel);
        // l->pos = target = wp.leg[i].neutral + wp.leg[i].lift_vel;

#ifdef PLATFORM_LINUX
        /*
        printf("Lift: (%g,%g,%g)->(%g,%g,%g)\n",
               l->pos.x,l->pos.y,l->pos.z,
               target.x,target.y,target.z);
        */
#endif
      }else{
        current.leg[i].pos.z = 0.0;
      }
      l->air = air;
    }

    if(air){
      t = (cycle - wp.leg[i].lift_time) / air_f;
      next.leg[i].pos = l->airpath.eval(t);

      // limit how high the legs are allowed to lift up
      if(i <= 1){
	next.leg[i].pos.z = min(next.leg[i].pos.z, wp.front_height); 
      }else{
	next.leg[i].pos.z = min(next.leg[i].pos.z, wp.back_height);
      }
    }else{
      next.leg[i].pos = (current.leg[i].pos - delta.loc).rotate_z(-delta.angle.z);
    }
    next.leg[i].attr = ATTR_POSITION;
  } // for

  sway   = wp.sway*cos(2*M_PI*cycle);
  hop    = wp.hop*sin(4*M_PI*cycle);
  height = wp.body_height;
  next.pos.loc.set(0,-sway,height+hop);
  next.pos.angle.set(0,wp.body_angle,0);
  Complete(next);

  time += TimeStep;
  cycle = frac(cycle + cycle_step);

  // DebugPlaySound((int)(2000+1000*sinf(2*M_PI*cycle)),64);

  return(LEGS_BUSY);
}

double Trot::getBasis(double dx,double dy,double da,int basis_idx)
{
  double basis_val = 0.0;

  switch(basis_idx){
    case 0:
      basis_val = (dx >= 0.0 ? dx : 0.0);
      break;
    case 1:
      basis_val = (dx <= 0.0 ? dx : 0.0);
      break;
    case 2:
      basis_val = dy;
      break;
    case 3:
      basis_val = da;
      break;
    case 4:
      basis_val = dx * dy;
      break;
    case 5:
    default:
      basis_val = 1.0;
      break;
  }

  return basis_val;
}

void Trot::applyMotionCorrection(double &dx,double &dy,double &da)
{
  if(ApplyOdometryCorrection){
    double new_dx,new_dy,new_da;
    
    new_dx = new_dy = new_da = 0.0;
    for(int basis_idx=0; basis_idx<OdometryParam::NumBases; basis_idx++){
      double basis = getBasis(dx,dy,da,basis_idx);
      new_dx += basis * odom_param.dx_coeffs[basis_idx];
      new_dy += basis * odom_param.dy_coeffs[basis_idx];
      new_da += basis * odom_param.da_coeffs[basis_idx];
    }

    dx = new_dx;
    dy = new_dy;
    da = new_da;
  }
}

void Trot::getMotionUpdate(MotionLocalizationUpdate &update)
{
  vector2d x;

  static const double update_time = 4*TimeStep / 1000.0;
  double dx,dy,da;
  dx = pos_delta.x / update_time;
  dy = pos_delta.y / update_time;
  da = angle_delta / update_time;
  applyMotionCorrection(dx,dy,da);
  pos_delta.x = dx * update_time;
  pos_delta.y = dy * update_time;
  angle_delta = da * update_time;

  x.set(1,0);
  update.pos_delta.set(pos_delta.x,pos_delta.y);
  update.heading_delta = x.rotate(angle_delta);

  pos_delta.set(0,0,0);
  angle_delta = 0;
}

int Trot::areBusy()
{
  return(LEGS_BUSY);
}

//====================================================================//
//  TurnWithBall Class
//====================================================================//

TurnWithBall::TurnWithBall()
{
  type = time = busy = substate = 0;
  generated_motion_update = false;
}

void TurnWithBall::load()
{
  m_grab.load("tb_grab.mot");
  m_step.load("tb_step.mot");
}

void TurnWithBall::init(int turn_type)
{
  type = turn_type;//left or right
  substate = GRAB; //which part of the motion we are in
  time = 0;
  busy = LEGS_BUSY|HEAD_BUSY;
  generated_motion_update = false;
}

//When we come to the end of a motion file, this
//determines which one will be executed next and 
//resets variables
void TurnWithBall::setNextMot(int turn_type){
  type = turn_type; //this can change if we decide to switch directions
  substate = STEP;
  time = 0;
  busy = LEGS_BUSY|HEAD_BUSY;
  generated_motion_update = false;
}

int TurnWithBall::getAngles(BodyState &current,BodyState &next)
{
  if(time == 0){
    switch(substate){
    case GRAB: m_grab.play(current,time,1,false); break;
    case STEP: 
      if(type==MOTION_TURN_WITH_BALL_L){
	m_step.play(current,time,1,false); 
      }else if(type==MOTION_TURN_WITH_BALL_R){
	m_step.play(current,time,1,true); 
      }
      break;
    }
    time += TimeStep;
  }

  switch(substate){
  case GRAB: busy = m_grab.get(next,time); break;
  case STEP: busy = m_step.get(next,time); break;
  }

  time += TimeStep;
  return(busy);
}

int TurnWithBall::areBusy()
{
  return(busy);
}

void TurnWithBall::getMotionUpdate(MotionLocalizationUpdate &update)
{
  update.stable = true;
  
  if(!generated_motion_update){
    vector2d x(1,0);

    if(kin.model==Kinematics::ERS210){
      // no turn motion for this model
    }else{
      switch(type){
        case MOTION_TURN_WITH_BALL_L: x = x.rotate(RAD( 30)); break;
        case MOTION_TURN_WITH_BALL_R: x = x.rotate(RAD(-30)); break;
      }
    }
 
    //pprintf(TextOutputStream,"TurnWithBall Motion Update: <%f,%f>\n",
    //      V2COMP(x));

    update.heading_delta = x;
    generated_motion_update = true;
  }
}


//====================================================================//
//  Getup Class
//====================================================================//

Getup::Getup()
{
  type = time = busy = 0;
}

void Getup::load()
{
  m_front.load("gu_front.mot");
  m_back .load("gu_back.mot" );
  m_side .load("gu_side.mot" );
}

void Getup::init(int getup_type)
{
  type = getup_type;
  time = 0;
  busy = LEGS_BUSY|HEAD_BUSY;
}

int Getup::getAngles(BodyState &current,BodyState &next)
{
  if(time == 0){
    switch(type){
      case MOTION_GETUP_FRONT: m_front.play(current,time,1,false); break;
      case MOTION_GETUP_BACK:  m_back.play(current,time,1,false);  break;
      case MOTION_GETUP_LEFT:  m_side.play(current,time,1,false);  break;
      case MOTION_GETUP_RIGHT: m_side.play(current,time,1,true);   break;
    }
    time += TimeStep;
  }

  switch(type){
    case MOTION_GETUP_FRONT: busy = m_front.get(next,time); break;
    case MOTION_GETUP_BACK:  busy = m_back.get(next,time);  break;
    case MOTION_GETUP_LEFT:  busy = m_side.get(next,time);  break;
    case MOTION_GETUP_RIGHT: busy = m_side.get(next,time); break;
  }

  time += TimeStep;
  return(busy);
}

int Getup::areBusy()
{
  return(busy);
}

void Getup::getMotionUpdate(MotionLocalizationUpdate &update)
{
  update.stable = false;
}

//====================================================================//
//  Kick Class
//====================================================================//

Kick::Kick()
{
  type = time = busy = 0;
  generated_motion_update = false;
}

void Kick::load()
{
  m_dive     .load("k_dive.mot" );
  m_bump     .load("k_bump.mot" );
  m_fwd      .load("k_fwd.mot"  );
  m_head     .load("k_head.mot" );
  m_heads    .load("k_heads.mot");
  m_headh    .load("k_headh.mot");
  m_hold     .load("k_hold.mot" );
  m_swing    .load("k_swing.mot");
  m_adive    .load("k_adive.mot");
  m_sback    .load("k_sback.mot");
  m_gdive    .load("b_gdive.mot");
  m_gblock   .load("b_gcntr.mot");
  m_suppblock.load("b_supp.mot");
  m_sdive    .load("b_sdive.mot");
  m_happy1   .load("dance1.mot");
  m_happy2   .load("dance2.mot");
  m_happy3   .load("dance3.mot");
  m_sad      .load("sad.mot");
  m_bw_kick  .load("k_bw.mot");
  m_bw_backup.load("bw_back.mot");
}

void Kick::init(int kick_type)
{
  type = kick_type;
  time = 0;
  busy = LEGS_BUSY|HEAD_BUSY|TARGET_BUSY;
  generated_motion_update = false;
}

int Kick::getAngles(BodyState &current,BodyState &next)
{
  if(time == 0){
    switch(type){
      case MOTION_KICK_DIVE:        m_dive .play(current,time,1,false);     break;
      case MOTION_KICK_HOLD:        m_hold .play(current,time,1,false);     break;
      case MOTION_KICK_BUMP:        m_bump .play(current,time,1,false);     break;
      case MOTION_KICK_FOREWARD:    m_fwd  .play(current,time,1,false);     break;
      case MOTION_KICK_HEAD_L:      m_head .play(current,time,1,false);     break;
      case MOTION_KICK_HEAD_R:      m_head .play(current,time,1, true);     break;
      case MOTION_KICK_HEAD_SOFT_L: m_heads.play(current,time,1,false);     break;
      case MOTION_KICK_HEAD_SOFT_R: m_heads.play(current,time,1, true);     break;
      case MOTION_KICK_HEAD_HARD_L: m_headh.play(current,time,1,false);     break;
      case MOTION_KICK_HEAD_HARD_R: m_headh.play(current,time,1, true);     break;
      case MOTION_KICK_SWING_L:     m_swing.play(current,time,1,false);     break;
      case MOTION_KICK_SWING_R:     m_swing.play(current,time,1, true);     break;
      case MOTION_KICK_ARM_DIVE_L:  m_adive.play(current,time,1,false);     break;
      case MOTION_KICK_ARM_DIVE_R:  m_adive.play(current,time,1, true);     break;
      case MOTION_KICK_SLAP_BACK_L: m_sback.play(current,time,1,false);     break;
      case MOTION_KICK_SLAP_BACK_R: m_sback.play(current,time,1, true);     break;
      case MOTION_BLOCK_GOALIE_L:   m_gdive    .play(current,time,1,false); break;
      case MOTION_BLOCK_GOALIE_R:   m_gdive    .play(current,time,1, true); break;
      case MOTION_BLOCK_GOALIE_C:   m_gblock   .play(current,time,1,false); break;
      case MOTION_BLOCK_SUPP:       m_suppblock.play(current,time,1,false); break; 
      case MOTION_BLOCK_SUPP_L:     m_sdive    .play(current,time,1,false); break;
      case MOTION_BLOCK_SUPP_R:     m_sdive    .play(current,time,1, true); break;
      case MOTION_GOAL_HAPPY1:      m_happy1   .play(current,time,1,false); break;
      case MOTION_GOAL_HAPPY2:      m_happy2   .play(current,time,1,false); break;
      case MOTION_GOAL_HAPPY3:      m_happy3   .play(current,time,1,false); break;
      case MOTION_GOAL_SAD:         m_sad      .play(current,time,1,false); break;
      case MOTION_KICK_BW_L:        m_bw_kick  .play(current,time,1,false); break;
      case MOTION_KICK_BW_R:        m_bw_kick  .play(current,time,1,true);  break;
      case MOTION_BACKUP_BW:        m_bw_backup.play(current,time,1,false);  break;
    }
    time += TimeStep;
  }

  switch(type){
    case MOTION_KICK_DIVE:         busy = m_dive.get(next,time);  break;
    case MOTION_KICK_HOLD:         busy = m_hold.get(next,time);  break;
    case MOTION_KICK_BUMP:         busy = m_bump.get(next,time);  break;
    case MOTION_KICK_FOREWARD:     busy = m_fwd.get(next,time);   break;
    case MOTION_KICK_HEAD_L:       busy = m_head.get(next,time);  break;
    case MOTION_KICK_HEAD_R:       busy = m_head.get(next,time);  break;
    case MOTION_KICK_HEAD_SOFT_L:  busy = m_heads.get(next,time); break;
    case MOTION_KICK_HEAD_SOFT_R:  busy = m_heads.get(next,time); break;
    case MOTION_KICK_HEAD_HARD_L:  busy = m_headh.get(next,time); break;
    case MOTION_KICK_HEAD_HARD_R:  busy = m_headh.get(next,time); break;
    case MOTION_KICK_SWING_L:      busy = m_swing.get(next,time); break;
    case MOTION_KICK_SWING_R:      busy = m_swing.get(next,time); break;
    case MOTION_KICK_ARM_DIVE_L:   busy = m_adive.get(next,time); break;
    case MOTION_KICK_ARM_DIVE_R:   busy = m_adive.get(next,time); break; 
    case MOTION_KICK_SLAP_BACK_L:  busy = m_sback.get(next,time); break;
    case MOTION_KICK_SLAP_BACK_R:  busy = m_sback.get(next,time); break;
    case MOTION_BLOCK_GOALIE_L:    busy = m_gdive.get(next,time); break;
    case MOTION_BLOCK_GOALIE_R:    busy = m_gdive.get(next,time); break;
    case MOTION_BLOCK_GOALIE_C:    busy = m_gblock.get(next,time);break;
    case MOTION_BLOCK_SUPP:        busy = m_suppblock.get(next,time);break;
    case MOTION_BLOCK_SUPP_L:      busy = m_sdive.get(next,time); break;
    case MOTION_BLOCK_SUPP_R:      busy = m_sdive.get(next,time); break;
    case MOTION_GOAL_HAPPY1:       busy = m_happy1.get(next,time);break;
    case MOTION_GOAL_HAPPY2:       busy = m_happy2.get(next,time);break;
    case MOTION_GOAL_HAPPY3:       busy = m_happy3.get(next,time);break;
    case MOTION_GOAL_SAD:          busy = m_sad.get(next,time);   break;
    case MOTION_KICK_BW_L:         busy = m_bw_kick  .get(next,time);break;
    case MOTION_KICK_BW_R:         busy = m_bw_kick  .get(next,time);break;
    case MOTION_BACKUP_BW:         busy = m_bw_backup.get(next,time);break;
  }
  if(busy) busy |= TARGET_BUSY;

  time += TimeStep;
  return(busy);
}

int Kick::areBusy()
{
  return(busy);
}

void Kick::getMotionUpdate(MotionLocalizationUpdate &update)
{
  update.stable = false;

  if(!generated_motion_update){
    vector2d x(1,0);

    if(kin.model==Kinematics::ERS210){
      // no kicks that turn it yet
    }else{
      switch(type){
        case MOTION_KICK_SWING_L: x = x.rotate(RAD( 90)); break;
        case MOTION_KICK_SWING_R: x = x.rotate(RAD(-90)); break;
      }
    }

    //pprintf(TextOutputStream,"Kick Motion Update: <%f,%f>\n",
    //        V2COMP(x));

    update.heading_delta = x;
    generated_motion_update = true;
  }
}

//====================================================================//
//  Motion Class
//====================================================================//

Motion::Motion()
{
  // start with a clean slate
  mzero(*this);

  state = MOTION_STAND_NEUTRAL;

  stand.init();
  trot.init(0,0,0,cmd.motion_cmd);
  sound.init();

  // Set up initial command
  // memset(&cmd,0,sizeof(cmd));
  // memset(&body,0,sizeof(body));
  LED_state.cmd = LED_ALL;

  // Initial body orienation (lying down)
  body.pos.loc.set(0,0,60.0);
  body.pos.angle.set(0,0,0);

  time = 0;
  new_cmd = false;

  head_scan_time_start = 0;
  new_head_command = false;
}

void Motion::init()
{
  int i;

  for(i=0; i<4; i++) body.leg[i].attr = ATTR_ANGLES;
  body.head.attr  = ATTR_ANGLES;
  body.mouth.attr = ATTR_ANGLES;

  set3(body.leg[0].angles,  1.2, 0.0, 0.5);
  set3(body.leg[1].angles,  1.2, 0.0, 0.5);
  set3(body.leg[2].angles, -0.5, 0.0, 2.6);
  set3(body.leg[3].angles, -0.5, 0.0, 2.6);
  set3(body.head.angles,   0.0, 0.0, 0.0);
  body.mouth.angle = 0.0;

  body.pos.loc.set(0,0,60.0);
  body.pos.angle.set(0,0,0);

  cmd.motion_cmd = MOTION_STAND_NEUTRAL;
  state = MOTION_STAND_NEUTRAL;
  top_state = MOTION_STATE(state);
}

void Motion::init(double *angles)
{
  int i;

  // Load params
  trot.load();
  getup.load();
  kick.load();
  sound.load();
  twb.load();

  {
    BodyState b;
    trot.getNeutral(b);
    stand.setNeutral(b);
  }

  if(angles){
    for(i=0; i<4; i++){
      body.leg[i].attr = ATTR_ANGLES;
      mcopy(body.leg[i].angles,&angles[3*i],3);
    }

    body.head.attr = ATTR_ANGLES;
    mcopy(body.head.angles,&angles[12],3);

    body.mouth.attr = ATTR_ANGLES;
    mcopy(&body.mouth.angle,&angles[15],1);
  }

  stand.setTarget(body,MOTION_STAND_NEUTRAL,4000);
}

void Motion::setParams(WalkParam &params)
{
  trot.setParams(params);
}

WalkParam* Motion::getXYParams() {
  return &trot.wp_xy;
}

WalkParam* Motion::getAParams() {
  return &trot.wp_a;
}

WalkParam* Motion::getParams() {
  return &trot.wp;
}

bool Motion::newParam(){
  return true;//trot.new_param;
}

void Motion::sentVelParam(){
  trot.new_param = false;
}

void Motion::setCommand(MotionCommand &ncmd)
{
  // get optional debug command overrides
  DebugGetCommand(ncmd);

  new_head_command = (ncmd.head_cmd != cmd.head_cmd);
  if(new_head_command) head_scan_time_start = time;

  // copy command into internal state
  cmd = ncmd;
  LED_state = cmd.led;
  sound.setCommand(ncmd);

  new_cmd = true;
}

void Motion::getAngles(double *angles)
{
#ifdef PLATFORM_APERIOS
  static EventTimeReporter reporter("Motion::getAngles",SecToTime(5.0),SecToTime(100.0),10000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);
#endif

  BodyState next;
  double a, da;
  int busy;
  int i;
  int cmd_top_state;

  next = body;
  busy = 0;

  top_state = MOTION_STATE(state);
  //printf("state=%d:%d  ",state,top_state);
  switch(top_state){
    case STATE_STANDING:
      busy = stand.getAngles(body,next);
      break;

    case STATE_WALKING:
      busy = trot.getAngles(body,next);
      busy = 0;
      break;

    case STATE_KICKING:
      busy = kick.getAngles(body,next);
      if(!busy){
        state = MOTION_STAND_NEUTRAL;
        stand.setTarget(body,state,200);
        busy = stand.getAngles(body,next);
      }
      break;

    case STATE_TURN_WITH_BALL:
      busy = twb.getAngles(body,next);
      if(!busy){
	twb.setNextMot(cmd.motion_cmd);
	busy = twb.getAngles(body,next);
      }
      break;

    case STATE_GETUP:
      busy = getup.getAngles(body,next);
      if(!busy){
        state = MOTION_STAND_NEUTRAL;
        stand.setTarget(body,state,500);
        busy = stand.getAngles(body,next);
      }
      break;

    case STATE_SKATING:
      busy = trot.getAngles(body,next);
      busy = 0;
      break;

  }

  cmd_top_state = MOTION_STATE(cmd.motion_cmd);
  //printf("cmd=%d ctstate=%d\n",cmd.motion_cmd,cmd_top_state);
  switch(cmd_top_state){
    case STATE_STANDING:
      if(cmd.motion_cmd!=state &&
         (!busy || (top_state==STATE_WALKING || top_state==STATE_STANDING ||
		    top_state==STATE_TURN_WITH_BALL))){
        stand.setTarget(body,cmd.motion_cmd,300);
        state = cmd.motion_cmd;
      }
      break;

    case STATE_WALKING: {
      // bool change_to_walk = (MOTION_STATE(cmd.motion_cmd) != MOTION_STATE(state));
      // printf("change_to_walk=%d\n",change_to_walk);
      if(!busy && state!=cmd.motion_cmd){
	if(MOTION_STATE(cmd.motion_cmd) != MOTION_STATE(state)){
	  trot.init(cmd.vx,cmd.vy,cmd.va,cmd.motion_cmd);
	}else{
	  trot.setWalkType(cmd.motion_cmd);
	}
        state = cmd.motion_cmd;
        new_cmd = true;
      }else if(top_state==STATE_TURN_WITH_BALL){
        stand.setTarget(body,MOTION_STAND_NEUTRAL,300);
        state = MOTION_STAND_NEUTRAL;
      }
      if(new_cmd && state==cmd.motion_cmd){
        trot.setTarget(cmd.vx,cmd.vy,cmd.va,1.5,cmd.bound_mode);
        new_cmd = false;
      }
    } break;

    case STATE_KICKING:
      if(!busy || top_state==STATE_WALKING){
        kick.init(cmd.motion_cmd);
        state = cmd.motion_cmd;
      }else if(top_state==STATE_TURN_WITH_BALL){
        stand.setTarget(body,MOTION_STAND_NEUTRAL,300);
        state = MOTION_STAND_NEUTRAL;
      }
      break;

    case STATE_TURN_WITH_BALL:
      if(cmd_top_state != top_state){
        twb.init(cmd.motion_cmd);
        state = cmd.motion_cmd;
      }
      break;

    case STATE_GETUP:
      if(cmd.motion_cmd!=state &&
         !(busy && (top_state==STATE_GETUP ||
                    top_state==STATE_STANDING ||
                    top_state==STATE_KICKING ||
		    top_state==STATE_TURN_WITH_BALL))){
        getup.init(cmd.motion_cmd);
        state = cmd.motion_cmd;
      }
      break;

    case STATE_SKATING:
      if(!busy && cmd.motion_cmd!=state){
	// trot.init();
	trot.init(0,0,0,cmd.motion_cmd);
        state = cmd.motion_cmd;
        new_cmd = true;
      }
      if(new_cmd && state==cmd.motion_cmd){
        // printf("SetTarget(%f,%f,%f)\n",cmd.vx,cmd.vy,cmd.va);
        trot.setTarget(cmd.vx,cmd.vy,cmd.va,1.5,cmd.bound_mode);
        new_cmd = false;
      }
      break;

  }
  top_state = MOTION_STATE(state);

  // pprintf(TextOutputStream,"state=%d busy=%d cmd=%d hcmd=%d\n",
  //         state,busy,cmd.motion_cmd,cmd.head_cmd);

  // Set head
  if(!(busy & HEAD_BUSY)){

    // Only used in HEAD_ANGLES, but compiler whines if not done
    // outside of switch.
    double third = 0;

    switch(cmd.head_cmd){
      case HEAD_NO_CMD:
        next.head.attr = ATTR_ANGLES;
        set3(next.head.angles,
             bound(0.0,kin.robot.head_tilt_min,kin.robot.head_tilt_max),
             bound(0.0,kin.robot.head_pan_min ,kin.robot.head_pan_max ),
             bound(0.0,kin.robot.head_roll_min,kin.robot.head_roll_max));
        break;
      case HEAD_LOOKAT:
        next.head.attr = ATTR_POSITION;
        next.head.target = cmd.head_lookat;
        break;
      case HEAD_ANGLES:
        next.head.attr = ATTR_ANGLES;
	
	if(kin.model==Kinematics::ERS210)
	  third = bound(cmd.head_roll,kin.robot.head_roll_min,
			kin.robot.head_roll_max);
	else
	  third = bound(cmd.head_tilt2, kin.robot.head_tilt2_min, 
			kin.robot.head_tilt2_max);

        set3(next.head.angles,
             bound(cmd.head_tilt,kin.robot.head_tilt_min,kin.robot.head_tilt_max),
             bound(cmd.head_pan ,kin.robot.head_pan_min ,kin.robot.head_pan_max ),
             third);
        break;
      case HEAD_SCAN_BALL:
      case HEAD_SCAN_BALL_TURN:
         da = (top_state == STATE_WALKING)? trot.velocity().z : 0.0;

	 if(fabs(da)<0.5 && cmd.head_cmd!=HEAD_SCAN_BALL_TURN){
	   next.head.attr = ATTR_ANGLES;

	   if(kin.model==Kinematics::ERS210){
	     a = (time / 2000.0) * 2*M_PI;
	     set3(next.head.angles,
		  body.pos.angle.y - RAD(20) +
		  min(cos(a)*camera_vert_fov,0.0),
		  1.4*sin(a),0.0);
	   }else{
	     a = ((time - head_scan_time_start) / 2000.0) * 2*M_PI;
	     set3(next.head.angles,
		  body.pos.angle.y - RAD(13) +
		  0.8*min(cos(a)*camera_vert_fov,80.0),
		  1.4*sin(a), - 0.25);
	   }
	 }else{
	   next.head.attr = ATTR_ANGLES;
	   if(kin.model==Kinematics::ERS210){
	     set3(next.head.angles,
		  RAD(-90)+body.pos.angle.y,
		  RAD( 70)*sign(da),
		  0.0);
	   }else{
	     set3(next.head.angles,
		  RAD(-70)+body.pos.angle.y,
		  RAD( 70)*sign(da),
		  0.0);
	   }
	 }
        break;
      case HEAD_SCAN_BALL_NEAR:
	a = (time / 500.0);
	next.head.attr = ATTR_POSITION;
	next.head.target.set(fabs(cos(a))*750 - 150,sin(a)*300,10.0);
	
	break;
      case HEAD_SCAN_DRIBBLE:
        next.head.attr = ATTR_ANGLES;
        a = (time / 1000.0) * 2*M_PI;
        set3(next.head.angles,body.pos.angle.y-0.40,1.0*sin(a),0.0);
        break;
      case HEAD_SCAN_MARKERS:
        next.head.attr = ATTR_ANGLES;
        a = (time / 2500.0) * 2*M_PI;
        set3(next.head.angles,body.pos.angle.y,1.4*sin(a),0.0);
        break;
      case HEAD_SCAN_MARKERS_HIGH:
        next.head.attr = ATTR_ANGLES;
        a = (time / 2500.0) * 2*M_PI;
        set3(next.head.angles,body.pos.angle.y,1.4*sin(a),0.25);
        break;
      case HEAD_SCAN_POINT:
        a = (time / 750.0) * 2*M_PI;
        next.head.attr = ATTR_POSITION;
        next.head.target = cmd.head_lookat.rotate_z(0.3*sin(a));
        break;
      case HEAD_SCAN_OBSTACLES:
        next.head.attr = ATTR_ANGLES;
        a = (time / 2000.0) * 2*M_PI;
        set3(next.head.angles,body.pos.angle.y+cos(a)*0.60,1.2*sin(a),0.0);
        break;
    }
  }

  // Set mouth
  if(!(busy & MOUTH_BUSY)){
    next.mouth.attr = ATTR_ANGLES;
    next.mouth.angle = RAD(bound((-cmd.mouth*50)/100,-50,0));
  }

  // update body state and bound tilt speed
  Complete(next);

  // add in head tilt offset
  // body.head.angles[0] += cmd.head_tilt_offset;

  
  // bound tilt speed
  //double b;
  //a = bound(next.head.angles[0],
  //          body.head.angles[0]-0.5,
  //          body.head.angles[0]+0.5);
  //b = bound(next.head.angles[1],
  //          body.head.angles[1]-0.1,
  //          body.head.angles[1]+0.1);
  //

  body = next;
  
  //body.head.angles[0] = a;
  //body.head.angles[1] = b;
  

  // extract angles
  for(i=0; i<4; i++){
    mcopy(&angles[3*i],body.leg[i].angles,3);
  }
  mcopy(&angles[12],body.head.angles,3);

  // Set tail
  switch(cmd.tail_cmd) {
    case TAIL_NO_CMD:
      angles[15] = 0.0;
      angles[16] = 0.0;
      break;
    case TAIL_AIM:
      angles[15] = kin.calcTailPan(cmd.tail_pan);
      angles[16] = kin.calcTailTilt(cmd.tail_tilt);
      //      angles[15] = bound(RAD((cmd.tail_pan *22)/100),kin.robot.tail_min,kin.robot.tail_max);
      //angles[16] = bound(RAD((cmd.tail_tilt*22)/100),kin.robot.tail_min,kin.robot.tail_max);
      break;
    case TAIL_FOLLOW:
#ifdef PLATFORM_APERIOS
      {
        double tail_pan,tail_tilt;
        tail_pan = tail_tilt = 0.0;
        for(int i=0; i<4; i++){
          tail_pan  += sensor.getFrame(-i*4)->tailAngles[0];
          tail_tilt += sensor.getFrame(-i*4)->tailAngles[1];
        }
        tail_pan  /= 4.0;
        tail_tilt /= 4.0;
        angles[15] = bound(tail_pan,kin.robot.tail_pan_min,kin.robot.tail_pan_max);
        angles[16] = bound(tail_tilt,kin.robot.tail_tilt_min,kin.robot.tail_tilt_max);
      }
#endif
      break;
  }

  // extract mouth angles
  mcopy(&angles[17],&body.mouth.angle,1);

  // blink top LED to indicate when new commands have been recieved
//   if(new_cmd){

//     if(kin.model==Kinematics::ERS210){ LED_state.cmd |= LED_TOP; }
//     else{ LED_state.cmd |= LED_TOP_ORANGE;}

//     new_cmd = false;
//   }else{
//     if(kin.model==Kinematics::ERS210){ LED_state.cmd &= ~LED_TOP; }
//     else{ LED_state.cmd &= ~LED_TOP_ORANGE;}
    
//   }

  time += TimeStep;
}

void Motion::getMotionUpdate(MotionLocalizationUpdate &update,ulong time)
{  
  update.timestamp  = time;
  // update.state_type = (enable)? top_state : STATE_WAITING;
  update.state_type = top_state;
  update.state      = state;

  update.pos_delta.set(0,0);
  update.heading_delta.set(0,0);
  update.body.height = body.pos.loc.z;
  update.body.angle  = body.pos.angle.y;
  update.body.neck_offset.set(kin.robot.body_to_neck.x*cos(body.pos.angle.y),0);
  update.stable      = true;

  switch(top_state){
    case STATE_GETUP:
      getup.getMotionUpdate(update);
      break;

    case STATE_STANDING:
      stand.getMotionUpdate(update);
      break;

    case STATE_WALKING:
      trot.getMotionUpdate(update);
      break;

    case STATE_TURN_WITH_BALL:
      twb.getMotionUpdate(update);
      break;

    case STATE_KICKING:
      kick.getMotionUpdate(update);
      break;

    case STATE_SKATING:
      stand.getMotionUpdate(update);
      break;
  }

  // body = update.body;
}

} // Namespace