/* 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"

/*
  f 120 68 0
  b 100 70 0
  bh 93
  ba 8 deg
*/

void MakeForwardKicks()
{
  MotionMaker m;

  //==== ERS7 Forward Dive Kick w/ Starting Tilt ====//
  m.start("k_diveE7.mot");
  m.trans(300);

  // Rear up
  int fwd = 0;
  m.bodyPos(107,RAD(-15));
  m.headAng( 0.0, 0.0, 0.75); // -0.25);
  m.legPos(0, 120+fwd, 68,0);
  m.legPos(1, 120+fwd,-68,0);
  m.legPos(2,-100+fwd, 70,0);
  m.legPos(3,-100+fwd,-70,0);

  m.trans(100);
  m.hold(100);

  // Flop over by swaying legs back
  m.legAng(0, 1.0, 0.3, 2.0);
  m.legAng(1, 1.0, 0.3, 2.0);
  m.legAng(2, 1.5, 0.1, 1.2);
  m.legAng(3, 1.5, 0.1, 1.2);

  m.hold(200);
  m.trans(200);

  // Curl all four legs
  m.headAng( 0.5, 1.5, 0.0);
  m.legAng(0,-0.0, 0.2, 2.0);
  m.legAng(1,-0.0, 0.2, 2.0);
  m.legAng(2,-1.0, 0.2, 2.0);
  m.legAng(3,-1.0, 0.2, 2.0);

  m.hold(200);
  m.end();

  //==== ERS7 Forward Dive Kick w/ Starting Tilt ====//
  m.start("k_diveE7b.mot");
  m.trans(250);

  // Rear up
  fwd = 0;
  // m.bodyPos(107,RAD(-15));
  m.bodyPos(107+15,RAD(-20));
  m.headAng( 0.0, 0.0, 0.75); // -0.25);
  m.legPos(0, 114-fwd, 68,0);
  m.legPos(1, 114-fwd,-68,0);
  m.legPos(2,-100-fwd, 70,0);
  m.legPos(3,-100-fwd,-70,0);

  m.trans(100);
  fwd = 30;
  m.legPos(0, 114-fwd, 68,0);
  m.legPos(1, 114-fwd,-68,0);
  m.legPos(2,-100-fwd, 70,0);
  m.legPos(3,-100-fwd,-70,0);

  m.trans(150);

  m.bodyPos(107,RAD(8));
  m.legPos(0, 114-fwd, 68,0);
  m.legPos(1, 114-fwd,-68,0);
  m.legPos(2,-100-fwd, 70,0);
  m.legPos(3,-100-fwd,-70,0);

  /*
  // Flop over by swaying legs back
  m.legAng(0, 1.0, 0.3, 2.0);
  m.legAng(1, 1.0, 0.3, 2.0);
  m.legAng(2, 1.5, 0.1, 1.2);
  m.legAng(3, 1.5, 0.1, 1.2);

  m.hold(200);
  m.trans(200);

  // Curl all four legs
  // m.headAng( 0.5, 1.5, 0.0);
  m.legAng(0,-0.0, 0.2, 2.0);
  m.legAng(1,-0.0, 0.2, 2.0);
  m.legAng(2,-1.0, 0.2, 2.0);
  m.legAng(3,-1.0, 0.2, 2.0);
  */

  m.hold(200);
  m.end();

  //==== Forward Kick ====//
  //Made during RoboCup 2004
  m.start("k_fwdE7.mot");
  m.trans(200);

  m.legAng(0, -0.034720, 0.365163, 0.251300);
  m.legAng(1, -0.334720, -0.165163, 1.051300);
  m.legAng(2, -0.820859, 0.159467, 1.816568);
  m.legAng(3, -0.820859, 0.159467, 1.816568);
  m.headAng( -0.781417, -0.858032,-0.242211);

  m.hold(200);
  m.trans(200);

  // Flop over by swaying legs back
  m.legAng(0, 1.0, 0.0, 1.0);
  m.legAng(1,-1.5, 0.0, 2.0);
  m.legAng(2, 1.5, 0.1, 1.2);
  m.legAng(3, 1.5, 0.1, 1.2);
  m.headAng( -0.781417, -0.858032,-0.242211);

  m.hold(300);
  m.trans(200);

  // Curl all four legs
  m.headAng( 0.5, 1.5, 0.0);
  m.legAng(0,-0.0, 0.2, 2.0);
  m.legAng(1,-0.0, 0.2, 2.0);
  m.legAng(2,-1.0, 0.2, 2.0);
  m.legAng(3,-1.0, 0.2, 2.0);

  m.hold(200);
  m.end();

  //==== Arm Dive ====//
  m.start("k_adiveE7.mot");
  m.trans(300);

  // Rear up
  fwd = 0;
  m.bodyPos(107,RAD(-15));
  m.headAng( 0.0, 0.0, -0.25);
  m.legPos(0, 120+fwd, 68,0);
  m.legPos(1, 120+fwd,-68,0);
  m.legPos(2,-100+fwd, 70,0);
  m.legPos(3,-100+fwd,-70,0);

  m.trans(150);

  // Flop over by swaying legs back
  m.legAng(0, 1.0, 0.0, 1.0);
  m.legAng(1,-1.5, 0.0, 2.0);
  m.legAng(2, 1.5, 0.1, 1.2);
  m.legAng(3, 1.5, 0.1, 1.2);

  m.hold(200);
  m.trans(200);

  // Curl all four legs
  m.headAng( 0.5, 1.5, 0.0);
  m.legAng(0,-0.0, 0.2, 2.0);
  m.legAng(1,-0.0, 0.2, 2.0);
  m.legAng(2,-1.0, 0.2, 2.0);
  m.legAng(3,-1.0, 0.2, 2.0);

  m.hold(200);
  m.end();

  //==== ERS7 Tilting Bump ====//
  m.start("k_bumpE7.mot",1.0);
  m.trans(200);

  // Lean back
  fwd = 0;
  m.bodyPos(90,RAD(-35));
  m.headAng( 0.0, 0.0, -0.25);
  m.legPos(0, 120+fwd, 68,0);
  m.legPos(1, 120+fwd,-68,0);
  m.legPos(2,-100+fwd, 70+30,0);
  m.legPos(3,-100+fwd,-70-30,0);

  m.trans(150);

  // move forward
  fwd = -50;
  m.legPos(0, 120+fwd, 68,0);
  m.legPos(1, 120+fwd,-68,0);
  m.legPos(2,-100+fwd, 70+30,0);
  m.legPos(3,-100+fwd,-70-30,0);

  m.trans(100);

  // drop down on ball
  m.bodyPos(90,RAD(0));

  m.hold(200);
  m.end();

  //==== ERS7 Slap Backward (135 deg) ====//
  m.start("k_sbackE7.mot",1.0);
  m.trans(300);

  //RoboCup 2004 version
  // stand neutral
  m.bodyPos(107,0.25);
  m.headAng(-1.4, 0.0, 0.8);
  m.legPos(0, 120+ 0, 68,0);
  m.legPos(1, 120+ 0,-68,0);
  m.legPos(2,-100+ 0, 70,0);
  m.legPos(3,-100+ 0,-70,0);

  m.trans(250);

  // grab ball
  m.headAng(-1.2, 0.0, 0.3);
  m.legAng(0, 0.5, 0.0, 1.0);
  m.legAng(1, 0.5, 0.0, 1.0);

  m.trans(100);

  m.legAng(0, 0.5,-0.2, 1.0);
  m.legAng(1, 0.5,-0.2, 1.0);

  m.trans(300);

  // lean over
  m.legAng(0, 0.5, 0.6, 1.0);
  m.legAng(1, 1.5, 0.0, 0.0);
  m.legAng(2, 0.0,-0.3, 0.0);
  m.legAng(3, 0.0, 1.5, 2.2);

  m.hold(200);
  m.trans(0);

  m.headAng(-1.5, 0.3, 0.0);
  m.legAng(0, 2.2, 0.0, 1.0);

  m.hold(200);
  m.trans(0);

  // smack leg down
  m.legAng(0,-1.3, 0.4,-0.3);
  
  m.hold(300);
  m.trans(300);

  m.headAng(0.0, 0.0, 0.0);
  m.legAng(0, 0.5, 0.2, 2.2);
  m.legAng(1, 0.5, 0.2, 2.2);
  m.legAng(2,-0.5, 0.2, 2.2);
  m.legAng(3,-0.5, 0.2, 2.2);

  m.end();



  // Jim's version for lab
//   // stand neutral
//   m.bodyPos(107,0.25);
//   m.headAng(-1.4, 0.0, 0.8);
//   m.legPos(0, 120+ 0, 68,0);
//   m.legPos(1, 120+ 0,-68,0);
//   m.legPos(2,-100+ 0, 70,0);
//   m.legPos(3,-100+ 0,-70,0);

//   m.trans(250);

//   // grab ball
//   m.headAng(-1.2, 0.0, 0.3);
//   m.legAng(0, 0.5, 0.0, 1.0);
//   m.legAng(1, 0.5, 0.0, 1.0);

//   m.trans(100);

//   m.legAng(0, 0.5,-0.2, 1.0);
//   m.legAng(1, 0.5,-0.2, 1.0);

//   m.trans(300);

//   // lean over
//   m.legAng(0, 0.5, 0.6, 1.0);
//   m.legAng(1, 1.5, 0.0, 0.0);
//   m.legAng(2, 0.0,-0.3, 0.0);
//   m.legAng(3, 0.0, 1.5, 2.2);

//   m.hold(200);
//   m.trans(100);

//   m.headAng(-1.5, 0.3, 0.3);
//   m.legAng(0, 2.2, 0.4, 1.0);

//   m.hold(100);
//   m.trans(0);

//   // smack leg down
//   m.legAng(0,-1.0, 0.4,-0.3);

//   m.hold(200);
//   m.trans(300);

//   m.headAng(0.0, 0.0, 0.0);
//   m.legAng(0, 0.5, 0.2, 2.2);
//   m.legAng(1, 0.5, 0.2, 2.2);
//   m.legAng(2,-0.5, 0.2, 2.2);
//   m.legAng(3,-0.5, 0.2, 2.2);

//   m.end();
}

