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

/*
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  fl.set( 125, 82);
  bl.set(-100, 75);

  BodyPos(b,98,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegPos(b,0, fl.x, fl.y, 0);
  LegPos(b,1, fl.x,-fl.y, 0);
  LegPos(b,2, bl.x, bl.y, 0);
  LegPos(b,3, bl.x,-bl.y, 0);
  m[n].body = b;
  m[n].time = 0;
  n++;

  a = 2.0;
  fl = fl.rotate(a);
  bl = bl.rotate(a);

  LegPos(b,0, fl.x, fl.y, 0);
  LegPos(b,1, fl.x,-fl.y, 0);
  LegPos(b,2, bl.x, bl.y, 0);
  LegPos(b,3, bl.x,-bl.y, 0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  LegAng(b,0,-0.0, 1.5, 0.0);
  LegAng(b,1,-0.0, 1.5, 0.0);
  LegAng(b,2, 0.1, 0.0, 0.2);
  LegAng(b,3, 0.1, 0.0, 0.2);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  m[n].body = b;
  m[n].time = 100;
  n++;
*/

/*
  // Spin kick 1.0
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,98+10,RAD(16-16));
  HeadAng(b, 0.5,-1.5, 0.0);
  LegPos(b,0, 125   , 82   ,  0);
  LegPos(b,1, 125-80,-82   , 15);
  LegPos(b,2,-100   , 75   ,  0);
  LegPos(b,3,-100   ,-75   ,  0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegPos(b,0, 125   , 82   ,  0);
  LegPos(b,1, 125-80,-82   ,  0);
  LegPos(b,2,-100   , 75   ,  0);
  LegPos(b,3,-100   ,-75   ,  0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegPos(b,0, 125+40, 82+30, 15);
  LegPos(b,1, 125-40,-82   ,  0);
  LegPos(b,2,-100+40, 75   ,  0);
  LegPos(b,3,-100+40,-75   ,  0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  p[0].set( 125, 82);
  p[1].set( 125,-82);
  p[2].set(-100, 75);
  p[3].set(-100,-75);

  a = -0.25;
  for(i=0; i<4; i++){
    tp = p[i].rotate(a);
    LegPos(b,i,tp.x,tp.y,0);
  }
  m[n].body = b;
  m[n].time = 0;
  n++;

  a = 1.0;
  for(i=0; i<4; i++){
    tp = p[i].rotate(a);
    LegPos(b,i,tp.x,tp.y,0);
  }
  LegAng(b,0, 1.0,-0.2, 1.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  m[n].body = b;
  m[n].time = 100;
  n++;

  out = fopen("k_spin.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
*/

vector2d rotate(vector2d p,vector2d origin,double a)
{
  return(origin + (p-origin).rotate(a));
}

