/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include "Behaviors.h"
#include "../headers/Dictionary.h"
#include "../headers/CircBufPacket.h"
#include "../Main/globals.h"
#include "../Main/RobotMain.h"
#include "../headers/Utility.h"
#include "../headers/Util.h"
#include "../headers/field.h"

/*
const double goalConfidence = 0.4;
const double ballConfidence = 0.4;
const double markerConfidence = 0.4;

const double cyan_goal_x = 1500;
const double yellow_goal_x = -1500;
const double cyan_goal_left_y = 300;
const double cyan_goal_right_y = -300;
const double yellow_goal_left_y = -300;
const double yellow_goal_right_y = 300;

const double vision_sd = M_PI/20;
*/

void BehaviorSystem::distanceReset() {
  relative_dist.x = relative_dist.y = relative_angle = 0.0;
  dist_calls = 0;
}


void BehaviorSystem::distanceTravelled() {
  pprintf(TextOutputStream, "Went (%f, %f) distance %f, final angle %f"
	  "(%d calls)\n", relative_dist.x, relative_dist.y,
	  relative_dist.length(), relative_angle*TODEG, dist_calls);
  distanceReset();
}

void BehaviorSystem::distanceUpdate(Motion::MotionLocalizationUpdate *move) {
  double movedist = move->pos_delta.length();
  double da;
  if (move->heading_delta.x == 0.0) {
    if (move->heading_delta.y == 0.0) {
      if (movedist == 0.0) return;
      da = 0.0;
    } else if (move->heading_delta.y > 0) da = M_PI/2.0; 
    else da = -M_PI/2.0;
  }
  else da = atan2(move->heading_delta.y, move->heading_delta.x);

  dist_calls++;

  relative_dist.x += move->pos_delta.x*cos(relative_angle) + 
    move->pos_delta.y*cos(relative_angle+M_PI/2.0);
  relative_dist.y += move->pos_delta.x*sin(relative_angle) +
    move->pos_delta.y*sin(relative_angle+M_PI/2.0);
  relative_angle += da;
  // angle_wrap(relative_angle);
}

/* FIXME: Rotation translations are fundamentally different from explicit
   translations. i.e. the standard deviations are very different! */
void BehaviorSystem::modifyMoveToHeadCoords(Motion::MotionLocalizationUpdate *move) {
  const bool debug = false;
  
  if ((move->heading_delta.x == 0) && (move->heading_delta.y == 0)) {
    return;
  }
  
  vector2d local_neck_offset = move->body.neck_offset;
  double rot_angle = atan2a(move->heading_delta.y, move->heading_delta.x);
  
  if (debug) {
    pprintf(TextOutputStream,"last a:%f\n", lastCommand.va*TODEG);
    pprintf(TextOutputStream,"rth: a:%f x:%f y:%f\n", rot_angle*TODEG,
        move->heading_delta.x, move->heading_delta.y);
  }
  
  local_neck_offset = local_neck_offset.rotate(rot_angle);
  local_neck_offset -= move->body.neck_offset;
  
  if (debug) {
    pprintf(TextOutputStream,"rot_neck: x:%f y:%f\n",
        local_neck_offset.x, local_neck_offset.y);
  }
  
  move->pos_delta += local_neck_offset;
}

void BehaviorSystem::forwardOdometry() {
  static int last_idx = 0;
  static ulong last_timestamp = 0;

  // cout << "forwardOdometry start" << endl;
  if(LocUpdateQueueBuffer!=NULL) {
    bool done = false;
      
    while(!done) {
      // cout << "last_idx: " << last_idx << endl;
      ulong buffer_timestamp=LocUpdateQueueBuffer->buffer[last_idx].timestamp;
      if(last_timestamp < buffer_timestamp && buffer_timestamp < timestamp) {
        last_timestamp=buffer_timestamp;

        // avoid messing up update queue
	MotionLocalizationUpdate move_update_storage = LocUpdateQueueBuffer->buffer[last_idx];
	MotionLocalizationUpdate *move_update = &move_update_storage;

        local_model.process_motion_update(move_update,last_timestamp);

	distanceUpdate(move_update);
	modifyMoveToHeadCoords(move_update);
        neck_offset = move_update->body.neck_offset;
  
        actual_walk_state = move_update->state_type;
  
	//        modeller.updateModelMovement(move_update, buffer_timestamp);
        botModeller.updateModelMovement(move_update);
	modeller->updateModelMotion(move_update);

        last_idx = (last_idx+1) % MotionLocalizationUpdateQueueBufferSize;
      } else {
        done=true;
      }
    }
  }
  // cout << "forwardOdometry end" << endl;

}

