/* 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 <math.h>

#include "../headers/CircBufPacket.h"
#include "../headers/Config.h"
#include "../headers/Dictionary.h"
#include "../headers/MessageStructures.hh"
#include "../headers/Reporting.h"
#include "../headers/Sensors.h"
#include "../headers/SPInBufferIDs.h"
#include "../headers/SPOutEncoder.h"
#include "../Logger/LoggerInterface.h"
#include "../Main/MainObject.h"
#include "../Main/RobotMain.h"
#include "../Motion/MotionInterface.h"
#include "../Vision/VisionInterface.h"
#include "Behaviors.h"
#include "Goalie.h"

const double LOCALIZE_LED_THETA_MAX = M_PI/8;
const double LOCALIZE_LED_DIST_MAX = 40;
const double MDODEL_BALL_ANG_DEV_LED_DIST_MAX = M_PI/8;

const double COMMAND_DIFFS_DECAY = 0.9;

const double jaw_open_position = 75;

BehaviorSystem::BehaviorSystem() {
  silence_is_golden = true;
  object_info = NULL;
  command = NULL;
  timestamp = 0;
  posture = PostureDetector::PosStable;
  topFunc = BEHAVIOR_CHASE_BALL;
  callChainPosn = 0;

  // Why don't we just initialize the world model in its
  // constructor?
  //  memset(&modeller, 0, sizeof(WorldModel));
  //  modeller.goalColor = DEFEND_CYAN_GOAL;
  //  modeller.teamColor = TEAM_COLOR_BLUE;
  set_localization_leds = set_ball_led = true;
  cachedmarkers = 0;
  relative_dist.x = relative_dist.y = 0.0;
  relative_angle = 0.0;
  neck_offset.set(70, 0);
  for (int i = 0; i < 6; i++) {
    markerconf[i] = 0.0;
  }
  //  fieldSide = 0;

  behavior_names_stream = NULL;
  behavior_trace_stream = NULL;

  local_model.init();

  features = NULL;

  modeller = &(RobotMain::GetObject().world_model);
}

void BehaviorSystem::initializeStreams(PacketStreamCollection *out_psc) {
  if(features==NULL) features = (FeatureSet *)(FeatureSet::create());
  features       ->initializeStreams(out_psc);

  //  modeller        .initializeStreams(out_psc);

  int stream_id;
  
  stream_id = out_psc->allocateStream(256*10,10);
  if(stream_id >= 0){
    behavior_trace_stream = out_psc->getStream(stream_id);
    behavior_trace_stream->type = SPOutEncoder::BehaviorTraceLead;
    behavior_trace_stream->binary = true;
  }

  stream_id = out_psc->allocateStream(4*1024*5,5);
  if(stream_id >= 0){
    behavior_names_stream = out_psc->getStream(stream_id);
    behavior_names_stream->type = SPOutEncoder::BehaviorNamesLead;
    behavior_names_stream->binary = true;
  }
}

void
BehaviorSystem::updateFeatureSet()
{
  bool am_goalie = false;
  if(features==NULL) features = (FeatureSet *)(FeatureSet::create());
  features->update(timestamp, robotID, modeller->goalColor, 
		   modeller->teamColor, am_goalie,
                   modeller, &botModeller, object_info, 
                   radial_map, &local_model, team_msg_mgr,
                   neck_offset, stuck_info, head_vel, 
                   RobotMain::GetObject().robot_is_paused, actual_walk_state);
}

void
BehaviorSystem::prerun(MotionCommand* command_arg,
                       SensorData* sens_data, TeamMsgMgr *_team_msg_mgr,
                       ulong time)
{
  static EventTimeReporter reporter("BehaviorSystem::prerun",SecToTime(5.0),SecToTime(100.0),1000UL,&TextOutputStream);
  EventTimeReporter::EventTimer timer(&reporter,config.spoutConfig.dumpProfile);

  object_info = RobotMain::GetObject().getVision()->object_info;
  radial_map = RobotMain::GetObject().getVision()->getRadialMap();
  head_vel = RobotMain::GetObject().getVision()->getHeadVelocity();
  command = command_arg;
  sensor_data = sens_data;
  team_msg_mgr = _team_msg_mgr;
  team_msg_mgr->sendOutput();
  timestamp = time;

  callChainPosn = 0;

  memset(command, 0, sizeof(MotionCommand));
  command->motion_cmd = MOTION_STAND_NEUTRAL;
  command->head_cmd = HEAD_LOOKAT;
  command->tail_cmd = TAIL_AIM;
  command->head_lookat.x = 200;
  command->head_lookat.y = 0;
  command->head_lookat.z = 50;

  command->led.cmd = 0; // turn off all LEDs

  Vision *vision = RobotMain::GetObject().getVision();
  camera_loc = vision->get_camera_loc();
  camera_dir = vision->get_camera_dir();
  
  forwardOdometry();
   
  local_model.process_vision_update(radial_map,timestamp);
  
  // FIXME: this is an ugly compatibility hack, note that the position is lagged a frame
  RobotPositionInfo posn;
  EventManager *em;
  LocalizationEP *loc;
  em = EventManager::getManager();
  loc = dynamic_cast<LocalizationEP *>(em->getEventProcessor(em->getEventProcessorId("Localizer")));
  posn = *(loc->get(0UL)); // more hack
  modeller->updateModel(object_info, sensor_data, team_msg_mgr, 
			   posn, head_vel,timestamp);
 
  team_msg_mgr->updateSelf(modeller, object_info, timestamp);

  botModeller.updateModelSensors(object_info, sensor_data, 
				 camera_loc, camera_dir, timestamp);
}

void
BehaviorSystem::run(MotionCommand* command_arg,
		    SensorData* sens_data, TeamMsgMgr *_team_msg_mgr,
		    ulong time) {

//   pprintf(TextOutputStream, "Start Current ball dist: %f angle: %f\n",
// 	  modeller.ball_mean.dist(),
// 	  modeller.ball_mean.ang()*TODEG);

  if (handleGetUp()) {
    return;
  }

  updateFeatureSet();

  switch(topFunc){

    case BEHAVIOR_NEW_ATTACK:       new_attack       (features); break;
    case BEHAVIOR_GOALIE:           goalie           (features); break;
    case BEHAVIOR_OBSTACLE_AVOIDANCE:obstacleAvoidance(features); break;
    case BEHAVIOR_LOCALIZATION_TEST:localization_test(features); break;
    case BEHAVIOR_BWBALL:          bwball_challenge  (features); break;

    case BEHAVIOR_CHASE_BALL:      chaseBall         (features); break;
    case BEHAVIOR_KICK_TEST:       kickTest          (features); break;
    case BEHAVIOR_CAMERA:          camera            (features); break;

    case BEHAVIOR_TEST_GENERIC:    test_generic      (features); break;

    case BEHAVIOR_MOTION_TEST:     motion_test       (features); break;
    
    default:
      pprintf(TextOutputStream,"Top function not specified in Behaviours!");
  }

  static bool wrote_log = false;
  if((sensor_data->button & LogButton)!=0) {
    pprintf(TextOutputStream,"got log button press, wrote_log=%d\n",wrote_log?1:0);
  }
  if((sensor_data->button & HeadBackButton)!=0) {
    pprintf(TextOutputStream,"got head back button press\n");
  }
  if((sensor_data->button & LogButton)!=0 && !wrote_log) {
    if(LogControlStream!=NULL) {
      pprintf(TextOutputStream,"writing log\n");
      LogControl control;
      control.command = LogControl::WriteLog;
      LogControlStream->writeBinary((const uchar *)&control,(int)sizeof(control));
      wrote_log = true;
    } else {
      pprintf(TextOutputStream,"LogControlStream is null\n");
    }
  }
}