/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

this file is part of rcssserver3D
Fri May 9 2003
Copyright (C) 2002,2003 Koblenz University
Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
$Id: kicknrun.cpp,v 1.2 2004/02/12 14:07:21 fruit Exp $

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; version 2 of the License.

This program 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 General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "kicknrun.h"
#include <sstream>
#include <boost/random.hpp>

using namespace zeitgeist;
using namespace std;
using namespace boost;
using namespace oxygen;
using namespace salt;

KickNRun::KickNRun() : Soccer()
{
}

KickNRun::~KickNRun()
{
}

void KickNRun::BehaveBeforeKickOff()
{
    int num = mWM->GetTeamUnum();
    float f = mWM->GetAgentRadius() * 10;

    mt19937	gen;
    uniform_int<> px(-49,-10);
    variate_generator< mt19937, uniform_int<> > px_rand(gen, px);
    uniform_int<> py(-31,31);
    variate_generator< mt19937, uniform_int<> > py_rand(gen, py);

    Vector3f pos;
    switch(num){
       case 1:
	  pos[0] = -10; //px_rand(); 
	  pos[1] = -10; //py_rand(); 
	  pos[2] = 0;
	  break;

       case 2:
	  pos[0] = -20; //(-1 - (num % 4)) * f;
	  pos[1] = -30; //((num / 4) - 2) *f;
	  pos[2] = 0;
	  break;
    }


    mWM->SetBeamPosition(pos);
    Beam(Vector3f(pos));
    Drive(Vector3f(0,0,0));
}

void KickNRun::BehaveGoal()
{
   BehaveBeforeKickOff();
   cout << "!--- Goal In ---!" << endl;
}

void KickNRun::BehaveKickOff()
{
    TTeamIndex ti = mWM->GetMyTeam();
    if (ti == TI_NONE)
        {
            return;
        }

    TPlayMode myKickOff = (mWM->GetMyTeam() == TI_LEFT) ?
        PM_KickOff_Left : PM_KickOff_Right;

    if (mWM->GetPlayMode() == myKickOff)
        {
            BehaveMyKickOff();
            GetLog()->Debug() << "MyKickOff\n";
        } else
            {
                BehaveTheirKickOff();
                GetLog()->Debug() << "TheirKickOff\n";
            }
}

void KickNRun::BehaveMyKickOff()
{
    // for now, just kick and run
    BehavePlayOn();
}

void KickNRun::BehaveTheirKickOff()
{
    // do nothing
    Drive(Vector3f(0,0,0));
}

void KickNRun::BehaveKickIn()
{
    TTeamIndex ti = mWM->GetMyTeam();
    if (ti == TI_NONE)
        {
            return;
        }

    TPlayMode myKickIn = (mWM->GetMyTeam() == TI_LEFT) ?
        PM_KickIn_Left : PM_KickIn_Right;

    if (mWM->GetPlayMode() == myKickIn)
        {
            BehaveMyKickIn();
            GetLog()->Debug() << "MyKickIn\n";
        } else
            {
                BehaveTheirKickIn();
                GetLog()->Debug() << "TheirKickIn\n";
            }
}

void KickNRun::BehaveMyKickIn()
{
    // for now just kick and run
    BehavePlayOn();
}

void KickNRun::BehaveTheirKickIn()
{
    // do nothing
    Drive(Vector3f(0,0,0));
}

void KickNRun::BehavePlayOn()
{
    WorldModel::VisionSense ballSense =
        mWM->GetVisionSense(WorldModel::VO_BALL);

    WorldModel::VisionSense Opp1Sense =
       mWM->GetVisionSense(WorldModel::VO_OPP1);
    Vector3f Opp1 = mWM->GetPosition(Opp1Sense);
    // Vector3f Opp1 = mWM->GetObjectPosition(WorldModel::VO_OPP1);
    cout << "Opp1\t" << Opp1[0] << "\t" << Opp1[1] << endl;

    Vector3f goal(
                  mWM->GetFieldLength()/2.0,
                  0,
                  0
                  );

    Vector3f ballPos = mWM->GetPosition(ballSense);
    cout << "BallPos\t" <<
       ballPos[0] << "\t" <<
       ballPos[1] << endl;

    Vector3f goalDriveLine(goal - ballPos);
    goalDriveLine.Normalize();

    float minKickDist = mWM->GetMinimalKickDistance();

    Vector3f kickPos = ballPos - (goalDriveLine * (minKickDist - 0.15));
    cout << "kickPos\t" <<
       kickPos[0] << "\t" <<
       kickPos[1] << endl;

    Vector3f myPos = mWM->GetMyPosition();
    cout << "myPos\t" << 
       myPos[0] << "\t" <<
       myPos[1] << endl;
    Vector3f kickDriveLine = kickPos - myPos;
    Vector3f BallLine = ballPos - myPos;
    BallLine.Normalize();

    float kickPosDist = kickDriveLine.Length();
    cout << "kickPosDist\t" << kickPosDist << endl;
    kickDriveLine.Normalize();

    float BallDist = ballSense.distance;
    cout << "BallDist\t" << BallDist << endl;

    Vector3f TurnDriveLine = move_to_kickPos(kickPos, ballPos);

    if(kickPosDist > 0.6) {
       kickDriveLine *= 90;
       Drive(kickDriveLine);
    } else if(BallDist > minKickDist) {
       cout << "!---Over Distance---!" << endl;
       cout << "kickDriveLine\t" << kickDriveLine[0] << "\t" << kickDriveLine[1] << endl;
       kickDriveLine *= 25;
       Drive(kickDriveLine);
    } else if(Judge_kickable(kickPos)) {
       cout << "!---Kick Ball---!" << endl;
       Kick(0,15);
    // } else if(Judge_kickLine(kickPos,ballPos)) {
    //    cout << "!---On kickLine---!" << endl;
    //    cout << "BallLine\t" << BallLine[0] << "\t" << BallLine[1] << endl;
    //    BallLine *= 20;
    //    Drive(BallLine);
    } else {
       cout << "!---Turn Drive---!" << endl;
       cout << "TurnDriveLine\t" << TurnDriveLine[0] << "\t" << TurnDriveLine[1] << endl;
       TurnDriveLine *= 25;
       Drive(TurnDriveLine);
    }
}

