/* 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 "KickLib.h"

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

Kick KickLib[NUM_KICKS]= {

  { Motion::MOTION_KICK_DIVE,                0,   2500.0},
  { Motion::MOTION_KICK_BUMP,                0,    500.0},
  { Motion::MOTION_KICK_FOREWARD,            0,   2500.0}, 

  { Motion::MOTION_KICK_HEAD_L,      RAD(  70),   1500.0}, 
  { Motion::MOTION_KICK_HEAD_R,      RAD( -70),   1500.0},
  { Motion::MOTION_KICK_HEAD_SOFT_L, RAD(  55),   1000.0}, 
  { Motion::MOTION_KICK_HEAD_SOFT_R, RAD( -55),   1000.0},
  { Motion::MOTION_KICK_HEAD_HARD_L, RAD(  70),   2500.0},
  { Motion::MOTION_KICK_HEAD_HARD_R, RAD( -70),   2500.0},

  { Motion::MOTION_KICK_HOLD,        RAD(   1),   0.0},

  { Motion::MOTION_KICK_SWING_L,     RAD(  80),   2500.0},
  { Motion::MOTION_KICK_SWING_R,     RAD( -80),   2500.0},
  { Motion::MOTION_KICK_ARM_DIVE_L,  RAD(   1),   2500.0},
  { Motion::MOTION_KICK_ARM_DIVE_R,  RAD(  -1),   2500.0},
  { Motion::MOTION_KICK_SLAP_BACK_L, RAD( 135),   1500.0},
  { Motion::MOTION_KICK_SLAP_BACK_R, RAD(-135),   1500.0}

};

const char *names[] = { "DIVE","BUMP", "FOREWARD",
				"HEAD_L", "HEAD_R",
				"HEAD_SOFT_L", "HEAD_SOFT_R",
				"HEAD_HARD_L", "HEAD_HARD_R",
				"KICK_HOLD",
				"SWING_L", "SWING_R",
				"ARM_DIVE_L", "ARM_DIVE_R",
				"SLAP_BACK_L", "SLAP_BACK_R"
};

bool getKickAngleDist(int name,float & angle,float & dist) {
  for (int i=0;i<NUM_KICKS;i++) {
    if (KickLib[i].kick_name == name) {
      dist = KickLib[i].dist;
      angle = KickLib[i].angle;
      return true;
    }
  }
  return false;
}

bool getKickAngle(int name,float & angle) {
  for (int i=0;i<NUM_KICKS;i++) {
    if (KickLib[i].kick_name == name) {
      angle = KickLib[i].angle;
      return true;
    }
  }
  return false;
}

bool getKickDist(int name, float & dist) {
  for (int i=0;i<NUM_KICKS;i++) {
    if (KickLib[i].kick_name == name) {
      dist = KickLib[i].dist;
      return true;
    }
  }
  return false;
}

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

  double angle_weight = 1.2;
  double dist_weight = 1.6;

  double target_angle = rel_target_pt.angle();
  double target_dist = rel_target_pt.length();

  //double best_angle_diff = RAD(360);
  double angle_diff;

  //double best_dist_diff = 10*1000;
  double dist_diff;

  double best_score = -100000;
  double score;

  pprintf(TextOutputStream,"target dist %f, target angle %f\n",
	  target_dist, target_angle);

  for(i=0; i<NUM_KICKS; i++){
    if( (mask>>i) & 1){
      
      angle_diff = fabs(norm_angle(KickLib[i].angle - target_angle));
      if(through)
	dist_diff = max(target_dist - KickLib[i].dist,0.0);
      else
	dist_diff = fabs(target_dist - KickLib[i].dist);
      
      if(angle_only)
	dist_diff = 0;

      score = -dist_diff*dist_weight - angle_diff*1000.0*angle_weight;
      //pprintf(TextOutputStream,"kick %d, score = %f\n",i,score);

      if(score > best_score){
	best_score = score;
	best = i;
      }

//       if(angle_diff < best_angle_diff){
// 	best_angle_diff = angle_diff;
// 	best = i;
//       }else if(fabs(angle_diff - best_angle_diff) < RAD(1.5)){
// 	if(through){
// 	  if(KickLib[i].dist > KickLib[best].dist) best = i;
// 	}else{
// 	  dist_diff = fabs(target_dist - KickLib[i].dist);
// 	  if(dist_diff < best_dist_diff){
// 	    best_dist_diff = dist_diff;
// 	    best = i;
// 	  }
// 	}
//       }
    }
  }
  return KickLib[best];
}