<commit_before>/**
* @file PotentialActionSimulator.h
*
* @author <a href="mailto:<EMAIL>"><NAME></a>
* Implementation of class PotentialActionSimulator
*/

#include "PotentialActionSimulator.h"

// Debug
#include "Tools/Debug/DebugModify.h"
#include <Tools/Debug/DebugRequest.h>

using namespace naoth;
using namespace std;

PotentialActionSimulator::PotentialActionSimulator()
{
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:goal_target", "draw goal target", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:goal_field_geometry","some debug stuff for how the target is calculated", false);

  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:attackDirection:local", "it is what it is", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:attackDirection:global", "it is what it is", false);

  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:different_colors","...", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:local","draw local potential field", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:global","draw gobal potential field", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:high_res","draw gobal potential field with high resolution around the opponent goal", false);

  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:sensitivity", "", false);

  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:best_action","best action",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:my_pos","My Pos",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:long_kick","Long Kick",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:short_kick","Short Kick",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:sidekick_right","Sidekick right",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:sidekick_left","Sidekick left",false);
}


void PotentialActionSimulator::execute()
{
  const Pose2D& robotPose = getRobotPose();
  Vector2<double> ballRelative = getBallModel().position;


  // choose the goal model:
  // use the selflocator by default
  GoalModel::Goal oppGoalModel = getSelfLocGoalModel().getOppGoal(getCompassDirection(), getFieldInfo());

  //FIXME: frameWhenOpponentGoalWasSeen not supported by Model anymore!
  if ( getFrameInfo().getTimeSince(getLocalGoalModel().frameWhenOpponentGoalWasSeen.getTime()) < 10000
    //&& getLocalGoalModel().someGoalWasSeen
    && getLocalGoalModel().opponentGoalIsValid )
  {
    const GoalModel::Goal localOppGoalModel = getLocalGoalModel().getOppGoal(getCompassDirection(), getFieldInfo());
    if ( localOppGoalModel.calculateCenter().abs() < 4000 )
    {
      oppGoalModel = localOppGoalModel;
    }
  }

  // calculate the target point to play the ball to based on the 
  // goal model and the ball model 
  Vector2<double> targetPoint = getGoalTarget(ballRelative, oppGoalModel);
  // preview
  targetPoint = getMotionStatus().plannedMotion.hip / targetPoint;
  // ----------

  DEBUG_REQUEST("PotentialActionSimulator:goal_target",
    FIELD_DRAWING_CONTEXT;

    PEN("0000FF", 10);
    CIRCLE((robotPose * oppGoalModel.leftPost).x, (robotPose * oppGoalModel.leftPost).y, 50);
    CIRCLE((robotPose * oppGoalModel.rightPost).x, (robotPose * oppGoalModel.rightPost).y, 50);

    PEN("FF00FF", 10);
    CIRCLE((robotPose * targetPoint).x, (robotPose * targetPoint).y, 50);
  );

  // get valide obstacles
  std::list<Vector2<double> > obstacles = getValidObstacles();


  // calculate the potential field at the ball
  Vector2<double> attackDirection = calculatePotentialField(ballRelative, targetPoint, obstacles);
  getRawAttackDirection().attackDirection = attackDirection;


  //calculate the actions

  double action_potential[5];
  //action[0] = my_pos_potential;
  //action[1] = action_long_kick_potential;
  //action[2] = action_short_kick_potential;
  //action[3] = action_sidekick_left_potential;
  //action[4] = action_sidekick_right_potential;

  //first we get our potential to the goal, to see at the end if we can get our position better then yet
  Vector2<double> goal_target = getGoalTarget(getRobotPose().translation, oppGoalModel);
  double my_pos_potential = calculatePotential(getRobotPose().translation, goal_target, obstacles);

  //calculation for long kick
  Vector2<double> action_kicks[5];

  action_kicks[0].x = 0;
  action_kicks[0].y = 0;

  action_kicks[1].x = theParameters.action_long_kick_distance;
  action_kicks[1].y = 0;
  action_kicks[1].rotate(getRobotPose().rotation);

  action_kicks[2].x = theParameters.action_short_kick_distance;
  action_kicks[2].y = 0;
  action_kicks[2].rotate(getRobotPose().rotation);

  action_kicks[3].x = 0;
  action_kicks[3].y = -theParameters.action_sidekick_distance;
  action_kicks[3].rotate(getRobotPose().rotation);

  action_kicks[4].x = 0;
  action_kicks[4].y = theParameters.action_sidekick_distance;
  action_kicks[4].rotate(getRobotPose().rotation);

  int location = 0;
  double maximum = 0;

  for (int i = 0 ; i < 5 ; i++ ) {
	action_potential[i] = calculatePotential(getRobotPose().translation+action_kicks[i], goal_target, obstacles);
    if ( action_potential[i] > maximum ){
      maximum = action_potential[i];
      location = i;
    }
  }

  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:my_pos",
    FIELD_DRAWING_CONTEXT;
    PEN("000000", 1);

    CIRCLE(getRobotPose().translation.x, getRobotPose().translation.y, 50);
  );

   DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:long_kick",
    FIELD_DRAWING_CONTEXT;
    PEN("FF0000", 1);

    CIRCLE(getRobotPose().translation.x+action_kicks[1].x, getRobotPose().translation.y+action_kicks[1].y, 50);
  );

  
  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:short_kick",
    FIELD_DRAWING_CONTEXT;
    PEN("FF0000", 1);

	CIRCLE(getRobotPose().translation.x+action_kicks[2].x, getRobotPose().translation.y+action_kicks[2].y, 50);
  );
 
  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:sidekick_right",
    FIELD_DRAWING_CONTEXT;
    PEN("0000FF", 1);

	CIRCLE(getRobotPose().translation.x+action_kicks[3].x, getRobotPose().translation.y+action_kicks[3].y, 50);
  );
    
  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:sidekick_left",
    FIELD_DRAWING_CONTEXT;
    PEN("0000FF", 1);

	CIRCLE(getRobotPose().translation.x+action_kicks[4].x, getRobotPose().translation.y+action_kicks[4].y, 50);
  );

  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:best_action",
    FIELD_DRAWING_CONTEXT;
    PEN("F0F0F0", 1);


	CIRCLE(getRobotPose().translation.x+action_kicks[location].x, getRobotPose().translation.y+action_kicks[location].y, 50);
  );
  
  // ATTENTION: PREVIEW
  // while aplying preview consider following facts:
  // - transformation has to be applied to all points involved in the calculations
  //   (i.e., ball, goal(target point), opponents, myself)
  // - because all our functions are homogeneous with respect to linear transformations
  //   we may aply the preview AFTER all calculations are done
  //   (in particular it is because ||T*x + T*y|| = ||T||*||x-y|| = 1*||x-y||)
  // - in case it is not true in the future, I suggest to apply the preview
  //   in calculatePotentialField(...) to all points uniformly
  // ...
  // so for now it is much easier to calculate the previews in the behavior
  // (check StrategySymbols for this)
  //
  //Vector2<double> attackDirectionPreviewHip = getMotionStatus().plannedMotion.hip / attackDirection;
  //Vector2<double> attackDirectionPreviewLFoot = getMotionStatus().plannedMotion.lFoot / attackDirection;
  //Vector2<double> attackDirectionPreviewRFoot = getMotionStatus().plannedMotion.rFoot / attackDirection;


  DEBUG_REQUEST("PotentialActionSimulator:attackDirection:local",
    FIELD_DRAWING_CONTEXT;

    PEN("FFFFFF", 20);

    Vector2<double> targetDir = getRawAttackDirection().attackDirection;
    targetDir.normalize(200);

    ARROW(
          getBallModel().positionPreview.x,<commit_msg>changes<commit_after>/**
* @file PotentialActionSimulator.h
*
* @author <a href="mailto:<EMAIL>"><NAME></a>
* Implementation of class PotentialActionSimulator
*/

#include "PotentialActionSimulator.h"

// Debug
#include "Tools/Debug/DebugModify.h"
#include <Tools/Debug/DebugRequest.h>
#include <limits.h>

using namespace naoth;
using namespace std;

PotentialActionSimulator::PotentialActionSimulator()
{
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:goal_target", "draw goal target", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:goal_field_geometry","some debug stuff for how the target is calculated", false);

  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:attackDirection:local", "it is what it is", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:attackDirection:global", "it is what it is", false);

  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:different_colors","...", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:local","draw local potential field", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:global","draw gobal potential field", false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:high_res","draw gobal potential field with high resolution around the opponent goal", false);

  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_potential_field:sensitivity", "", false);

  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:best_action","best action",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:my_pos","My Pos",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:long_kick","Long Kick",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:short_kick","Short Kick",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:sidekick_right","Sidekick right",false);
  DEBUG_REQUEST_REGISTER("PotentialActionSimulator:draw_action_points:sidekick_left","Sidekick left",false);
}