bool KickNRun::Judge_kickLine(Vector3f kickPos, Vector3f ballPos)
{
   Vector3f myPos = mWM->GetMyPosition();
   Vector3f diff = ballPos - kickPos;
   float fx = diff[1]/diff[0];
   float inter = ballPos[1] - fx*ballPos[0];
   float Err = fx*myPos[0] + inter - myPos[1];

   if(abs(Err) < 0.2 && ballPos[0] > myPos[0]){
      return true;
   } else {
      return false;
   }
}

salt::Vector3f KickNRun::move_to_kickPos(Vector3f kickPos, Vector3f ballPos)
{
   //! kickPosへ移動するためのDriveLineを計算
   Vector3f DriveLine(1, 1, 0);
   switch (check_move_pattern()){
      case 1:
	 DriveLine[1] *= -tan(30.0/180 * M_PI);
	 break;

      case 2:
	 DriveLine[1] *= -tan(10.0/180 * M_PI);
	 break;

      case 3:
	 DriveLine[1] *= -tan(-20.0/180 * M_PI);
	 break;

      case 4:
	 DriveLine[1] *= tan(50.0/180 * M_PI);
	 break;

      case 5:
	 DriveLine[1] *= tan(60.0/180 * M_PI);
	 break;

      case 6:
	 DriveLine[1] *= tan(80.0/180 * M_PI);
	 break;

      case -1:
	 DriveLine[0] *= -1;
	 DriveLine[1] *= -tan(80.0/180 * M_PI);
	 break;

      case -2:
	 DriveLine[0] *= -1;
	 DriveLine[1] *= -tan(60.0/180 * M_PI);
	 break;

      case -3:
	 DriveLine[0] *= -1;
	 DriveLine[1] *= -tan(50.0/180 * M_PI);
	 break;

      case -4:
	 DriveLine[0] *= -1;
	 DriveLine[1] *= tan(-20.0/180 * M_PI);
	 break;

      case -5:
	 DriveLine[0] *= -1;
	 DriveLine[1] *= tan(15.0/180 * M_PI);
	 break;

      case -6:
	 DriveLine[0] *= -1;
	 DriveLine[1] *= tan(30.0/180 * M_PI);
	 break;

      default:
	 cout << "unknown move pattern" << endl;
	 break;
   }

   // Vector3f kickPosDir = kickPos - ballPos;
   // float tan = kickPosDir[1]/kickPosDir[0];
   // float kickDir = atan(tan)*180/M_PI;
   // float BallDir = mWM->GetVisionSense(WorldModel::VO_BALL).theta;
   // if((kickDir - BallDir) < 0) DriveLine[1] *= -1;
   
   DriveLine.Normalize();
   return DriveLine;
}

int KickNRun::check_move_pattern()
{
   //! 移動パターン検出
   int DirPattern = 0;
   float BallDir = mWM->GetVisionSense(WorldModel::VO_BALL).theta;
   
   if(-180 <= BallDir && BallDir < -150) { DirPattern = -6;}
   else if(-150 <= BallDir && BallDir < -120) { DirPattern = -5;}
   else if(-120 <= BallDir && BallDir < -90) { DirPattern = -4;}
   else if(-90 <= BallDir && BallDir < -60) { DirPattern = -3;}
   else if(-60 <= BallDir && BallDir < -30) { DirPattern = -2;}
   else if(-30 <= BallDir && BallDir < 0) { DirPattern = -1;}
   else if(0 <= BallDir && BallDir < 30){ DirPattern = 1;}
   else if(30 <= BallDir && BallDir < 60) { DirPattern =2;}
   else if(60 <= BallDir && BallDir < 90) { DirPattern = 3;}
   else if(90 <= BallDir && BallDir < 120) { DirPattern = 4;}
   else if(120 <= BallDir && BallDir < 150) { DirPattern = 5;}
   else if(150 <= BallDir && BallDir <= 180) { DirPattern = 6;}

   cout << "BallDir\t" << BallDir << endl;
   cout << "DirPattern\t" << DirPattern << endl;
   return DirPattern;
}

bool KickNRun::Judge_kickable(Vector3f kickPos)
{
   Vector3f myPos = mWM->GetMyPosition();
   Vector3f PosErr = myPos - kickPos;
   cout << "PosErr\t" << PosErr[0] << "\t" << PosErr[1] << endl;
   if(abs(PosErr[0]) < 0.15 && abs(PosErr[1]) < 0.15){
      return true;
   } else {
      return false;
   }
}

void KickNRun::Behave()
{
    switch (mWM->GetPlayMode())
        {
        case PM_BeforeKickOff :
            BehaveBeforeKickOff();
            break;

        case PM_KickOff_Left :
        case PM_KickOff_Right :
            BehaveKickOff();
            break;

        case PM_PlayOn :
            BehavePlayOn();
            break;

        case PM_KickIn_Left :
        case PM_KickIn_Right :
            BehaveKickIn();
            break;

	case PM_Goal_Left :
	case PM_Goal_Right :
	    BehaveGoal();
	    break;

        default:
            break;
        }
}