text: // -*-c++-*-
  
  /*!
    \file bhv_normal_dribble.cpp
    \brief normal dribble action that uses DribbleTable
  */
  
  /*
   *Copyright:
  
   Copyright (C) Hidehisa AKIYAMA
  
   This code is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 3 of the License, or (at your option) any later version.
  
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
  
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  
   *EndCopyright:
   */
  
  /////////////////////////////////////////////////////////////////////
  
  #ifdef HAVE_CONFIG_H
  #include <config.h>
  #endif
  
  #include "bhv_normal_dribble.h"
  
  #include "action_chain_holder.h"
  #include "action_chain_graph.h"
  #include "cooperative_action.h"
  
  #include "dribble.h"
  #include "short_dribble_generator.h"
  
  #include <rcsc/action/basic_actions.h>
  #include <rcsc/action/neck_scan_field.h>
  #include <rcsc/action/neck_turn_to_ball_or_scan.h>
  #include <rcsc/action/view_synch.h>
  
  #include <rcsc/player/intercept_table.h>
  #include <rcsc/player/player_agent.h>
  #include <rcsc/player/soccer_intention.h>
  #include <rcsc/player/debug_client.h>
  #include <rcsc/common/logger.h>
  #include <rcsc/common/audio_memory.h>
  #include <rcsc/common/server_param.h>
  #include <rcsc/soccer_math.h>
  #include <rcsc/math_util.h>
  
  //#define DEBUG_PRINT
  
  using namespace rcsc;
  
  class IntentionNormalDribble
      : public SoccerIntention {
  private:
      const Vector2D M_target_point; //!< trapped ball position
  
      int M_turn_step; //!< remained turn step
      int M_dash_step; //!< remained dash step
  
      GameTime M_last_execute_time; //!< last executed time
  
      NeckAction::Ptr M_neck_action;
      ViewAction::Ptr M_view_action;
  
  public:
  
      IntentionNormalDribble( const Vector2D & target_point,
                              const int n_turn,
                              const int n_dash,
                              const GameTime & start_time ,
                              NeckAction::Ptr neck = NeckAction::Ptr(),
                              ViewAction::Ptr view = ViewAction::Ptr() )
          : M_target_point( target_point ),
            M_turn_step( n_turn ),
            M_dash_step( n_dash ),
            M_last_execute_time( start_time ),
            M_neck_action( neck ),
            M_view_action( view )
        { }
  
      bool finished( const PlayerAgent * agent );
  
      bool execute( PlayerAgent * agent );
  
  private:
      /*!
        \brief clear the action queue
       */
      void clear()
        {
            M_turn_step = M_dash_step = 0;
        }
  
      bool checkOpponent( const WorldModel & wm );
      bool doTurn( PlayerAgent * agent );
      bool doDash( PlayerAgent * agent );
  };
  
  /*-------------------------------------------------------------------*/
  /*!
  
  */
  bool
  IntentionNormalDribble::finished( const PlayerAgent * agent )
  {
      if ( M_turn_step + M_dash_step == 0 )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (finished) check finished. empty queue" );
          return true;
      }
  
      const WorldModel & wm = agent->world();
  
      if ( M_last_execute_time.cycle() + 1 != wm.time().cycle() )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": finished(). last execute time is not match" );
          return true;
      }
  
      if ( wm.existKickableTeammate()
           || wm.existKickableOpponent() )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (finished). exist other kickable player" );
          return true;
      }
  
      if ( wm.ball().pos().dist2( M_target_point ) < std::pow( 1.0, 2 )
           && wm.self().pos().dist2( M_target_point ) < std::pow( 1.0, 2 ) )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (finished). reached target point" );
          return true;
      }
  
      const Vector2D ball_next = wm.ball().pos() + wm.ball().vel();
      if ( ball_next.absX() > ServerParam::i().pitchHalfLength() - 0.5
           || ball_next.absY() > ServerParam::i().pitchHalfWidth() - 0.5 )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (finished) ball will be out of pitch. stop intention." );
          return true;
      }
  
      if ( wm.audioMemory().passRequestTime() == agent->world().time() )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (finished). heard pass request." );
          return true;
      }
  
      // playmode is checked in PlayerAgent::parse()
      // and intention queue is totally managed at there.
  
      dlog.addText( Logger::DRIBBLE,
                    __FILE__": (finished). not finished yet." );
  
      return false;
  }
  
  
  /*-------------------------------------------------------------------*/
  /*!
  
  */
  bool
  IntentionNormalDribble::execute( PlayerAgent * agent )
  {
      if ( M_turn_step + M_dash_step == 0 )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (intention:execute) empty queue." );
          return false;
      }
  
      dlog.addText( Logger::DRIBBLE,
                    __FILE__": (intention:execute) turn=%d dash=%d",
                    M_turn_step, M_dash_step );
  
      const WorldModel & wm = agent->world();
  
      //
      // compare the current queue with other chain action candidates
      //
  
      if ( wm.self().isKickable()
           && M_turn_step <= 0 )
      {
          CooperativeAction::Ptr current_action( new Dribble( wm.self().unum(),
                                                              M_target_point,
                                                              wm.ball().vel().r(),
                                                              0,
                                                              M_turn_step,
                                                              M_dash_step,
                                                              "queuedDribble" ) );
          current_action->setIndex( 0 );
          current_action->setFirstDashPower( ServerParam::i().maxDashPower() );
  
          ShortDribbleGenerator::instance().setQueuedAction( wm, current_action );
  
          ActionChainHolder::instance().update( wm );
          const ActionChainGraph & search_result = ActionChainHolder::i().graph();
          const CooperativeAction & first_action = search_result.getFirstAction();
  
          if ( first_action.category() != CooperativeAction::Dribble
               || ! first_action.targetPoint().equals( current_action->targetPoint() ) )
          {
              agent->debugClient().addMessage( "CancelDribbleQ" );
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": (intention:execute) cancel. select other action." );
              return false;
          }
      }
  
      //
      //
      //
  
      if ( checkOpponent( wm ) )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (intention:execute). but exist opponent. cancel intention." );
          return false;
      }
  
      //
      // execute action
      //
  
      if ( M_turn_step > 0 )
      {
          if ( ! doTurn( agent ) )
          {
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": (intention:exuecute) failed to turn. clear intention" );
              this->clear();
              return false;
          }
      }
      else if ( M_dash_step > 0 )
      {
          if ( ! doDash( agent ) )
          {
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": (intention:execute) failed to dash.  clear intention" );
              this->clear();
              return false;
          }
      }
      else
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (intention:execute). No command queue" );
          this->clear();
          return false;
      }
  
      dlog.addText( Logger::DRIBBLE,
                    __FILE__": (intention:execute). done" );
      agent->debugClient().addMessage( "NormalDribbleQ%d:%d", M_turn_step, M_dash_step );
      agent->debugClient().setTarget( M_target_point );
  
  
      if ( ! M_view_action )
      {
          if ( wm.gameMode().type() != GameMode::PlayOn
               || M_turn_step + M_dash_step <= 1 )
          {
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": (intention:execute) default view synch" );
              agent->debugClient().addMessage( "ViewSynch" );
              agent->setViewAction( new View_Synch() );
          }
          else
          {
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": (intention:execute) default view normal" );
              agent->debugClient().addMessage( "ViewNormal" );
              agent->setViewAction( new View_Normal() );
          }
      }
      else
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (intention:execute) registered view" );
          agent->debugClient().addMessage( "ViewRegisterd" );
          agent->setViewAction( M_view_action->clone() );
      }
  
      if ( ! M_neck_action )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (intention:execute) default turn_neck scan field" );
          agent->debugClient().addMessage( "NeckScan" );
          agent->setNeckAction( new Neck_TurnToBallOrScan( 0 ) );
      }
      else
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (intention:execute) registered turn_neck" );
          agent->debugClient().addMessage( "NeckRegistered" );
          agent->setNeckAction( M_neck_action->clone() );
      }
  
      M_last_execute_time = wm.time();
  
      return true;
  }
  
  /*-------------------------------------------------------------------*/
  /*!
  
  */
  bool
  IntentionNormalDribble::checkOpponent( const WorldModel & wm )
  {
      const Vector2D ball_next = wm.ball().pos() + wm.ball().vel();
  
      /*--------------------------------------------------------*/
      // exist near opponent goalie in NEXT cycle
      if ( ball_next.x > ServerParam::i().theirPenaltyAreaLineX()
           && ball_next.absY() < ServerParam::i().penaltyAreaHalfWidth() )
      {
          const PlayerObject * opp_goalie = wm.getOpponentGoalie();
          if ( opp_goalie
               && opp_goalie->distFromBall() < ( ServerParam::i().catchableArea()
                                                 + ServerParam::i().defaultPlayerSpeedMax() )
               )
          {
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": existOpponent(). but exist near opponent goalie" );
              this->clear();
              return true;
          }
      }
  
      const PlayerObject * nearest_opp = wm.getOpponentNearestToSelf( 5 );
  
      if ( ! nearest_opp )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": existOppnent(). No opponent" );
          return false;
      }
  
      /*--------------------------------------------------------*/
      // exist very close opponent at CURRENT cycle
      if (  nearest_opp->distFromBall() < ServerParam::i().defaultKickableArea() + 0.2 )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": existOpponent(). but exist kickable opp" );
          this->clear();
          return true;
      }
  
      /*--------------------------------------------------------*/
      // exist near opponent at NEXT cycle
      if ( nearest_opp->pos().dist( ball_next )
           < ServerParam::i().defaultPlayerSpeedMax() + ServerParam::i().defaultKickableArea() + 0.3 )
      {
          const Vector2D opp_next = nearest_opp->pos() + nearest_opp->vel();
          // oppopnent angle is known
          if ( nearest_opp->bodyCount() == 0
               || nearest_opp->vel().r() > 0.2 )
          {
              Line2D opp_line( opp_next,
                               ( nearest_opp->bodyCount() == 0
                                 ? nearest_opp->body()
                                 : nearest_opp->vel().th() ) );
              if ( opp_line.dist( ball_next ) > 1.2 )
              {
                  // never reach
                  dlog.addText( Logger::DRIBBLE,
                                __FILE__": existOpponent(). opp never reach." );
              }
              else if ( opp_next.dist( ball_next ) < 0.6 + 1.2 )
              {
                  dlog.addText( Logger::DRIBBLE,
                                __FILE__": existOpponent(). but opp may reachable(1)." );
                  this->clear();
                  return true;
              }
  
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": existOpponent(). opp angle is known. opp may not be reached." );
          }
          // opponent angle is not known
          else
          {
              if ( opp_next.dist( ball_next ) < 1.2 + 1.2 ) //0.6 + 1.2 )
              {
                  dlog.addText( Logger::DRIBBLE,
                                __FILE__": existOpponent(). but opp may reachable(2)." );
                  this->clear();
                  return true;
              }
          }
  
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": existOpponent(). exist near opp. but avoidable?" );
      }
  
      return false;
  }
  
  /*-------------------------------------------------------------------*/
  /*!
  
  */
  bool
  IntentionNormalDribble::doTurn( PlayerAgent * agent )
  {
      if ( M_turn_step <= 0 )
      {
          return false;
      }
  
      const double default_dist_thr = 0.5;
  
      const WorldModel & wm = agent->world();
  
      --M_turn_step;
  
      Vector2D my_inertia = wm.self().inertiaPoint( M_turn_step + M_dash_step );
      AngleDeg target_angle = ( M_target_point - my_inertia ).th();
      AngleDeg angle_diff = target_angle - wm.self().body();
  
      double target_dist = ( M_target_point - my_inertia ).r();
      double angle_margin
          = std::max( 15.0,
                      std::fabs( AngleDeg::atan2_deg( default_dist_thr,
                                                      target_dist ) ) );
  
      if ( angle_diff.abs() < angle_margin )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (doTurn)  but already facing. diff = %.1f  margin=%.1f",
                        angle_diff.degree(), angle_margin );
          this->clear();
          return false;
      }
  
      dlog.addText( Logger::DRIBBLE,
                    __FILE__": (doTurn) turn to (%.2f, %.2f) moment=%f",
                    M_target_point.x, M_target_point.y, angle_diff.degree() );
  
      agent->doTurn( angle_diff );
  
      return true;
  }
  
  /*-------------------------------------------------------------------*/
  /*!
  
  */
  bool
  IntentionNormalDribble::doDash( PlayerAgent * agent )
  {
      if ( M_dash_step <= 0 )
      {
          return false;
      }
  
      const WorldModel & wm = agent->world();
  
      --M_dash_step;
  
      double dash_power
          = wm.self().getSafetyDashPower( ServerParam::i().maxDashPower() );
      double accel_mag = dash_power * wm.self().dashRate();
  
      Vector2D dash_accel = Vector2D::polar2vector( accel_mag, wm.self().body() );
  
      Vector2D my_next = wm.self().pos() + wm.self().vel() + dash_accel;
      Vector2D ball_next = wm.ball().pos() + wm.ball().vel();
      Vector2D ball_next_rel = ( ball_next - my_next ).rotatedVector( - wm.self().body() );
      double ball_next_dist = ball_next_rel.r();
  
      if ( ball_next_dist < ( wm.self().playerType().playerSize()
                              + ServerParam::i().ballSize() )
           )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (doDash) collision may occur. ball_dist = %f",
                        ball_next_dist );
          this->clear();
          return false;
      }
  
      if ( ball_next_rel.absY() > wm.self().playerType().kickableArea() - 0.1 )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (doDash) next Y difference is over. y_diff = %f",
                        ball_next_rel.absY() );
          this->clear();
          return false;
      }
  
      // this dash is the last of queue
      // but at next cycle, ball is NOT kickable
      if ( M_dash_step <= 0 )
      {
          if ( ball_next_dist > wm.self().playerType().kickableArea() - 0.15 )
          {
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": (doDash) last dash. but not kickable at next. ball_dist=%f",
                            ball_next_dist );
              this->clear();
              return false;
          }
      }
  
      if ( M_dash_step > 0 )
      {
          // remain dash step. but next dash cause over run.
          AngleDeg ball_next_angle = ( ball_next - my_next ).th();
          if ( ( wm.self().body() - ball_next_angle ).abs() > 90.0
               && ball_next_dist > wm.self().playerType().kickableArea() - 0.2 )
          {
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": (doDash) dash. but run over. ball_dist=%f",
                            ball_next_dist );
              this->clear();
              return false;
          }
      }
  
      dlog.addText( Logger::DRIBBLE,
                    __FILE__": (doDash) power=%.1f  accel_mag=%.2f",
                    dash_power, accel_mag );
      agent->doDash( dash_power );
  
      return true;
  }
  
  
  
  
  
  /*-------------------------------------------------------------------*/
  /*-------------------------------------------------------------------*/
  /*-------------------------------------------------------------------*/
  
  /*-------------------------------------------------------------------*/
  /*!
  
  */
  Bhv_NormalDribble::Bhv_NormalDribble( const CooperativeAction & action,
                                        NeckAction::Ptr neck,
                                        ViewAction::Ptr view )
      : M_target_point( action.targetPoint() ),
        M_first_ball_speed( action.firstBallSpeed() ),
        M_first_turn_moment( action.firstTurnMoment() ),
        M_first_dash_power( action.firstDashPower() ),
        M_first_dash_angle( action.firstDashAngle() ),
        M_total_step( action.durationStep() ),
        M_kick_step( action.kickCount() ),
        M_turn_step( action.turnCount() ),
        M_dash_step( action.dashCount() ),
        M_neck_action( neck ),
        M_view_action( view )
  {
      if ( action.category() != CooperativeAction::Dribble )
      {
          M_target_point = Vector2D::INVALIDATED;
          M_total_step = 0;
      }
  }
  
  /*-------------------------------------------------------------------*/
  /*!
  
  */
  bool
  Bhv_NormalDribble::execute( PlayerAgent * agent )
  {
      const WorldModel & wm = agent->world();
  
      if ( ! wm.self().isKickable() )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (execute). no kickable..." );
          return false;
      }
  
      if ( ! M_target_point.isValid()
           || M_total_step <= 0 )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (execute). illegal target point or illegal total step" );
  #ifdef DEBUG_PRINT
          std::cerr << wm.self().unum() << ' ' << wm.time()
                    << " Bhv_NormalDribble::execute() illegal target point or illegal total step"
                    << std::endl;
  #endif
          return false;
      }
  
      const ServerParam & SP = ServerParam::i();
  
      if ( M_kick_step > 0 )
      {
          Vector2D first_vel = M_target_point - wm.ball().pos();
          first_vel.setLength( M_first_ball_speed );
  
          const Vector2D kick_accel = first_vel - wm.ball().vel();
          const double kick_power = kick_accel.r() / wm.self().kickRate();
          const AngleDeg kick_angle = kick_accel.th() - wm.self().body();
  
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (execute). first kick: power=%f angle=%f, n_turn=%d n_dash=%d",
                        kick_power, kick_angle.degree(),
                        M_turn_step, M_dash_step );
  
          if ( kick_power > SP.maxPower() + 1.0e-5 )
          {
              dlog.addText( Logger::DRIBBLE,
                            __FILE__": (execute) over the max power" );
  #ifdef DEBUG_PRINT
              std::cerr << __FILE__ << ':' << __LINE__ << ':'
                        << wm.self().unum() << ' ' << wm.time()
                        << " over the max power " << kick_power << std::endl;
  #endif
          }
  
          agent->doKick( kick_power, kick_angle );
  
          dlog.addCircle( Logger::DRIBBLE,
                          wm.ball().pos() + first_vel,
                          0.1,
                          "#0000ff" );
      }
      else if ( M_turn_step > 0 )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (execute). first turn: moment=%.2f, n_turn=%d n_dash=%d",
                        M_first_turn_moment,
                        M_turn_step, M_dash_step );
  
          agent->doTurn( M_first_turn_moment );
  
      }
      else if ( M_dash_step > 0 )
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (execute). first dash: dash power=%.1f dir=%.1f",
                        M_first_dash_power, M_first_dash_angle.degree() );
          agent->doDash( M_first_dash_power, M_first_dash_angle );
      }
      else
      {
          dlog.addText( Logger::DRIBBLE,
                        __FILE__": (execute). no action steps" );
          std::cerr << __FILE__ << ':' << __LINE__ << ':'
                    << wm.self().unum() << ' ' << wm.time()
                    << " no action step." << std::endl;
          return false;
      }
  
  
     agent->setIntention( new IntentionNormalDribble( M_target_point,
                                                      M_turn_step,
                                                      M_total_step - M_turn_step - 1, // n_dash
                                                      wm.time() ) );
  
     if ( ! M_view_action )
     {
         if ( M_turn_step + M_dash_step >= 3 )
         {
             agent->setViewAction( new View_Normal() );
         }
     }
     else
     {
         agent->setViewAction( M_view_action->clone() );
     }
  
     if ( ! M_neck_action )
     {
         agent->setNeckAction( new Neck_ScanField() );
     }
     else
     {
         agent->setNeckAction( M_neck_action->clone() );
     }
  
      return true;
  }
  