/* 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 <stdlib.h>
#include <stdio.h>
#include <vector>

#include "../../headers/Util.h"
#include "../RobotConstants.h"
#include "../Kinematics.h"
#include "../Motion.h"
#include "../MotionInterface.h"

static const unsigned NumPIDJoints = 18;
static const int TimeStep = Motion::TimeStep;

void BodyPos(Motion::BodyState &b,double height,double angle);
void LegPos(Motion::BodyState &b,int leg,double x,double y,double z);
void LegAng(Motion::BodyState &b,int leg,double r,double s,double k);
void HeadPos(Motion::BodyState &b,double x,double y,double z);
void HeadAng(Motion::BodyState &b,double t,double p,double r);
void MouthAng(Motion::BodyState &b,double a);

// high level interface
class MotionMaker{
  std::vector<Motion::BodyStateMotion> states;
  Motion::BodyStateMotion bsm;
  const char *filename;
  double speed; // transition time multiplier
public:
  void start(const char *_filename,double _speed = 1.0);

  void bodyPos(double height,double angle);
  void legPos(int leg,double x,double y,double z);
  void legAng(int leg,double r,double s,double k);
  void headPos(double x,double y,double z);
  void headAng(double t,double p,double r);
  void mouthAng(double a);

  void hold(double ms) {trans(ms);}
  void trans(double ms);
  void end();
};

// walks
void make_walk_param();
void test_walks();

// kicks
void make_side_kicks();
void make_dive_kicks();
void make_kicks();
void make_kicks_de();
void make_kicks_unsw();
void make_blocks();

void MakeKicksERS7();

// misc motions
void make_getups();
void make_fun_motions();
void make_skateboard_motions();
void make_turn_with_ball();