void MakeHeadKicks()
{
  MotionMaker m;
  m.start("k_headE7.mot");

  int shift=35;

  m.trans(100);
  // Go to a "neutral" standing position
  m.mouthAng(-0.8);
  m.bodyPos(107,0.25);
  m.headAng(-0.8,-1.3, 0.1); // The windup
  m.legPos(0, 120, 68+shift,0);
  //  m.legPos(1, 120,-68+shift,0);
  m.legPos(1,  153, -98+shift, 30); // raise the front leg before shifting
  m.legPos(2,-100, 70+shift,0);
  m.legPos(3,-100,-100+shift,0); 
  m.trans(100);

  // Raise and bring the front right leg forward (to avoid sliding)
  //  m.legPos(1,  153, -98+shift, 30);
  //  m.trans(100);

  m.legPos(1,  153, -98+shift, 0);
  m.trans(50);

  // Raise and bring the front left leg backward (to avoid sliding)
  m.legPos(0,   73,  68+shift, 30);
  m.trans(50);

  m.legPos(0,   73,  68+shift, 0);
  m.trans(50);

  m.trans(0);
  m.headAng(-0.9, 0.5, 0.1); // Follow through... not as much

  m.trans(200); 
  // The kick
  m.legPos(0,   73,   68-shift, 0);
  m.legPos(1,  153,  -98-shift, 0);
  m.legPos(2, -100,   70-shift, 0);
  m.legPos(3, -100, -100-shift, 0); // 70
  // The follow-through
  m.hold(300);

  m.end();  

#if 0
  //=====================================================================
  m.start("k_headbuttE7.mot");

  m.trans(100);
  // Go to a "neutral" standing position
  m.bodyPos(107,0.25);

  // Change this to set the head position to the "ready" state
  m.trans(200);
  m.headAng(0.0, -0.4, 0.0);
  m.legPos(0,  120,  68 ,0);
  m.legPos(1,  120, -68 ,0);
  m.legPos(2, -100,  70 ,0);
  m.legPos(3, -100, -70 ,0); 

  m.trans(0);

  // Slam the head straight down

  m.headAng(-1.2, -0.4, 0.0);

  m.hold(300);

  m.end();  
#endif
}