void BehaviorSystem::readConfig(const char *filename) {
  /* Load our sender ID from file */
  Dictionary *NetConfig = new Dictionary();

  NetConfig->read("/MS/config/wavelan.cfg");

  if(!NetConfig->getValueInt("SenderID", robotID))
    robotID = 1;
  
  delete NetConfig;

  Dictionary cfg_dict;
  cfg_dict.read(filename);

  {
    const char *goalColorString = "";
    if (cfg_dict.getValueString("defendGoal",goalColorString)) {
      if (!strcmp(goalColorString, "cyan")) {
        modeller->goalColor = DEFEND_CYAN_GOAL;
      } else if (!strcmp(goalColorString, "yellow")) {
        modeller->goalColor = DEFEND_YELLOW_GOAL;
      }
    }
  }
  
  {
    const char *teamColorString = "";
    if (cfg_dict.getValueString("teamColor",teamColorString)) {
      if (!strcmp(teamColorString, "blue")) {
        modeller->teamColor = TEAM_COLOR_BLUE;
      } else if (!strcmp(teamColorString, "red")) {
        modeller->teamColor = TEAM_COLOR_RED;
      }
    }
  }

  {
    const char *roleString = "";
    if (cfg_dict.getValueString("role",roleString)) {
      if(!strcmp(roleString, "new_attack")) {
	topFunc = BEHAVIOR_NEW_ATTACK;
      } else if (!strcmp(roleString, "goalie")) {
        topFunc = BEHAVIOR_GOALIE;
      } else if (!strcmp(roleString, "camera")) {
        topFunc = BEHAVIOR_CAMERA;
      } else if (!strcmp(roleString, "chase")) {
        topFunc = BEHAVIOR_CHASE_BALL;
      } else if (!strcmp(roleString, "kick_test")) {
        topFunc = BEHAVIOR_KICK_TEST;
      } else if (!strcmp(roleString, "localization_test")) {
        topFunc = BEHAVIOR_LOCALIZATION_TEST;
      } else if (!strcmp(roleString, "motion_test")) {
        topFunc = BEHAVIOR_MOTION_TEST;
      } else if (!strcmp(roleString, "obstacle_challenge")) {
        topFunc = BEHAVIOR_OBSTACLE_AVOIDANCE;
      } else if (!strcmp(roleString, "test_slenser")) {
        topFunc = BEHAVIOR_TEST_SLENSER;
      } else if (!strcmp(roleString, "test_jbruce")) {
        topFunc = BEHAVIOR_TEST_JBRUCE;
      } else if (!strcmp(roleString, "test_dvail2")) {
	topFunc = BEHAVIOR_TEST_DVAIL2;
      } else if (!strcmp(roleString, "test_brettb")) {
	topFunc = BEHAVIOR_TEST_BRETTB;
      } else if (!strcmp(roleString, "test_jfasola")) {
	topFunc = BEHAVIOR_TEST_JFASOLA;
      } else if (!strcmp(roleString, "test_generic")) {
	topFunc = BEHAVIOR_TEST_GENERIC;
      } else if (!strcmp(roleString, "bwball_challenge")) {
	topFunc = BEHAVIOR_BWBALL;
      }
    }
  }

  {
    int sub_role = 0;
    if (cfg_dict.getValueInt("subRole",sub_role)) {
      subRole = sub_role;
    } else {
      subRole = 0;
    }
  }

  {
    int LEDLoc = 1;
    cfg_dict.getValueInt("LocLEDs",LEDLoc);
    set_localization_leds = (LEDLoc == 1);
  }
  
  {
    int LEDBall = 1;
    cfg_dict.getValueInt("BallLED",LEDBall);
    set_ball_led = (LEDBall == 1);
  }

  {
    int setup_states = 0;
    if(cfg_dict.getValueInt("setupStates",setup_states)){
      setupStates = setup_states;
    } else {
      setupStates = 0;
    }
  }

  {

    int own_kick_off = 0;
    if(cfg_dict.getValueInt("ownKickOff",own_kick_off)){
      RobotMain::GetObject().have_kickoff = own_kick_off!=0;
    } else {
      RobotMain::GetObject().have_kickoff = true;
    }
  }

  pprintf(TextOutputStream, "Initializing model with goalColor = %i, teamColor = %i\n",
	  modeller->goalColor, modeller->teamColor);
  modeller->init(modeller->goalColor, modeller->teamColor, robotID, GetTime());
  botModeller.init();
}

void BehaviorSystem::changeTeamColor(int color) {
  if(color==TEAM_COLOR_BLUE){
    modeller->goalColor = DEFEND_CYAN_GOAL;
  }else{
    modeller->goalColor = DEFEND_YELLOW_GOAL;
  }
  modeller->teamColor = color;
}

bool BehaviorSystem::handleGetUp() {

  int cmd;

  switch (posture) {
    case PostureDetector::PosFallFront:
      cmd = Motion::MOTION_GETUP_FRONT;
      break;
    case PostureDetector::PosFallBack:
      cmd = Motion::MOTION_GETUP_BACK;
      break;
    case PostureDetector::PosFallLeft:
      cmd = Motion::MOTION_GETUP_LEFT;
      break;
    case PostureDetector::PosFallRight:
      cmd = Motion::MOTION_GETUP_RIGHT;
      break;
    default:
      cmd = -1;
      break;
  }

  if (cmd != -1) {
    command->motion_cmd = cmd;
    //pprintf(TextOutputStream,"Getting up: \n", getUpDir);
    // *((ulong*)0) = 0xF00F;
    return true;
  } else {
    return false;
  }
}

// const double edge_loose_delta = wedgeSize;
const double edge_delta = 75;
const double angle_cutoff = M_PI/10;
const double corner_dist = sqrt(halfLength*halfLength + halfWidth*halfWidth) - sqrt(wedgeSize*wedgeSize/2) - edge_delta/2;

double BehaviorSystem::angleToObject(vector3d obj) {
/*
  if (will_debug) {
    pprintf(TextOutputStream, "angleToObject:\n");
    pprintf(TextOutputStream, "obj: %f %f %f\n", obj.x, obj.y, obj.z);
    pprintf(TextOutputStream, "camera_loc: %f %f %f\n", camera_loc.x, camera_loc.y, camera_loc.z);
    pprintf(TextOutputStream, "camera_dir: %f %f %f\n", camera_dir.x, camera_dir.y, camera_dir.z);
  }
*/
  vector3d obj_dir = (obj-camera_loc).norm();
  double result = acos(camera_dir.dot(obj_dir));
/*
  if (will_debug) {
    pprintf(TextOutputStream, "obj_dir: %f %f %f\n", obj_dir.x, obj_dir.y, obj_dir.z);
    pprintf(TextOutputStream, "result: %f\n", result*TODEG);
  }
*/
  return result;
}