/* 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.
  ========================================================================= */

#ifndef __MOTION_H__
#define __MOTION_H__

class MotionPlayer;

#include "../headers/Geometry.h"
#include "RobotConstants.h"
#include "Kinematics.h"
#include "Spline.h"
#include "Path.h"

#include "MotionInterface.h"
#include "WalkParam.h"
#include "Sound.h"

typedef SplinePath<vector3d,double> splinepath;
typedef HermiteSplineSegment<vector3d,double> spline;

template <class data>
void set3(data *arr,data v0,data v1,data v2)
{
  arr[0] = v0;
  arr[1] = v1;
  arr[2] = v2;
}

namespace Motion{
  
  /*
    Joint array:
    0-11 : Legs (LF,RF,LH,RH) (Rotator, Shoulder, Knee)
    12-14 : Head (Tilt,Pan,Roll)
    15-16 : Tail (Diag Bottom Right, Diag Upper Right)
    17    : Mouth
    18-19 : Ears (L,R)
  */

  extern Kinematics kin;
  extern double camera_vert_fov;
  extern double camera_horz_fov;
/*
class MotionStream{
  int length;
public:
  virtual bool LoadMotion(char *file);
  virtual bool Interpolate();
  virtual int length();
};
*/

// Whether legs and head are busy (i.e. something else can't use them yet)
// Also indicates whether the motion generator cannot get a new target itself
#define LEGS_BUSY   1
#define HEAD_BUSY   2
#define MOUTH_BUSY  4
#define TARGET_BUSY 8

const int TimeStep = 8; // 8 ms, can use another for slow motion testing

#define ATTR_ANGLES   1
#define ATTR_POSITION 2

struct LegState{
  long attr,reserved;
  point3d pos;
  double angles[3];
};

struct HeadState{
  long attr,reserved;
  vector3d target;
  double angles[3];
};

struct MouthState{
  long attr,reserved;
  double angle;
};

struct BodyState{
  BodyPosition pos;
  LegState leg[4];
  HeadState head;
  MouthState mouth;
};

struct BodyStateMotion{
  BodyState body;
  long time; // ms
  long reserved;
};

class MotPlayer{
  BodyStateMotion *frame;
  int num_frames;
  int current_frame;
  int base_time;
  bool flip;
  double speed;
public:
  MotPlayer() {frame=NULL;}
  ~MotPlayer() {close();}

  bool load(char *filename);
  void close();
  void play(BodyState &body,int time,double nspeed,bool flip_lr);
  int get(BodyState &body,int time);
};


class Stand{
  BodyState start,target;
  int time,length;

  BodyState neutral;
  BodyState left,right;
  BodyState crouch;
  BodyState skate_neutral;
  BodyState wall_turn_neutral;
public:
  Stand();
  void init();
  void setNeutral(const BodyState &b);
  void setTarget(const BodyState &current,const BodyState &ntarget,int ntime);
  void setTarget(const BodyState &body,int target,int ntime);

  int getAngles(const BodyState &current,BodyState &next);
  int areBusy();
  void getMotionUpdate(MotionLocalizationUpdate &update);
};

struct LegWalkState{
  spline airpath;
  int time_lift;
  bool air;
};

struct OdometryParam{
  static const int NumBases = 6;
  double dx_coeffs[NumBases];
  double dy_coeffs[NumBases];
  double da_coeffs[NumBases];
};

class Trot{
 public:
  WalkParam wp;
  WalkParam wp_xy,wp_xyf;
  WalkParam wp_a,wp_dxy,wp_da,wp_beh;

  OdometryParam odom_param;
  LegWalkState legw[4];
  splinepath body_loc,body_angle;
  BodyPosition wpos;

  vector3d pos_delta;
  double angle_delta;

  int time, type;
  double cycle;

  vector3d vel_xya;
  vector3d target_vel_xya;
 
  bool new_param;
private:
  void evalPosition(BodyPosition &bp,int time);
  vector3d projectPosition(BodyPosition &bp,vector3d pos);
public:
  Trot();
  void init(double dx,double dy,double da,int walk_type);
  void setWalkType(int walk_type) {type=walk_type;}
  void load();

