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


void make_walk_param()
{
  Motion::WalkParam wp;
  FILE *out;
  mzero(wp);

  //dribble walk
  wp.leg[0].neutral.set( 125, 82,0);
  wp.leg[1].neutral.set( 125,-82,0);
  wp.leg[2].neutral.set( -100, 75,0);
  wp.leg[3].neutral.set( -100,-75,0);
  wp.period = 640;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 150);  
  wp.leg[3].lift_vel.set(0,0, 150); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
    
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.max_vel.max_dx = 240;
  wp.max_vel.max_dy = 210;
  wp.max_vel.max_da = 2.2;

  wp.body_angle  = RAD(16.0);
  wp.body_height = 98;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 9.0;
  wp.back_height = 9.0;


  out = fopen("walk_dxy.prm","wb");
  if(out){
    fwrite(&wp,sizeof(wp),1,out);
    fclose(out);
  }
  
  /*********************************/
  //walk for turning, max va = 2.2
  wp.leg[0].neutral.set( 126, 85,0);
  wp.leg[1].neutral.set( 126,-85,0);
  wp.leg[2].neutral.set( -100, 78,0);
  wp.leg[3].neutral.set( -100,-78,0);
  wp.period = 550;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 150);  
  wp.leg[3].lift_vel.set(0,0, 150); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
    
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.max_vel.max_dx = 240;
  wp.max_vel.max_dy = 210;
  wp.max_vel.max_da = 2.2;

  wp.body_angle  = RAD(14.0);
  wp.body_height = 101;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0; 

  out = fopen("walk_a.prm","wb");
  if(out){
    fwrite(&wp,sizeof(wp),1,out);
    fclose(out);
  }

  /*************************/
  //the learned walk for the ERS7
  wp.leg[0].neutral.set( 114, 78, 0);
  wp.leg[1].neutral.set( 114, -78, 0);
  wp.leg[2].neutral.set( -111, 73, 0);
  wp.leg[3].neutral.set( -111, -73, 0);
  wp.period = 550; 

  wp.leg[0].lift_vel.set(0,0, 200);
  wp.leg[1].lift_vel.set(0,0, 200);
  wp.leg[2].lift_vel.set(0,0, 200);
  wp.leg[3].lift_vel.set(0,0, 200);

  wp.leg[0].down_vel.set(0,0,-200);
  wp.leg[1].down_vel.set(0,0,-200);
  wp.leg[2].down_vel.set(0,0,-200);
  wp.leg[3].down_vel.set(0,0,-200);

  wp.leg[0].lift_time=0.000000; wp.leg[0].down_time=0.500000;
  wp.leg[1].lift_time=0.500000; wp.leg[1].down_time=1.000000;
  wp.leg[2].lift_time=0.500000; wp.leg[2].down_time=1.000000;
  wp.leg[3].lift_time=0.000000; wp.leg[3].down_time=0.500000;
  
  wp.max_vel.max_dx = 340;
  wp.max_vel.max_dy = 210;
  wp.max_vel.max_da = 2.2;

  wp.body_angle  = 0.122111;
  wp.body_height = 104;
  wp.hop  = 1;
  wp.sway = 0;
  wp.front_height = 40.0;
  wp.back_height = 40.0;
  
  out = fopen("walk_xyf.prm","wb");
  if(out){
    fwrite(&wp,sizeof(wp),1,out);
    fclose(out);
  }

  /*************************/
  // smooth walk for the ERS7
  wp.leg[0].neutral.set( 120, 68, 0);
  wp.leg[1].neutral.set( 120,-68, 0);
  wp.leg[2].neutral.set(-100, 70, 0);
  wp.leg[3].neutral.set(-100,-70, 0);
  wp.period = 640; 

  wp.leg[0].lift_vel.set(0,0,  70);
  wp.leg[1].lift_vel.set(0,0,  70);
  wp.leg[2].lift_vel.set(0,0, 100);
  wp.leg[3].lift_vel.set(0,0, 100);

  wp.leg[0].down_vel.set(0,0,-100);
  wp.leg[1].down_vel.set(0,0,-100);
  wp.leg[2].down_vel.set(0,0,-150);
  wp.leg[3].down_vel.set(0,0,-150);

  wp.leg[0].lift_time=0.050000; wp.leg[0].down_time=0.500000;
  wp.leg[1].lift_time=0.550000; wp.leg[1].down_time=1.000000;
  wp.leg[2].lift_time=0.500000; wp.leg[2].down_time=1.000000;
  wp.leg[3].lift_time=0.000000; wp.leg[3].down_time=0.500000;

  wp.max_vel.max_dx = 240;
  wp.max_vel.max_dy = 210;
  wp.max_vel.max_da = 1.9;

  wp.body_angle  = RAD(8);
  wp.body_height = 107;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 40.0;
  wp.back_height = 40.0;
  
  out = fopen("walk_xy.prm","wb");
  if(out){
    fwrite(&wp,sizeof(wp),1,out);
    fclose(out);
  }

  /*************************/
  // smooth walk turn
  wp.leg[0].neutral.set( 120, 90, 0);
  wp.leg[1].neutral.set( 120,-90, 0);
  wp.leg[2].neutral.set(-100, 80, 0);
  wp.leg[3].neutral.set(-100,-80, 0);
  wp.period = 440;

  wp.leg[0].lift_vel.set(0,0, 150);
  wp.leg[1].lift_vel.set(0,0, 150);
  wp.leg[2].lift_vel.set(0,0, 150);
  wp.leg[3].lift_vel.set(0,0, 150);

  wp.leg[0].down_vel.set(0,0,-150);
  wp.leg[1].down_vel.set(0,0,-150);
  wp.leg[2].down_vel.set(0,0,-150);
  wp.leg[3].down_vel.set(0,0,-150);

  wp.leg[0].lift_time=0.00; wp.leg[0].down_time=0.50;
  wp.leg[1].lift_time=0.50; wp.leg[1].down_time=1.00;
  wp.leg[2].lift_time=0.50; wp.leg[2].down_time=1.00;
  wp.leg[3].lift_time=0.00; wp.leg[3].down_time=0.50;

  wp.max_vel.max_dx = 200;
  wp.max_vel.max_dy = 200;
  wp.max_vel.max_da = 2.3;

  wp.body_angle  = RAD(8);
  wp.body_height = 107+2; // +5;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 15.0;
  wp.back_height = 15.0;

  out = fopen("walk_a.prm","wb");
  if(out){
    fwrite(&wp,sizeof(wp),1,out);
    fclose(out);
  }

  /*********************************/
  //Turn with ball (left) parameters
  //
  //Tested with following head commands:
  //command->head_cmd = HEAD_ANGLES;
  //command->head_tilt = -1.1;
  //command->head_pan  = 0.0;
  //command->head_tilt2 = 1.1;

  wp.leg[0].neutral.set( 120, 90,0);
  wp.leg[1].neutral.set( 150,-80,0);
  wp.leg[2].neutral.set( -100, 80,0);
  wp.leg[3].neutral.set( -100,-80,0);
  wp.period = 440;

  wp.leg[0].lift_vel.set(0,0, 150); 
  wp.leg[1].lift_vel.set(0,0, 150); 
  wp.leg[2].lift_vel.set(0,0, 150);  
  wp.leg[3].lift_vel.set(0,0, 150); 
  
  wp.leg[0].down_vel.set(0,0,-150); 
  wp.leg[1].down_vel.set(0,0,-150); 
  wp.leg[2].down_vel.set(0,0,-150); 
  wp.leg[3].down_vel.set(0,0,-150); 
    
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.max_vel.max_dx = 200;
  wp.max_vel.max_dy = 200;
  wp.max_vel.max_da = 2.3;

  wp.body_angle  = RAD(8);
  wp.body_height = 107-2;
  // wp.body_angle  = RAD(14.0);
  // wp.body_height = 101;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 15.0;
  wp.back_height = 15.0; 

  out = fopen("walk_da.prm","wb");
  if(out){
    fwrite(&wp,sizeof(wp),1,out);
    fclose(out);
  }
}

