#include <iostream>		// mainを使わないとき不要
#include <vector>
#include <cmath>		// sqrt, powに必要
using namespace std;

// 戦略を分析した後に最適なものを返すクラス
class Teacher
{
private:
  vector< double > log;		// 各bhvのゴールとボールの距離の合計
  int bhvnum;			// bhvの数
  int curbhv;			// 行うべきbhv
  int precycle;			// 学習サイクル

  // 現在の状況を保持する変数
  double goalx;
  double goaly;
  double ballx;
  double bally;
  int cycle;

  double distance(double, double, double, double);

public:
  Teacher(int, int);
  void set_goalpos(double, double);
  void set_ballpos(double, double);
  void set_cycle(int);

  void update(void);
  int bhv(void);

};

// num = behaviorの数
// cyc = 学習するサイクル
Teacher::Teacher(int num, int cyc){
  curbhv = 0;
  bhvnum = num;
  precycle = cyc;

  log.resize(bhvnum);

  return;
}

// ゴールの座標設定
void Teacher::set_goalpos(double x, double y){
  goalx = x;
  goaly = y;
  return;
}

// ボールの座標設定
void Teacher::set_ballpos(double x, double y){
  ballx = x;
  bally = y;
  return;
}

// サイクル設定
void Teacher::set_cycle(int c){
  cycle = c;
  return;
}

// 学習データを更新
void Teacher::update(void){
  if (cycle < precycle ){
    log[curbhv] += distance(goalx, goaly, ballx, bally);
    curbhv = (int)( cycle / (precycle / bhvnum) );
  }else{
    int smaller = 0;
    for( int i=0; i<bhvnum; i++){
      if( log[smaller] > log[i] ) smaller = i;
    }
    curbhv = smaller;
  }
  return;
}

// エージェントが行うべきbehaviorを返す
int Teacher::bhv(void){
  return curbhv;
}

// ２点間の距離を計算する
double Teacher::distance(double ax, double ay, double bx, double by){
  double diffx = ax - bx;
  double diffy = ay - by;
  return pow( sqrt(diffx) + sqrt(diffy) , 2.0);
}


// 利用例
// behaviorが２つで50サイクル学習、
// ボールが徐々にゴールから離れる入力、
// 前半のほうがゴールに近いので、0が返れば正しい
int main(void){
  Teacher tcr(2, 50);
  tcr.set_goalpos(52.5, 0.0 );
  for(int i=0;i<80;i++){
    tcr.set_ballpos(50 - i , 0.0);
    tcr.set_cycle(i);
    tcr.update();
    cout << "now: "<< i << "bhv: "<< tcr.bhv() << endl;
  }
  return 0;
}