/* 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 "../Motion/MotionInterface.h"
#include "../headers/CircBufPacket.h"

#ifndef _KickLib_H_
#define _KickLib_H_

struct Kick{
  int kick_name;
  double angle;
  double dist;
};

const int NUM_KICKS = 16;

const int SHOOT_MASK = {
  1<<(Motion::MOTION_KICK_FOREWARD-300) |
  //1<<(Motion::MOTION_KICK_ARM_DIVE_L-300) |
  //1<<(Motion::MOTION_KICK_ARM_DIVE_R-300) |
  1<<(Motion::MOTION_KICK_BUMP-300) |
  1<<(Motion::MOTION_KICK_HEAD_L-300) |
  1<<(Motion::MOTION_KICK_HEAD_R-300) |
  1<<(Motion::MOTION_KICK_SWING_L-300) |
  1<<(Motion::MOTION_KICK_SWING_R-300) |
  1<<(Motion::MOTION_KICK_SLAP_BACK_L-300) |
  1<<(Motion::MOTION_KICK_SLAP_BACK_R-300)};

const int SHOOT_HEAD_ONLY = {
  1<<(Motion::MOTION_KICK_HEAD_HARD_L-300) |
  1<<(Motion::MOTION_KICK_HEAD_HARD_R-300)};

const int CORNER_MASK = {
  1<<(Motion::MOTION_KICK_BUMP-300) |
  1<<(Motion::MOTION_KICK_HEAD_SOFT_L-300) |
  1<<(Motion::MOTION_KICK_HEAD_SOFT_R-300) |
  1<<(Motion::MOTION_KICK_SLAP_BACK_L-300) |
  1<<(Motion::MOTION_KICK_SLAP_BACK_R-300)};


const int CLEAR_MASK = {
  1<<(Motion::MOTION_KICK_FOREWARD-300)};
  //1<<(Motion::MOTION_KICK_ARM_DIVE_L-300) |
  //1<<(Motion::MOTION_KICK_ARM_DIVE_R-300)};
  //1<<(Motion::MOTION_KICK_SLAP_BACK_L-300) |
  //1<<(Motion::MOTION_KICK_SLAP_BACK_R-300)};

const int PASS_MASK = {
  1<<(Motion::MOTION_KICK_ARM_DIVE_L-300) |
  1<<(Motion::MOTION_KICK_ARM_DIVE_R-300)};


// TODO: 
//    add the positional variance in the structure
//    add a _fast_ way of accessing the data in these kicks

extern Kick KickLib[NUM_KICKS];

bool getKickAngleDist(int name,float & angle,float & dist);

bool getKickAngle(int name,float & angle);

bool getKickDist(int name, float & dist);

// through: kick at *least* as far as rel_target_pt, not just *to* it
Kick selectKick(vector2d rel_target_pt,int mask,bool through, bool angle_only);

#endif