void make_side_kicks()
{
  Motion::BodyStateMotion m[64];
  Motion::BodyState b;
  FILE *out;
  int i,n;
  double a,amp,d,s;
  vector2d p[4],tp,origin;

  mzero(m,64);

  //==== Strong Head Kick ====//
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 150;
  n++;

  d = 0;
  p[0].set( 145-d-30, 82   );
  p[1].set( 145-d   ,-82   );
  p[2].set(-100-d   , 75   );
  p[3].set(-100-d   ,-75   );
  origin.set(-75,0);

  amp = 0.6;
  for(a= 0.0; a<1.0; a+=0.1){
    if(false){
      // Jim's version
      BodyPos(b,98-20*a,RAD(20));
      HeadAng(b,a*-1.1,a*-1.5, 0.0);
    }else{
      // Sonia's version
      BodyPos(b,98-20*a,RAD(20));
      HeadAng(b,a*-0.9,a*-1.5, 0.0);
    }

    for(i=0; i<4; i++){
      tp = rotate(p[i],origin,a*amp);
      LegPos(b,i,tp.x,tp.y,0);
    }
    m[n].body = b;
    m[n].time = 40;
    n++;
  }

  if(false) BodyPos(b,98-20*a,RAD(20));
  d = 20;
  p[0].set( 125-d-30, 82   );
  p[1].set( 125-d   ,-82   );
  p[2].set(-100-d   , 75   );
  p[3].set(-100-d   ,-75   );

  for(a=1.0; a>-1.0; a-=0.1){
    HeadAng(b,-1.2,a*-1.5, 0.0);
    for(i=0; i<4; i++){
      tp = rotate(p[i],origin,a*amp);
      LegPos(b,i,tp.x,tp.y,0);
    }
    m[n].body = b;
    m[n].time = 16;
    n++;
  }

  m[n].body = b;
  m[n].time = 100; // 240;
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_headh.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Spin Kick ====//
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 150;
  n++;

  d = -40; s = 0;
  p[0].set( 125-d-40, 82-s);
  p[1].set( 125-d   ,-82-s);
  p[2].set(-100-d   , 75-s);
  p[3].set(-100-d   ,-75-s);
  origin.set(-50,0);

  HeadAng(b, 0.5, 1.5, 0.0);
  BodyPos(b,98,RAD(20));

  for(a=0.0; a<1.0; a+=0.1){
    for(i=0; i<4; i++){
      tp = rotate(p[i],origin,0.75*a);
      LegPos(b,i,tp.x,tp.y,0);
    }
    m[n].body = b;
    m[n].time = 40;
    n++;
  }

  LegAng(b,1, 1.0, 1.2, 0.0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  d = -35;
  p[0].set( 125-d-40, 82-s);
  p[1].set( 125-d   ,-82-s);
  p[2].set(-100-d   , 75-s);
  p[3].set(-100-d   ,-75-s);

  for(a=1.0; a>-1.0; a-=0.2){
    for(i=0; i<4; i++){
      tp = rotate(p[i],origin,0.75*a);
      LegPos(b,i,tp.x,tp.y,0);
    }
    LegAng(b,1, 1.4, 0.0, 0.0);
    m[n].body = b;
    m[n].time = 24;
    n++;
  }

  m[n].body = b;
  m[n].time = 500;
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_spin.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
}

void make_dive_kicks()
{
  Motion::BodyStateMotion m[64];
  Motion::BodyState b;
  FILE *out;
  int n;
  double s;

  mzero(m,64);
  s = 1.0;

  //==== Weird ERS7 Kick ====//
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  // grab ball
  HeadAng(b,-0.9,0.0,0.8);
  LegAng(b,0, 1.0,-0.1, 1.0);
  LegAng(b,1, 1.0,-0.1, 1.0);
  LegAng(b,2, 1.5, 0.0, 2.1);
  LegAng(b,3, 1.5, 0.0, 2.1);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  HeadAng(b,-0.9, 1.5,0.8);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  HeadAng(b,-1.5, 1.5,0.4);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  // grab ball
  HeadAng(b,-1.5, 0.0,0.0);
  LegAng(b,1, 1.5, 1.5, 0.0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  m[n].body = b;
  m[n].time = 200;
  n++;

  // grab ball
  LegAng(b,1, 1.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  /*
  out = fopen("k_twist_E7.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
  */

  //==== ERS7 Bump Kick ====//
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  // Rear up (now rear too)
  HeadAng(b,  0.0, 1.57, 0.0);
  LegAng(b,0, 0.3, 0.00, 1.2);
  LegAng(b,1, 0.3, 0.00, 1.2);
  LegAng(b,2,-1.3, 0.00, 1.8);
  LegAng(b,3,-1.3, 0.00, 1.8);
  m[n].body = b;
  m[n].time = (int)(s*400);
  n++;

  // Rear up (now rear too)
  LegAng(b,0, 1.5, 0.5, 1.5);
  LegAng(b,1, 1.5, 0.5, 1.5);
  LegAng(b,2, 1.5, 0.0, 1.5);
  LegAng(b,3, 1.5, 0.0, 1.5);
  m[n].body = b;
  m[n].time = (int)(s*1000);
  n++;

  // Stall at bottom
  m[n].body = b;
  m[n].time = (int)(s*1000);
  n++;

  // Curl all four legs
  LegAng(b,0,-0.0, 0.2, 2.0);
  LegAng(b,1,-0.0, 0.2, 2.0);
  LegAng(b,2,-1.0, 0.2, 2.0);
  LegAng(b,3,-1.0, 0.2, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_sbumpE7.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== ERS7 Backward Kick ====//
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  // Grab ball
  HeadAng(b, -1.2, 1.5, 0.4);
  LegAng(b,0, 1.2,-0.2, 1.0);
  LegAng(b,1, 1.2,-0.2, 1.0);
  LegAng(b,2,-1.0, 0.5, 2.0);
  LegAng(b,3,-1.0, 0.5, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Grab ball between head and front leg
  HeadAng(b, -1.3,-0.2, 0.6);
  LegAng(b,0, 1.5, 0.0, 0.0);
  LegAng(b,1, 0.0, 1.4, 1.4);
  LegAng(b,2,-1.0, 0.5, 2.0);
  LegAng(b,3,-1.0, 0.5, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Use head to push onto leg
  HeadAng(b, -1.0,-1.0, 0.5);
  LegAng(b,0, 1.5, 0.0, 0.0);
  LegAng(b,1, 0.0, 0.5, 1.5);
  LegAng(b,2,-2.1, 0.6,-0.2);
  LegAng(b,3,-2.1, 0.6,-0.2);
  m[n].body = b;
  m[n].time = (int)(s*250);
  n++;

  // Twist front arm to get ball onto back leg
  LegAng(b,0, 1.5, 0.0, 0.0);
  LegAng(b,1, 2.0, 0.7, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  /*
  // Stall to let ball fall onto back leg
  HeadAng(b, -1.0, 0.0, 0.5);
  m[n].body = b;
  m[n].time = (int)(s*250);
  n++;
  */

  // Pick up ball onto back
  LegAng(b,0,-1.0, 0.50, 2.2);
  LegAng(b,1,-1.0, 0.50, 2.2);
  LegAng(b,2,-2.1,-0.20, 0.0);
  LegAng(b,3,-2.1,-0.20, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Throw ball backwards
  LegAng(b,0, 0.0, 0.00, 0.0);
  LegAng(b,1, 0.0, 0.00, 0.0);
  LegAng(b,2,-2.3,-0.20,-0.5);
  LegAng(b,3,-2.3,-0.20,-0.5);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Start getup sequence
  LegAng(b,2,-2.0, 1.57, 0.0);
  LegAng(b,3,-2.0, 1.57, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Start getup sequence
  LegAng(b,0, 0.0, 0.2, 2.0);
  LegAng(b,1, 0.0, 0.2, 2.0);
  LegAng(b,2, 1.57, 1.57, 0.0);
  LegAng(b,3, 1.57, 1.57, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Start getup sequence
  LegAng(b,2, 1.57, 0.00, 0.0);
  LegAng(b,3, 1.57, 0.00, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Curl all four legs
  LegAng(b,2,-1.0, 0.2, 2.0);
  LegAng(b,3,-1.0, 0.2, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_backwE7.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Launching Dive Kick ====//
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,98,RAD(16));
  HeadAng(b,-1.5, 0.0, 0.0);
  LegPos(b,0, 125, 82,0);
  LegPos(b,1, 125,-82,0);
  LegPos(b,2,-100, 75,0);
  LegPos(b,3,-100,-75,0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up (front only first)
  LegAng(b,0, 0.3, 0.10, 0.0);
  LegAng(b,1, 0.3, 0.10, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up (now rear too)
  LegAng(b,0, 0.3, 0.10, 0.0);
  LegAng(b,1, 0.3, 0.10, 0.0);
  LegAng(b,2,-0.6, 0.30, 0.6);
  LegAng(b,3,-0.6, 0.30, 0.6);
  m[n].body = b;
  m[n].time = (int)(s*300);
  n++;

  // Stall near top
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up high
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.1, 0.10, 0.0);
  LegAng(b,1, 0.1, 0.10, 0.0);
  LegAng(b,2,-0.4, 0.30, 0.6);
  LegAng(b,3,-0.4, 0.30, 0.6);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Flop over by swaying legs back
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0,-1.2, 0.5, 0.0);
  LegAng(b,1,-1.2, 0.5, 0.0);
  LegAng(b,2, 1.2, 0.5, 1.5);
  LegAng(b,3, 1.2, 0.5, 1.5);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Stall at bottom
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Start the get-up sequence
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 2.0);
  LegAng(b,1, 0.0, 1.5, 2.0);
  LegAng(b,2,-0.5, 1.5, 2.0);
  LegAng(b,3,-0.5, 1.5, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 1.0, 0.2, 1.0);
  LegAng(b,1, 1.0, 0.2, 1.0);
  LegAng(b,2,-1.0, 0.2, 2.0);
  LegAng(b,3,-1.0, 0.2, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_ldive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Launching Side Dive Kick ====//

  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,98,RAD(16));
  HeadAng(b,-1.5, 0.0, 0.0);
  LegPos(b,0, 125, 82,0);
  LegPos(b,1, 125,-82,0);
  LegPos(b,2,-100, 75,0);
  LegPos(b,3,-100,-75,0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up (front only first)
  LegAng(b,0, 0.3, 0.0, 0.0);
  LegAng(b,1, 0.3, 0.0, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*1000);
  n++;

  // Rear up (now rear too)
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.3, 0.0, 0.0);
  LegAng(b,1, 0.3, 0.0, 0.0);
  LegAng(b,2,-0.6, 0.0, 0.5);
  LegAng(b,3,-0.6, 0.0, 0.5);
  m[n].body = b;
  m[n].time = (int)(s*1000);
  n++;

  // Stall at top
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.3, 0.0, 1.0);
  LegAng(b,1, 0.3, 0.0, 1.0);
  LegAng(b,2,-0.6, 0.0, 0.5);
  LegAng(b,3,-0.6, 0.0, 0.5);
  m[n].body = b;
  m[n].time = (int)(s*1000);
  n++;

  m[n].body = b;
  m[n].time = (int)(s*100);
  n++;

  // Spin out back legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 1.5, 1.0, 1.0);
  LegAng(b,1, 1.5, 0.0, 1.0);
  LegAng(b,2,-1.2, 0.1, 0.5);
  LegAng(b,3, 0.0, 0.1, 0.5);
  m[n].body = b;
  m[n].time = (int)(s*1000);
  n++;

  // Stall at bottom
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Start the get-up sequence
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 2.0);
  LegAng(b,1, 0.0, 1.5, 2.0);
  LegAng(b,2,-0.5, 1.5, 2.0);
  LegAng(b,3,-0.5, 1.5, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 1.0, 0.2, 1.0);
  LegAng(b,1, 1.0, 0.2, 1.0);
  LegAng(b,2,-1.0, 0.2, 2.0);
  LegAng(b,3,-1.0, 0.2, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_sdive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  /*
  // Backward kick prototype
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,98,RAD(16));
  HeadAng(b,-1.5, 0.0, 0.0);
  LegPos(b,0, 125, 82,0);
  LegPos(b,1, 125,-82,0);
  LegPos(b,2,-100, 75,0);
  LegPos(b,3,-100,-75,0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up (front only first)
  LegAng(b,0, 0.3, 0.0, 0.0);
  LegAng(b,1, 0.3, 0.0, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up (now rear too)
  LegAng(b,0, 0.3, 0.0, 0.0);
  LegAng(b,1, 0.3, 0.0, 0.0);
  LegAng(b,2,-0.5, 0.0, 0.5);
  LegAng(b,3,-0.5, 0.0, 0.5);
  m[n].body = b;
  m[n].time = (int)(s*300);
  n++;

  // Stall at top
  LegAng(b,0, 0.1, 0.0, 0.0);
  LegAng(b,1, 0.1, 0.0, 0.0);
  LegAng(b,2, 0.2, 0.0, 0.0);
  LegAng(b,3, 0.2, 0.0, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*300);
  n++;

  for(int i=0; i<5; i++){
    a = 0.3;

    // Rear up high
    HeadAng(b, 0.5, 1.5, 0.0);
    LegAng(b,0, a+0.1, 0.0, 0.0);
    LegAng(b,1, a+0.1, 0.0, 0.0);
    LegAng(b,2,-a+0.2, 0.0, 0.0);
    LegAng(b,3,-a+0.2, 0.0, 0.0);
    m[n].body = b;
    m[n].time = (int)(s*120);
    n++;

    // Rear up high
    HeadAng(b, 0.5, 1.5, 0.0);
    LegAng(b,0,-a+0.1, 0.0, 0.0);
    LegAng(b,1,-a+0.1, 0.0, 0.0);
    LegAng(b,2, a+0.2, 0.0, 0.0);
    LegAng(b,3, a+0.2, 0.0, 0.0);
    m[n].body = b;
    m[n].time = (int)(s*120);
    n++;

    // Rear up high
    HeadAng(b, 0.5, 1.5, 0.0);
    LegAng(b,0,-a+0.1, 0.0, 0.0);
    LegAng(b,1,-a+0.1, 0.0, 0.0);
    LegAng(b,2,-a+0.2, 0.0, 0.0);
    LegAng(b,3,-a+0.2, 0.0, 0.0);
    m[n].body = b;
    m[n].time = (int)(s*120);
    n++;

    // Rear up high
    HeadAng(b, 0.5, 1.5, 0.0);
    LegAng(b,0, a+0.1, 0.0, 0.0);
    LegAng(b,1, a+0.1, 0.0, 0.0);
    LegAng(b,2,-a+0.2, 0.0, 0.0);
    LegAng(b,3,-a+0.2, 0.0, 0.0);
    m[n].body = b;
    m[n].time = (int)(s*120);
    n++;
  }

  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Spread front feet
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 0.5, 0.0);
  LegAng(b,3, 0.0, 0.5, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Then rear
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 1.5, 1.5, 1.0);
  LegAng(b,1, 1.5, 1.5, 1.0);
  LegAng(b,2, 0.0, 1.5, 0.0);
  LegAng(b,3, 0.0, 1.5, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Close front feet
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 1.5, 0.0, 1.0);
  LegAng(b,1, 1.5, 0.0, 1.0);
  LegAng(b,2, 0.0, 0.5, 0.0);
  LegAng(b,3, 0.0, 0.5, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*500);
  n++;

  // Stall at bottom
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Start the get-up sequence
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 2.0);
  LegAng(b,1, 0.0, 1.5, 2.0);
  LegAng(b,2,-0.5, 1.5, 2.0);
  LegAng(b,3,-0.5, 1.5, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 1.0, 0.2, 1.0);
  LegAng(b,1, 1.0, 0.2, 1.0);
  LegAng(b,2,-1.0, 0.2, 2.0);
  LegAng(b,3,-1.0, 0.2, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_sdive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
  */

  //==== Launching Dive Kick w/ Safety Arm ====//

  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,98,RAD(16));
  HeadAng(b,-1.5, 0.0, 0.0);
  LegPos(b,0, 125, 82,0);
  LegPos(b,1, 125,-82,0);
  LegPos(b,2,-100, 75,0);
  LegPos(b,3,-100,-75,0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up (front only first)
  LegAng(b,0, 0.3, 0.10, 0.0);
  LegAng(b,1, 0.3, 0.10, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up (now rear too)
  LegAng(b,0, 0.3, 0.10, 0.0);
  LegAng(b,1, 0.3, 0.10, 0.0);
  LegAng(b,2,-0.6, 0.30, 0.6);
  LegAng(b,3,-0.6, 0.30, 0.6);
  m[n].body = b;
  m[n].time = (int)(s*300);
  n++;

  // Stall near top
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up high
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.1, 0.10, 0.0);
  LegAng(b,1, 0.1, 0.10, 0.0);
  LegAng(b,2,-0.4, 0.30, 0.6);
  LegAng(b,3,-0.4, 0.30, 0.6);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Flop over by swaying legs back
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0,-1.2, 0.5, 0.0);
  LegAng(b,1, 1.4,-0.2, 1.6); // this leg forward
  LegAng(b,2, 1.2, 0.5, 1.5);
  LegAng(b,3, 1.2, 0.5, 1.5);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Stall at bottom
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Start the get-up sequence
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 1.0, 0.2, 1.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 2.0);
  LegAng(b,1, 1.0, 0.2, 1.0);
  LegAng(b,2,-0.5, 1.5, 2.0);
  LegAng(b,3,-0.5, 1.5, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 1.0, 0.2, 1.0);
  LegAng(b,1, 1.0, 0.2, 1.0);
  LegAng(b,2,-1.0, 0.2, 2.0);
  LegAng(b,3,-1.0, 0.2, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_sdive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  /*
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,98,RAD(16));
  HeadAng(b,-1.5, 0.0, 0.0);
  LegPos(b,0, 125, 82,0);
  LegPos(b,1, 125,-82,0);
  LegPos(b,2,-100, 75,0);
  LegPos(b,3,-100,-75,0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  a = 0.2;
  // Rear up (front only first)
  LegAng(b,0, 0.3  , 0.10, 0.0  );
  LegAng(b,1, 0.3+a, 0.10, 0.0+a);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up (now rear too)
  LegAng(b,0, 0.3  , 0.10, 0.0  );
  LegAng(b,1, 0.3+a, 0.10, 0.0+a);
  LegAng(b,2,-0.6  , 0.30, 0.6  );
  LegAng(b,3,-0.6-a, 0.30, 0.6+a);
  m[n].body = b;
  m[n].time = (int)(s*300);
  n++;

  // Stall near top
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Rear up high
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.1  , 0.10, 0.0  );
  LegAng(b,1, 0.1+a, 0.10, 0.0+a);
  LegAng(b,2,-0.4  , 0.30, 0.6  );
  LegAng(b,3,-0.4-a, 0.30, 0.6+a);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Flop over by swaying legs back
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0,-1.2, 0.5, 0.0);
  LegAng(b,1,-1.2, 0.5, 0.0);
  LegAng(b,2, 1.2, 0.5, 1.5);
  LegAng(b,3, 1.2, 0.5, 1.5);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Stall at bottom
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Start the get-up sequence
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = (int)(s*200);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 2.0);
  LegAng(b,1, 0.0, 1.5, 2.0);
  LegAng(b,2,-0.5, 1.5, 2.0);
  LegAng(b,3,-0.5, 1.5, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  // Curl all four legs
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 1.0, 0.2, 1.0);
  LegAng(b,1, 1.0, 0.2, 1.0);
  LegAng(b,2,-1.0, 0.2, 2.0);
  LegAng(b,3,-1.0, 0.2, 2.0);
  m[n].body = b;
  m[n].time = (int)(s*150);
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_sdive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
  */
}

void make_kicks()
{
  Motion::BodyStateMotion m[96];
  Motion::BodyState b;
  FILE *out;
  int n;
  double f,s,o;
  int t;

  mzero(m,32);
  t = 1;

  //==== Hold Kick ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  HeadAng(b, RAD(20), RAD(90), 0.0);
  LegAng(b,0, 0.6,-0.1, 1.57);
  LegAng(b,1, 0.6,-0.1, 1.57);
  LegAng(b,2, -.72, 0.2, 1.48);
  LegAng(b,3, -.72, 0.2, 1.48);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b, RAD(20), 0.0, 0.0);
  LegAng(b,0, 0.6,-0.1, 1.57);
  LegAng(b,1, 0.6,-0.1, 1.57);
  LegAng(b,2, -.72, 0.2, 1.48);
  LegAng(b,3, -.72, 0.2, 1.48);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b, RAD(20), RAD(-90), 0.0);
  LegAng(b,0, 0.6,-0.1, 1.57);
  LegAng(b,1, 0.6,-0.1, 1.57);
  LegAng(b,2, -.72, 0.2, 1.48);
  LegAng(b,3, -.72, 0.2, 1.48);
  m[n].body = b;
  m[n].time = 300;
  n++;


  out = fopen("k_hold.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Grab Kick ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegAng(b,0, 1.2,-0.2, 0.5);
  LegAng(b,1, 1.2,-0.2, 0.5);
  LegAng(b,2, 1.2, 1.0, 0.5);
  LegAng(b,3, 1.2, 1.0, 0.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  // LegAng(b,0, 1.8,-0.2,-0.5);
  // LegAng(b,1, 1.8,-0.2,-0.5);
  LegAng(b,2,-2.5, 1.0, 2.5);
  LegAng(b,3,-2.5, 1.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  // LegAng(b,0, 1.8,-0.2,-0.5);
  // LegAng(b,1, 1.8,-0.2,-0.5);
  LegAng(b,2,-1.0, 0.5, 0.0);
  LegAng(b,3,-1.0, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  m[n].body = b;
  m[n].time = 400;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.2, 0.1, 0.5);
  LegAng(b,1, 1.2, 0.1, 0.5);
  // LegAng(b,2, 1.2, 0.0, 0.5);
  // LegAng(b,3, 1.2, 0.0, 0.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.9, 0.1,-0.5);
  LegAng(b,1, 1.9, 0.1,-0.5);
  // LegAng(b,2, 1.2, 0.0, 0.5);
  // LegAng(b,3, 1.2, 0.0, 0.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.9,-0.2,-0.5);
  LegAng(b,1, 1.9,-0.2,-0.5);
  LegAng(b,2,-1.0, 0.5, 0.0);
  LegAng(b,3,-1.0, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 800;  // 2.2
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0,-0.5,-0.1, 0.0);
  LegAng(b,1,-0.5,-0.1, 0.0);
  LegAng(b,2,-0.2, 0.5, 0.0);
  LegAng(b,3,-0.2, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 400;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0,-0.5,-0.1, 0.0);
  LegAng(b,1,-0.5,-0.1, 0.0);
  LegAng(b,2,-0.2, 0.5, 0.0);
  LegAng(b,3,-0.2, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.5, 1.0, 1.5);
  LegAng(b,1, 1.5, 1.0, 1.5);
  LegAng(b,2,-2.0, 0.5, 0.0);
  LegAng(b,3,-2.0, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.0, 0.0, 0.5);
  LegAng(b,1, 1.0, 0.0, 0.5);
  LegAng(b,2,-2.0, 0.0, 2.5);
  LegAng(b,3,-2.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.0, 0.0, 0.5);
  LegAng(b,1, 1.0, 0.0, 0.5);
  LegAng(b,2,-1.0, 0.0, 2.5);
  LegAng(b,3,-1.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  m[n].body = b;
  m[n].time = 0;
  n++;

  out = fopen("k_grab.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

   //==== Dive Kick ====//

  /*
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.2, 0.2, 0.0);
  LegAng(b,1, 0.2, 0.2, 0.0);
  LegAng(b,2,-0.2, 0.5, 0.2);
  LegAng(b,3,-0.2, 0.5, 0.2);
  m[n].body = b;
  m[n].time = 100;
  n++;

  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 0.2, 0.0);
  LegAng(b,1, 0.0, 0.2, 0.0);
  LegAng(b,2, 0.0, 0.5, 1.5);
  LegAng(b,3, 0.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0,-1.2, 0.5, 0.0);
  LegAng(b,1,-1.2, 0.5, 0.0);
  LegAng(b,2, 1.2, 0.5, 1.5);
  LegAng(b,3, 1.2, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 1.5);
  LegAng(b,1, 0.0, 1.5, 1.5);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  m[n].body = b;
  m[n].time = 500;
  n++;
  */

  mzero(b);

   n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  /*HeadAng(b, 0.5 , 1.5, 0.0);
  LegAng(b,0, .3, 0.4, 1.6);
  LegAng(b,1, .3, 0.4, 1.6);
  LegAng(b,2, -1.13, .14, 1.74);
  LegAng(b,3, -1.13, .14, 1.74);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,0.5 , 1.5, 0.0);
  LegAng(b,0, 1.03, 0.09, .9);
  LegAng(b,1, 1.03, 0.09, .9);
  LegAng(b,2, -1.17, .14, 1.74);
  LegAng(b,3, -1.17, .14, 1.74);
  m[n].body = b;
  m[n].time = 200*3;
  n++;

  HeadAng(b,0.5 , 1.5, 0.0);
  LegAng(b,0, 1.53, -0.11, .72);
  LegAng(b,1, 1.53, -0.11, .72);
  LegAng(b,2, -1.45, .54, 2.35);
  LegAng(b,3, -1.45, .54, 2.35);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,0.5 , 1.5, 0.0);
  LegAng(b,0, 0.2, 0.4, 1.5);
  LegAng(b,1, 0.2, 0.4, 1.5);
  LegAng(b,2, -1.13, .14, 1.74);
  LegAng(b,3, -1.13, .14, 1.74);
  m[n].body = b;
  m[n].time = 300;
  n++;*/

  /**********************grab done, dive now**********************/
  // BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.2, 0.15, 0.0);
  LegAng(b,1, 0.2, 0.15, 0.0);
  LegAng(b,2,-0.2, 0.5, 0.2);
  LegAng(b,3,-0.2, 0.5, 0.2);
  m[n].body = b;
  m[n].time = 100;
  n++;
  
  m[n].body = b;
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].time = 50;
  n++;
  
  //BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 0.15, 0.0);
  LegAng(b,1, 0.0, 0.15, 0.0);//.2
  LegAng(b,2, 0.0, 0.5, 1.5);
  LegAng(b,3, 0.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  //BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0,-1.2, 0.5, 0.0);
  LegAng(b,1,-1.2, 0.5, 0.0);
  LegAng(b,2, 1.2, 0.5, 1.5);
  LegAng(b,3, 1.2, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 200;//500
  n++;

  m[n].body = b;
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].time = 100;//500
  n++;

  //BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 300;//500
  n++;

  //BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 1.5);
  LegAng(b,1, 0.0, 1.5, 1.5);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 100;//200
  n++;

  m[n].body = b;
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].time = 500;
  n++;

  out = fopen("k_dive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Dive Kick Safe (RoboCup2003) ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.2, 0.15, 0.0);
  LegAng(b,1, 0.2, 0.15, 0.0);
  LegAng(b,2,-0.2, 0.5, 0.2);
  LegAng(b,3,-0.2, 0.5, 0.2);
  m[n].body = b;
  m[n].time = 100;
  n++;
  
  m[n].body = b;
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].time = 50;
  n++;
  
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 0.15, 0.0);
  LegAng(b,1, 0.0, 0.15, 0.0);
  LegAng(b,2, 0.0, 0.5, 1.5);
  LegAng(b,3, 0.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0,-1.0, 1.0, 0.3);
  LegAng(b,1,-1.0, 1.0, 0.3);
  LegAng(b,2, 1.2, 0.5, 1.5);
  LegAng(b,3, 1.2, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  m[n].body = b;
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].time = 100;
  n++;

  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 1.5);
  LegAng(b,1, 0.0, 1.5, 1.5);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  m[n].body = b;
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].time = 500;
  n++;

  out = fopen("k_rdive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Foreward Kick)====//
  /*  
      mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b,-0.0, 0.0, 0.0);
  LegAng(b,0, 1.3,-0.2,0.5);
  LegAng(b,1, 1.3,-0.2,0.5);
  LegAng(b,2, 1.3, 1.0,0.5);
  LegAng(b,3, 1.3, 1.0,0.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.3, 0.2,0.5);
  LegAng(b,1, 1.3, 0.2,0.5);
  LegAng(b,2, 1.3, 0.5,0.5);
  LegAng(b,3, 1.3, 0.5,0.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegAng(b,0, 2.2, 0.2,1.6);
  LegAng(b,1, 2.2, 0.2,1.6);
  LegAng(b,2, 1.3, 0.5,0.5);
  LegAng(b,3, 1.3, 0.5,0.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegAng(b,0, 2.2,-0.2,1.6);
  LegAng(b,1, 2.2,-0.2,1.6);
  LegAng(b,2, 1.3, 0.5,0.5);
  LegAng(b,3, 1.3, 0.5,0.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.0,-0.2,1.3);
  LegAng(b,1, 1.0,-0.2,1.3);
  LegAng(b,2, 1.3, 0.5,0.5);
  LegAng(b,3, 1.3, 0.5,0.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  m[n].body = b;
  m[n].time = 0;
  n++;
*/

 mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b,-0.3, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 1.0);
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegPos(b,2, -2.0,0.5, 0);
  LegPos(b,3, -2.0,0.5,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,-0.5, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 0.5);
  LegAng(b,1, 1.0,-0.2, 0.5);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b,0, 1.0, 0.1, 0.5);
  LegAng(b,1, 1.0, 0.1, 0.5);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 2.1, 0.1, 1.6);
  LegAng(b,1, 2.1, 0.1, 1.6);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 300;//400
  n++;

  // Bring front legs inward
  HeadAng(b, 0.2, 0.0, 0.0);
  LegAng(b,0, 2.1,-0.3, 1.6);
  LegAng(b,1, 2.1,-0.3, 1.6);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  // Swing forarms downward
  LegAng(b,0, 1.5,-0.3, 1.6);//1.0
  LegAng(b,1, 1.5,-0.3, 1.6);
  LegAng(b,2,-1.0, 0.5, 1.5);
  LegAng(b,3,-1.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 100;//300
  n++;

  LegAng(b,0, 1.0,-0.3, 0.0);//1.0
  LegAng(b,1, 1.0,-0.3, 0.0);
  LegAng(b,2,-1.0, 0.5, 1.5);
  LegAng(b,3,-1.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 150;//300
  n++;

  //HeadAng(b,-0.0, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 1.0);//1.5
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegAng(b,2,-1.0,-0.1, 1.5);
  LegAng(b,3,-1.0,-0.1, 1.5);
  m[n].body = b;
  m[n].time =250;//300
  n++;

  out = fopen("k_fwd.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Split Foreward Kick====//
 
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b,-0.3, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 1.0);
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegPos(b,2, -100, 75, 0);
  LegPos(b,3, -100,-75,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

   HeadAng(b,-0.5, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 0.5);
  LegAng(b,1, 1.0,-0.2, 0.5);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b,0, 1.0, 0.1, 0.5);
  LegAng(b,1, 1.0, 0.1, 0.5);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_fwd1.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 2.0, 0.1, 1.6);
  LegAng(b,1, 2.0, 0.1, 1.6);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 300;//400
  n++;

   HeadAng(b,-0.0, 0.0, 0.0);
  LegAng(b,0, 2.0,-0.3, 1.6);
  LegAng(b,1, 2.0,-0.3, 1.6);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 10;//100
  n++;


  LegAng(b,0, 1.0,-0.2, 1.0);
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegAng(b,2,-1.0, 0.5, 1.5);
  LegAng(b,3,-1.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 250;//300
  n++;

  LegAng(b,0, 1.0,-0.2, 1.5);
  LegAng(b,1, 1.0,-0.2, 1.5);
  LegAng(b,2,-1.0,-0.1, 1.5);
  LegAng(b,3,-1.0,-0.1, 1.5);
  m[n].body = b;
  m[n].time =250;//300
  n++;

  out = fopen("k_fwd2.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Blocking Kick ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegPos(b,0, 123, 85,0);
  LegPos(b,1, 123,-85,0);
  LegPos(b,2, -80, 75,0);
  LegPos(b,3, -80,-75,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegAng(b,0, 0.5, 1.0, 0.5);
  LegAng(b,1, 0.5, 1.0, 0.5);
  LegAng(b,2,-2.0, 0.0, 2.5);
  LegAng(b,3,-2.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  HeadAng(b,-0.5, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 0.5);
  LegAng(b,1, 1.0,-0.2, 0.5);
  LegAng(b,2, 1.0, 0.0, 2.5);
  LegAng(b,3, 1.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 600;
  n++;

  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 1.0, 0.1, 0.5);
  LegAng(b,1, 1.0, 0.1, 0.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b,0, 2.0, 0.1, 1.6);
  LegAng(b,1, 2.0, 0.1, 1.6);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b,-0.0, 0.0, 0.0);
  LegAng(b,0, 2.0,-0.2, 1.6);
  LegAng(b,1, 2.0,-0.2, 1.6);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 2.0,-0.2, 1.6);
  LegAng(b,1, 2.0,-0.2, 1.6);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 1.0,-0.2, 1.0);
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegAng(b,2, 0.0, 0.0, 1.5);
  LegAng(b,3, 0.0, 0.0, 1.5);
  m[n].body = b;
  m[n].time = 400;
  n++;

  LegAng(b,0, 1.0,-0.2, 1.5);
  LegAng(b,1, 1.0,-0.2, 1.5);
  LegAng(b,2,-1.0, 0.0, 1.5);
  LegAng(b,3,-1.0, 0.0, 1.5);
  m[n].body = b;
  m[n].time = 400;
  n++;

  /*
  out = fopen("k_grab.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
  */

  //==== Bump Kick ====//
  mzero(b);

  t = 1;

  n = 0;
  m[n].body = b;
  m[n].time = 200*t;
  n++;

  f = -10;
  BodyPos(b,98,RAD(16));
  HeadAng(b,0.0, 0.0, 0.0);
  LegPos(b,0, 125-f, 82,0);
  LegPos(b,1, 125-f,-82,0);
  LegPos(b,2,-100-f, 75,0);
  LegPos(b,3,-100-f,-75,0);
  m[n].body = b;
  m[n].time = 100*t;
  n++;

  BodyPos(b,90,RAD(16));
  HeadAng(b,0.0, 0.0, 0.0);
  LegPos(b,0, 125-f+30, 82,0);
  LegPos(b,1, 125-f+30,-82,0);
  LegPos(b,2,-100-f   , 75,0);
  LegPos(b,3,-100-f   ,-75,0);
  m[n].body = b;
  m[n].time = 100*t;
  n++;

  f = 80;
  BodyPos(b,90,RAD(0));
  HeadAng(b,0.0, 0.0, 0.0);
  LegPos(b,0, 125-f+30, 82,0);
  LegPos(b,1, 125-f+30,-82,0);
  LegPos(b,2,-100-f   , 75,0);
  LegPos(b,3,-100-f   ,-75,0);
  m[n].body = b;
  m[n].time = 100*t;
  n++;

  m[n].body = b;
  m[n].time = 100*t;
  n++;

  m[n].body = b;
  m[n].time = 0; // ignored
  n++;

  out = fopen("k_bump.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Punch Kick ====//
  mzero(b);
  BodyPos(b,104,RAD(16));

  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  f =  0;
  s = 25;

  LegPos(b,0, 124+f, 85+s,0);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80+f, 75+s,0);
  LegPos(b,3, -80+f,-75+s,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b,0, 0.0, 0.0, 2.5);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80-f, 75+s,0);
  LegPos(b,3, -80-f,-75+s,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b,0, 2.0, 0.0, 1.0);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80-f, 75+s,0);
  LegPos(b,3, -80-f,-75+s,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b,0, 2.0, 0.0, 1.0);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80-f, 75+s,0);
  LegPos(b,3, -80-f,-75+s,0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 1.0, 0.0, 1.0);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80-f, 75+s,0);
  LegPos(b,3, -80-f,-75+s,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegPos(b,0, 124, 85,0);
  LegPos(b,1, 124,-85,0);
  LegPos(b,2, -80, 75,0);
  LegPos(b,3, -80,-75,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  out = fopen("k_punch.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Side Head Kick ====//

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  f = -38;
  s =  32;
  o =  15;

  HeadAng(b, 0.0, 0.0, 0.0);
  BodyPos(b,104,RAD(16));
  MouthAng(b,-.7);
  LegPos(b,0, 123-o, 85,0);
  LegPos(b,1, 123+o,-85,0);
  LegPos(b,2, -80  , 75,0);
  LegPos(b,3, -80  ,-75,0);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b,-1.2,-1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85+s,0);
  LegPos(b,1, 123+o+f,-85+s,0);
  LegPos(b,2, -80  +f, 75+s,0);
  LegPos(b,3, -80  +f,-75+s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,-1.2, 1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85-s,0);
  LegPos(b,1, 123+o+f,-85-s,0);
  LegPos(b,2, -80  +f, 75-s,0);
  LegPos(b,3, -80  +f,-75-s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_head.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Soft Side Head Kick ====//

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  f = -38;
  s =  32;
  o =  15;

  HeadAng(b, 0.0, 0.0, 0.0);
  BodyPos(b,104,RAD(16));
  MouthAng(b,-.7);
  LegPos(b,0, 123-o, 85,0);
  LegPos(b,1, 123+o,-85,0);
  LegPos(b,2, -80  , 75,0);
  LegPos(b,3, -80  ,-75,0);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b,-1.2,-1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85+s,0);
  LegPos(b,1, 123+o+f,-85+s,0);
  LegPos(b,2, -80  +f, 75+s,0);
  LegPos(b,3, -80  +f,-75+s,0);
  m[n].body = b;
  m[n].time = 400;
  n++;

  HeadAng(b,-1.2, 1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85-s,0);
  LegPos(b,1, 123+o+f,-85-s,0);
  LegPos(b,2, -80  +f, 75-s,0);
  LegPos(b,3, -80  +f,-75-s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_heads.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Diagonal Head Kick ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  f = -38;
  s =  32;
  o =  15;

  HeadAng(b, 0.0, 0.0, 0.0);
  BodyPos(b,104,RAD(16));
  LegPos(b,0, 123-o, 85,0);
  LegPos(b,1, 123+o,-85,0);
  LegPos(b,2, -80  , 75,0);
  LegPos(b,3, -80  ,-75,0);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b,-1.2,-1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85+s,0);
  LegPos(b,1, 123+o+f,-85+s,0);
  LegPos(b,2, -80  +f, 75+s,0);
  LegPos(b,3, -80  +f,-75+s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,-1.2, 1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegAng(b,0, 0.0, 0.5, 0.0);
  LegPos(b,1, 123+o+f,-85-s,0);
  LegPos(b,2, -80  +f, 75-s,0);
  LegPos(b,3, -80  +f,-75-s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_diag.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Swingging Arm Kirk  ====//
  // This is the first version, and it is a slower motion
  // Kicks with the right arm. Inspired by UPENN's kick.

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 1200;
  n++;

  //Extend left arm and legs outward
  HeadAng(b, 0.093362, 0.029716, -0.022663);
  LegAng(b, 0, -0.151333, 0.713020, 0.352468);
  LegAng(b, 1, -0.227270, 0.204719, 1.239733);
  LegAng(b, 2, -0.287274, 0.327517, 0.488304);
  LegAng(b, 3, -0.852987, 0.921325, 0.968888);
  m[n].body = b;
  m[n].time = 3300;	//Hold for some time
  n++;
  m[n].body = b;
  m[n].time = 0;
  n++;

  //Swing body around
  LegAng(b, 0, -0.184017, -0.191986, 2.365634);
  LegAng(b, 1, 0.019777, 1.289728, 0.977025);
  LegAng(b, 2, 0.145040, 1.528646, 0.092881);
  LegAng(b, 3, 0.122486, 0.136805, 1.535826);
  m[n].body = b;
  m[n].time = 100;
  n++;
  m[n].body = b;
  m[n].time = 0;
  n++;

  //Straighten out right arm for kick
  LegAng(b, 1, 1.536782, -0.005817, 0.188472);
  m[n].body = b;
  m[n].time = 600;
  n++;
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_swing.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Swingging Arm Kirk  (Revisited) ====//
  // This is the second  version, faster than the first.

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 400;
  n++;

  //Extend left arm and legs outward
  HeadAng(b, 0.093362, 0.029716, -0.022663);
  LegAng(b, 0, -0.151333, 0.713020, 0.352468);
  LegAng(b, 1, -0.227270, 0.204719, 1.239733);
  LegAng(b, 2, -0.287274, 0.327517, 0.488304);
  LegAng(b, 3, -0.852987, 0.921325, 0.968888);
  m[n].body = b;
  m[n].time = 200;	
  n++;
  m[n].body = b;
  m[n].time = 0;
  n++;

  //Swing body around
  LegAng(b, 0, -0.184017, -0.191986, 2.365634);
  LegAng(b, 1, 0.019777, 1.289728, 0.977025);
  LegAng(b, 2, 0.145040, 1.528646, 0.092881);
  LegAng(b, 3, 0.122486, 0.136805, 1.535826);
  m[n].body = b;
  m[n].time = 100;
  n++;
  m[n].body = b;
  m[n].time = 0;
  n++;

  //Straighten out right arm for kick
  LegAng(b, 1, 1.536782, -0.005817, 0.188472);
  m[n].body = b;
  m[n].time = 600;
  n++;
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_swing1.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== BW Ball Arm Kirk with Grab ====//
  // This is the swingging arm kick, specialized
  // for the black and white ball challenge, it
  // grabs the ball before shooting for more
  // power and better accuracy.

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 400;
  n++;

  //Initial grabbing position
  HeadAng(b, -1.5, 0.0, 0.0);
  LegAng(b, 0, -0.1, -0.1, 1.5);
  LegAng(b, 1, -0.1, -0.1, 1.5);
  LegPos(b, 2, -100, 75,0);
  LegPos(b, 3, -100,-75,0);
  m[n].body = b;
  m[n].time = 600;
  n++;
  m[n].body = b;
  m[n].time = 400;
  n++;

  //Extend left arm and legs outward
  HeadAng(b, 0.093362, 0.029716, -0.022663);
  LegAng(b, 0, -0.151333, 0.713020, 0.352468);
  LegAng(b, 1, -0.227270, 0.204719, 1.239733);
  LegAng(b, 2, -0.287274, 0.327517, 0.488304);
  LegAng(b, 3, -0.852987, 0.921325, 0.968888);
  m[n].body = b;
  m[n].time = 100;
  n++;
  m[n].body = b;
  m[n].time = 0;
  n++;

  //Swing body around
  LegAng(b, 0, -0.184017, -0.191986, 2.365634);
  LegAng(b, 1, 0.019777, 1.289728, 0.977025);
  LegAng(b, 2, 0.145040, 1.528646, 0.092881);
  LegAng(b, 3, 0.122486, 0.136805, 1.535826);
  m[n].body = b;
  m[n].time = 100;
  n++;
  m[n].body = b;
  m[n].time = 0;
  n++;

  //Straighten out right arm for kick
  LegAng(b, 1, 1.536782, -0.005817, 0.188472);
  m[n].body = b;
  m[n].time = 600;
  n++;
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_bw.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //=== Back Up With BW Ball ===//
  // This motion is for the black and white
  // ball challenge. It grabs onto the ball
  // and backs up with it.

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 400;
  n++;

  //Grab the ball and get in initial position
  HeadAng(b, -0.8, 0.0, 0.0);
  LegAng(b, 0, 0.392698, -0.39626, 1.487235);
  LegAng(b, 1, 0.392698, -0.39626, 1.487235);
  LegAng(b, 2, 0.2, 0.13, 0.0);
  LegAng(b, 3, -0.7, 0.13, 1.1);
  m[n].body = b;
  m[n].time = 200;
  n++;
  m[n].body = b;
  m[n].time = 200;
  n++;

  //Do the steps to walk backwards
  for(int i=0; i<8; i++)
    {
      LegAng(b, 2, -0.4, 0.13, -0.2);
      LegAng(b, 3, -0.3, 0.13, 1.5);
      m[n].body = b;
      m[n].time = 200;
      n++;
      m[n].body = b;
      m[n].time = 0;
      n++;

      LegAng(b, 2, -0.7, 0.13, 1.1);
      LegAng(b, 3, 0.2, 0.13, 0.0);
      m[n].body = b;
      m[n].time = 200;
      n++;
      m[n].body = b;
      m[n].time = 0;
      n++;

      LegAng(b, 2, -0.3, 0.13, 1.5);
      LegAng(b, 3, -0.4, 0.13, -0.2);
      m[n].body = b;
      m[n].time = 200;
      n++;
      m[n].body = b;
      m[n].time = 0;
      n++;

      LegAng(b, 2, 0.2, 0.13, 0.0);
      LegAng(b, 3, -0.7, 0.13, 1.1);
      m[n].body = b;
      m[n].time = 200;
      n++;
      m[n].body = b;
      m[n].time = 0;
      n++;
    }

  m[n].body = b;
  m[n].time = 400;
  n++;

  out = fopen("bw_back.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
}