void MakeSwingKicks()
{
  MotionMaker m;

  //==== New Faster Swing Kick ====//
  m.start("k_swingE7.mot");
  m.trans(200);

  //Initial twisting motion
  m.headAng(-0.410665, 0.052359, 0.006145);
  m.legAng(0, -0.263445, 0.307545, 1.305823);
  m.legAng(1, -0.541051, 0.472708, 1.030770);
  m.legAng(2, -0.466217, 0.467013, 0.829645);
  m.legAng(3, -0.466217, 0.467013, 0.829645);
  m.hold(100);
  m.trans(100);

  //Drop back leg to free up front leg for kick
  m.legAng(2, 0.000000, 1.218790, 2.166588);
  m.trans(200);

  //Swing around and kick
  m.legAng(0, -0.849392, -0.147973, 2.171394);
  m.legAng(1, 0.451588, 0.013240, 0.361517);
  m.legAng(2, 0.000000, 1.218790, 2.166588);
  m.legAng(3, 0.838441, -0.085369, 1.384109);
  m.hold(100);
  m.trans(200);

  //Stretch legs out for get-up
  m.headAng(0.5, 1.5, 0.0);
  m.legAng(0, 1.5, 0.0, 2.0);
  m.legAng(1, 1.5, 0.0, 2.0);
  m.legAng(2, 1.5, 0.0, 2.0);
  m.legAng(3, 1.5, 0.0, 2.0);
  m.trans(200);

  //Curl legs in towards body
  m.headAng(0.5, 1.5, 0.0);
  m.legAng(0, -0.0, 0.2, 2.0);
  m.legAng(1, -0.0, 0.2, 2.0);
  m.legAng(2, -1.0, 0.2, 2.0);
  m.legAng(3, -1.0, 0.2, 2.0);
  m.hold(200);
  m.trans(100);

  m.end();
}

void MakeKicksERS7()
{
  MakeForwardKicks();
  MakeHeadKicks();
  MakeSwingKicks();
}