  void setParams(WalkParam &params);
  void getNeutral(BodyState &s);
  void mirrorParam(WalkParam &wp);
  void chooseParam(double dx,double dy,double da);
  void setTarget(double dx,double dy,double da,
                 double accel_time,int bound_mode);
  vector3d &velocity() {return(vel_xya);}

  int getAngles(BodyState &current,BodyState &next);
  int areBusy();

  double getBasis(double dx,double dy,double da,int basis_idx);
  void applyMotionCorrection(double &dx,double &dy,double &da);
  void getMotionUpdate(MotionLocalizationUpdate &update);
};

class TurnWithBall{
  MotPlayer m_grab;
  MotPlayer m_step;

  enum States{GRAB,STEP};

  int type,time,busy,substate;
  bool generated_motion_update;
public:
  TurnWithBall();
  void load();
  void init(int turn_type);

  int getAngles(BodyState &current,BodyState &next);
  int areBusy();
  void getMotionUpdate(MotionLocalizationUpdate &update);
  void setNextMot(int turn_type);
};

class Getup{
  MotPlayer m_front;
  MotPlayer m_back;
  MotPlayer m_side;

  int type,time,busy;
public:
  Getup();
  void load();
  void init(int getup_type);

  int getAngles(BodyState &current,BodyState &next);
  int areBusy();
  void getMotionUpdate(MotionLocalizationUpdate &update);
};

class Kick{
  MotPlayer m_dive;
  MotPlayer m_bump;
  MotPlayer m_fwd;
  MotPlayer m_head;
  MotPlayer m_heads;
  MotPlayer m_headh;
  MotPlayer m_swing;
  MotPlayer m_hold;
  MotPlayer m_adive;
  MotPlayer m_sback;
  MotPlayer m_gdive;    //goalie dive to the side
  MotPlayer m_gblock;   //goalie block center
  MotPlayer m_suppblock;
  MotPlayer m_sdive;    //supporter side block/dive
  MotPlayer m_happy1;   
  MotPlayer m_happy2;   
  MotPlayer m_happy3;   
  MotPlayer m_sad; 
  MotPlayer m_bw_kick; 
  MotPlayer m_bw_backup; 

  int type,time,busy;
  bool generated_motion_update;
public:
  Kick();
  void load();
  void init(int kick_type);

  int getAngles(BodyState &current,BodyState &next);
  int areBusy();
  void getMotionUpdate(MotionLocalizationUpdate &update);
};

class Motion{
public:
  MotionCommand cmd;

private:
  int state,top_state;
  int time;

  Stand stand;
  Trot trot;
  Getup getup;
  Kick kick;
  Sound sound;
  TurnWithBall twb;

  BodyState body;
  LEDState LED_state;

  bool new_cmd;
  int head_scan_time_start;
  bool new_head_command;
public:
  Motion();
  void init();
  void init(double *angles);
  void setCommand(MotionCommand &ncmd);

  // set wp_beh with new values (from Aperios message)
  void setParams(WalkParam &params);
  WalkParam* getParams();
  WalkParam* getXYParams();
  WalkParam* getAParams();
  const Trot &getTrot() {return(trot);}

  // void toggle();
  // bool isEnabled() {return(enable);}

  void getAngles(double *angles);
  void getMotionUpdate(MotionLocalizationUpdate &update,ulong time);

  LEDState *getLEDState()
    {return(&LED_state);}
  void setLEDState(unsigned mask,unsigned val)
    {LED_state.cmd = (LED_state.cmd & ~mask) | (val & mask);}

  void setSpeakerSamplingRate(int sample_rate)
    {sound.setSpeakerSamplingRate(sample_rate);}
  void getSoundData(char *data,int data_size)
    {sound.getSoundData(data,data_size);}

  bool newParam();
  void sentVelParam();
};

} // Namespace

#endif
// __MOTION_H__