/* 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_INTERFACE_H__
#define __MOTION_INTERFACE_H__

#include "../headers/DogTypes.h"
#include "../headers/Geometry.h"
#include "LEDInterface.h"

//==== Input into motion system ======================================//

namespace Motion{

// Top Level States (state_type in MotionL-nUpdate)
const int STATE_WAITING        = 0;
const int STATE_STANDING       = 1;
const int STATE_WALKING        = 2;
const int STATE_KICKING        = 3;
const int STATE_GETUP          = 4;
const int STATE_SKATING        = 5;
const int STATE_TURN_WITH_BALL = 6;

// Sub-states for each top level state (state in MotionL-nUpdate)

// Standing States
const int MOTION_STAND_NEUTRAL     =  100;
const int MOTION_STAND_LEFT        =  101;
const int MOTION_STAND_RIGHT       =  102;
const int MOTION_STAND_CROUCH      =  103;
const int MOTION_STAND_SKATEBOARD  =  104;
const int MOTION_STAND_WALL_TURN_L =  105;
const int MOTION_STAND_WALL_TURN_R =  106;

// Walking States
const int MOTION_WALK_TROT       =  200;
const int MOTION_WALK_TROT_FAST  =  201;
const int MOTION_WALK_DRIBBLE    =  210;
const int MOTION_WALK_BEH        =  211; // walk specified by behaviors

// Kicking States
const int MOTION_KICK_DIVE        =  300;
const int MOTION_KICK_BUMP        =  301;
const int MOTION_KICK_FOREWARD    =  302;
const int MOTION_KICK_HEAD_L      =  303;
const int MOTION_KICK_HEAD_R      =  304;
const int MOTION_KICK_HEAD_SOFT_L =  305;
const int MOTION_KICK_HEAD_SOFT_R =  306;
const int MOTION_KICK_HEAD_HARD_L =  307;
const int MOTION_KICK_HEAD_HARD_R =  308;
const int MOTION_KICK_HOLD        =  309;
const int MOTION_KICK_SWING_L     =  310;
const int MOTION_KICK_SWING_R     =  311;
const int MOTION_KICK_ARM_DIVE_L  =  312;
const int MOTION_KICK_ARM_DIVE_R  =  313;
const int MOTION_KICK_SLAP_BACK_L =  314;
const int MOTION_KICK_SLAP_BACK_R =  315;

const int MOTION_BLOCK_GOALIE_L   =  316;
const int MOTION_BLOCK_GOALIE_R   =  317;
const int MOTION_BLOCK_GOALIE_C   =  318;
const int MOTION_BLOCK_SUPP       =  319;
const int MOTION_BLOCK_SUPP_L     =  320;
const int MOTION_BLOCK_SUPP_R     =  321;

const int MOTION_GOAL_HAPPY1      =  322;
const int MOTION_GOAL_HAPPY2      =  323;
const int MOTION_GOAL_HAPPY3      =  324;
const int MOTION_GOAL_SAD         =  325;

//Black & White Ball Challenge
const int MOTION_KICK_BW_L        =  326;
const int MOTION_KICK_BW_R        =  327;
const int MOTION_BACKUP_BW        =  328;

// Fallen States
const int MOTION_GETUP_NEUTRAL =  400;
const int MOTION_GETUP_BACK    =  401;
const int MOTION_GETUP_FRONT   =  402;
const int MOTION_GETUP_LEFT    =  403;
const int MOTION_GETUP_RIGHT   =  404;

// Skating States
const int MOTION_SKATE =  500;

//Turn with ball
const int MOTION_TURN_WITH_BALL_L = 600;
const int MOTION_TURN_WITH_BALL_R = 601;

#define MOTION_STATE(s) ((s)/100)

// Head States
const int HEAD_NO_CMD           =  0;
const int HEAD_LOOKAT           =  1;
const int HEAD_ANGLES           =  2; // Don't use this unless you have to!
const int HEAD_SCAN_BALL        =  3;
const int HEAD_SCAN_BALL_NEAR   =  4;
const int HEAD_SCAN_BALL_TURN   =  5;
const int HEAD_SCAN_DRIBBLE     =  6;
const int HEAD_SCAN_MARKERS     =  7;
const int HEAD_SCAN_MARKERS_HIGH=  8;
const int HEAD_SCAN_POINT       =  9;
const int HEAD_SCAN_OBSTACLES   =  10;
const int NUM_HEAD_CMDS         =  11;

// Tail States
const int TAIL_NO_CMD       =  0;
const int TAIL_AIM          =  1;
const int TAIL_FOLLOW       =  2;
const int NUM_TAIL_CMDS     =  3;

// Bounding modes
const int BOUND_GOTO_EGO = 0;
const int BOUND_NONE     = 1;
const int BOUND_SPEED    = 2;

// Sound States
const int SOUND_NONE = 0;
const int SOUND_NOTE = 1;
const int SOUND_FILE = 2;

// Sound Files
const int SOUND_FILE_TEST      = 0;
const int SOUND_FILE_SQUARE    = 1;
const int SOUND_FILE_RECTANGLE = 2;
const int SOUND_FILE_T         = 3;
const int SOUND_FILE_L         = 4;
const int SOUND_FILE_TRIANGLE  = 5;
const int SOUND_FILE_TADA      = 6;
const int NUM_SOUND_FILES      = 7;

// time (seconds) for a 90 degree windmill turn
const double PI_2_TURN_TIME = 1;

class MotionCommand{
public:
  short motion_cmd;       // target locomotion state
  short head_cmd;         // head command type
  short tail_cmd;         // tail command type
  short bound_mode;       // do not automatically bound motion
                          // (see bounding modes above)

  double vx,vy,va;      // desired position and angular velocity
                        // specified in mm/sec and rad/sec

  vector3d head_lookat;  // where look at for head (relative to front of body)
  double head_tilt_offset;  // tilt offset from head target
  double head_tilt; // only necessary if head_cmd==HEAD_ANGLES (should be rare)
  double head_pan;  // only necessary if head_cmd==HEAD_ANGLES (should be rare)
  double head_roll; // only necessary if head_cmd==HEAD_ANGLES (should be rare)
  double head_tilt2; // top tilt joint in ERS-7. Roll is not present

  short tail_pan;       // where to point tail in panning [-100,100]
  short tail_tilt;      // where to point tail in panning [-100,100]
  short mouth;          // mouth open fraction [0,100]

  unsigned short sound_cmd;
  unsigned short sound_frequency;
  unsigned long  sound_duration;
  unsigned short sound_file;

  LEDState led;

  void setFaceLEDs(int mask, int intensity, int mode){
    led.mode = mode;
    led.set = true;
    led.cmd |= mask; 
    for(int led_idx=0; led_idx<NUM_LEDS; led_idx++) {
      if((mask >> led_idx) & 1){
	led.intensity[led_idx] = intensity;
      }
    }
  }

  void setBinLEDs(int mask){
    led.set = true;
    led.cmd = mask;
  }

  void addBinLEDs(int mask){
    led.set = true;
    led.cmd |= mask;
  }

  void setBackLEDs(int mask, int intensity){
    led.set = true;
    led.cmd |= mask;
    for(int led_idx=0; led_idx<NUM_LEDS; led_idx++) {
      if((mask >> led_idx) & 1){
	led.intensity[led_idx] = intensity;
      }
    }
  }
  


/*
public:
  bool operator==(MotionCommand &mot) const
    {return(target_state == mot.target_state &&
            pos          == mot.pos &&
            heading      == mot.heading &&
            lookat       == mot.lookat &&
            led          == mot.led);}
*/
} __attribute__ ((aligned(8)));

struct BodyAttitude{
  double height; // height of body at center
  double angle;  // angle of body (y axis)
  vector2d neck_offset;
};

struct MotionLocalizationUpdate{
  ulong timestamp;        // timestamp in microseconds
  short state_type,state; // Which motion 

  vector2d pos_delta;     // current position relative to (0,0) from previous
  vector2d heading_delta; // current heading relative to (1,0) from previous

  BodyAttitude body;      // current position of the body
  bool stable;            // whether or not current attitude is reliable
};

const int MotionLocalizationUpdateQueueBufferSize = 8;

// Motion Posiiton Feedback Queue
// gives MainObj access to the position of the body and camera relative to ground
class MotionLocalizationUpdateQueue {
public:
  MotionLocalizationUpdate buffer[MotionLocalizationUpdateQueueBufferSize];
public:
  MotionLocalizationUpdate *getLatest()
  {
    int i,latest = 0;
    for(i=1; i<MotionLocalizationUpdateQueueBufferSize; i++){
      if(buffer[i].timestamp > buffer[latest].timestamp) latest = i;
    }
    return(&buffer[latest]);
  }
};

} // namespace MotionInterface

#endif
// __MOTION_INTERFACE_H__