// -*-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;
}