//this function exhaustively tests the motion paramaters using 
//different velocity values for vx,vy and va.  It is NOT ON BY
//DEFAULT since it takes so long to run.  The function that tests
//for kinematic errors for a single velocity vector lives in 
//genmot.cc
void test_walks()
{
  Motion::Motion m;
  Motion::MotionCommand cmd;
  Kinematics k;
  double angles[NumPIDJoints];
  double da;
  int dx,dy;
  int i,e;

  mset(angles,0.0,NumPIDJoints);
  mzero(cmd);

  m.init();
  m.init(angles);

  for(da=0.0; da<1.9; da+=1.9/10){
    //printf("Table: da=%f\n",da);
    for(dx=-230; dx<=230; dx+=20){
      //printf("  ");
      for(dy=-230; dy<=230; dy+=10){
        cmd.motion_cmd = Motion::MOTION_WALK_TROT;
        cmd.vx = dx;
        cmd.vy = dy;
        cmd.va = da;
        m.setCommand(cmd);

        for(i=0; i<600/8*4; i++) m.getAngles(angles);
        k.clearErrors();
        for(i=0; i<600/8*4; i++) m.getAngles(angles);
        e = k.getTotalErrors();
        //printf("%c",e?'.':'#'); fflush(stdout);
      }
      //printf("\n");
    }
  }
}