void PotentialActionSimulator::execute()
{
  const Pose2D& robotPose = getRobotPose();
  Vector2<double> ballRelative = getBallModel().position;


  // choose the goal model:
  // use the selflocator by default
  GoalModel::Goal oppGoalModel = getSelfLocGoalModel().getOppGoal(getCompassDirection(), getFieldInfo());

  //FIXME: frameWhenOpponentGoalWasSeen not supported by Model anymore!
  if ( getFrameInfo().getTimeSince(getLocalGoalModel().frameWhenOpponentGoalWasSeen.getTime()) < 10000
    //&& getLocalGoalModel().someGoalWasSeen
    && getLocalGoalModel().opponentGoalIsValid )
  {
    const GoalModel::Goal localOppGoalModel = getLocalGoalModel().getOppGoal(getCompassDirection(), getFieldInfo());
    if ( localOppGoalModel.calculateCenter().abs() < 4000 )
    {
      oppGoalModel = localOppGoalModel;
    }
  }

  // calculate the target point to play the ball to based on the 
  // goal model and the ball model 
  Vector2d targetPoint = getGoalTarget(ballRelative, oppGoalModel);
  // preview
  targetPoint = getMotionStatus().plannedMotion.hip / targetPoint;
  // ----------

  DEBUG_REQUEST("PotentialActionSimulator:goal_target",
    FIELD_DRAWING_CONTEXT;

    PEN("0000FF", 10);
    CIRCLE((robotPose * oppGoalModel.leftPost).x, (robotPose * oppGoalModel.leftPost).y, 50);
    CIRCLE((robotPose * oppGoalModel.rightPost).x, (robotPose * oppGoalModel.rightPost).y, 50);

    PEN("FF00FF", 10);
    CIRCLE((robotPose * targetPoint).x, (robotPose * targetPoint).y, 50);
  );

  // get valide obstacles
  std::list<Vector2d> obstacles = getValidObstacles();


  // calculate the potential field at the ball
  Vector2d attackDirection = calculatePotentialField(ballRelative, targetPoint, obstacles);
  getRawAttackDirection().attackDirection = attackDirection;


  //calculate the actions

  double action_potential[5];
  //action[0] = my_pos_potential;
  //action[1] = action_long_kick_potential;
  //action[2] = action_short_kick_potential;
  //action[3] = action_sidekick_left_potential;
  //action[4] = action_sidekick_right_potential;

  //first we get our potential to the goal, to see at the end if we can get our position better then yet
  Vector2d goal_target = getGoalTarget(getRobotPose().translation, oppGoalModel);
  double my_pos_potential = calculatePotential(getRobotPose().translation, goal_target, obstacles);


  Vector2d ballPosition;

  //calculation for long kick
  Vector2d action_kicks[5];
  action_kicks[0] = ballPosition;
  action_kicks[1] = ballPosition + Vector2d(theParameters.action_long_kick_distance, 0);
  action_kicks[2] = ballPosition + Vector2d(theParameters.action_short_kick_distance, 0);
  action_kicks[3] = ballPosition + Vector2d(0, -theParameters.action_sidekick_distance);
  action_kicks[4] = ballPosition + Vector2d(0, theParameters.action_sidekick_distance);

  int location = -1;
  double minimum = std::numeric_limits<double>::max();

  FIELD_DRAWING_CONTEXT;
  PEN("000000", 1);

  for (int i = 0 ; i < 5 ; i++ ) {
	Vector2d actionGlobal = robotPose * action_kicks[i];
	action_potential[i] = calculatePotential(actionGlobal, goal_target, obstacles);
    if ( action_potential[i] < minimum || location == -1){
      minimum = action_potential[i];
      location = i;
    }

	CIRCLE(actionGlobal.x, actionGlobal.y, 50);
	TEXT_DRAWING(actionGlobal.x+100, actionGlobal.y+100, action_potential[i]);
  }

  ASSERT(location >= 0);

  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:my_pos",
    FIELD_DRAWING_CONTEXT;
    PEN("000000", 1);

    CIRCLE(getRobotPose().translation.x, getRobotPose().translation.y, 50);
  );

   DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:long_kick",
    FIELD_DRAWING_CONTEXT;
    PEN("FF0000", 1);

    CIRCLE(getRobotPose().translation.x+action_kicks[1].x, getRobotPose().translation.y+action_kicks[1].y, 50);
  );

  
  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:short_kick",
    FIELD_DRAWING_CONTEXT;
    PEN("FF0000", 1);

	CIRCLE(getRobotPose().translation.x+action_kicks[2].x, getRobotPose().translation.y+action_kicks[2].y, 50);
  );
 
  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:sidekick_right",
    FIELD_DRAWING_CONTEXT;
    PEN("0000FF", 1);

	CIRCLE(getRobotPose().translation.x+action_kicks[3].x, getRobotPose().translation.y+action_kicks[3].y, 50);
  );
    
  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:sidekick_left",
    FIELD_DRAWING_CONTEXT;
    PEN("0000FF", 1);

	CIRCLE(getRobotPose().translation.x+action_kicks[4].x, getRobotPose().translation.y+action_kicks[4].y, 50);
  );

  DEBUG_REQUEST("PotentialActionSimulator:draw_action_points:best_action",
    FIELD_DRAWING_CONTEXT;
    PEN("F0F0F0", 1);
	Vector2d actionGlobal = robotPose * action_kicks[location];

	CIRCLE(actionGlobal.x, actionGlobal.y, 50);
  );
  
  // ATTENTION: PREVIEW
  // while aplying preview consider following facts:
  // - transformation has to be applied to all points involved in the calculations
  //   (i.e., ball, goal(target point), opponents, myself)
  // - because all our functions are homogeneous with respect to linear transformations
  //   we may aply the preview AFTER all calculations are done
  //   (in particular it is because ||T*x + T*y|| = ||T||*||x-y|| = 1*||x-y||)
  // - in case it is not true in the future, I suggest to apply the preview
  //   in calculatePotentialField(...) to all points uniformly
  // ...
  // so for now it is much easier to calculate the previews in the behavior
  // (check StrategySymbols for this)
  //
  //Vector2<double> attackDirectionPreviewHip = getMotionStatus().plannedMotion.hip / attackDirection;
  //Vector2<double> attackDirectionPreviewLFoot = getMotionStatus().plannedMotion.lFoot / attackDirection;
  //Vector2<double> attackDirectionPreviewRFoot = getMotionStatus().plannedMotion.rFoot / attackDirection;


  DEBUG_REQUEST("PotentialActionSimulator:attackDirection:local",
    FIELD_DRAWING_CONTEXT;

    PEN("FFFFFF", 20);

    Vector2<double> targetDir = getRawAttackDirection().attackDirection;
    targetDir.normalize(200);

    ARROW(
          getBallModel().positionPreview.x,
{'fraction_non_alphanumeric': 0.11899663505659223, 'fraction_numerical': 0.015907005200367086, 'mean_word_length': 5.7573377428689545, 'pattern_counts': {'":': 0, '<': 39, '<?xml version=': 0, '>': 34, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '23437', 'n_tokens_mistral': 5651, 'n_tokens_neox': 5173, 'n_words': 940}