/* 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 <vector>

#include "../headers/gvector.h"
#include "../Main/Events.h"
#include "../Main/SystemEvent.h"
#include "../Motion/MotionInterface.h"

#include "summarize.h"

// ===== MotionUpdateSummary implementation ================================ //

std::string MotionUpdateSummary::name("MotionUpdateSummary");

MotionUpdateSummary::MotionUpdateSummary()
{
  updates.reserve(5);

  update_source = NULL;

  last_queue_timestamp = 0UL;
  last_idx = 0;
}

MotionUpdateSummary::~MotionUpdateSummary()
{
}

bool MotionUpdateSummary::initConnections()
{
  EventManager *em;
  uint us_id;

  em = EventManager::getManager();
  us_id = em->getEventProcessorId(MotionUpdateSource::name);
  update_source = dynamic_cast<MotionUpdateSource *>(em->listenEventProcessor(name,us_id));

  return true;
}

bool MotionUpdateSummary::update(ulong time,const EventList *events)
{
  return true;
}

const std::vector<Motion::MotionLocalizationUpdate> *MotionUpdateSummary::get(ulong time)
{
  if(time!=timestamp){
    timestamp = time;

    updates.clear();

    Motion::MotionLocalizationUpdateQueue *queue;
    queue = update_source->get(time);
    if(queue==NULL){
      pprintf(TextOutputStream,"no update queue\n");
      return &updates;
    }

    bool done = false;
    while(!done){
      ulong buffer_timestamp=queue->buffer[last_idx].timestamp;
      if(last_queue_timestamp < buffer_timestamp && buffer_timestamp <= time){
        updates.push_back(queue->buffer[last_idx]);
        
        last_queue_timestamp=buffer_timestamp;      
        last_idx = (last_idx+1) % Motion::MotionLocalizationUpdateQueueBufferSize;
      } else {
        done=true;
      }
    }
  }

  return &updates;
}

REGISTER_EVENT_PROCESSOR(MotionUpdateSummary,MotionUpdateSummary::name,MotionUpdateSummary::create);

// ===== MotionUpdateHeadSummary implementation ================================ //

std::string MotionUpdateHeadSummary::name("MotionUpdateHeadSummary");

MotionUpdateHeadSummary::MotionUpdateHeadSummary()
{
  updates.reserve(5);

  update_source = NULL;

  last_queue_timestamp = 0UL;
  last_idx = 0;
}

MotionUpdateHeadSummary::~MotionUpdateHeadSummary()
{
}

bool MotionUpdateHeadSummary::initConnections()
{
  EventManager *em;
  uint us_id;

  em = EventManager::getManager();
  us_id = em->getEventProcessorId(MotionUpdateSource::name);
  update_source = dynamic_cast<MotionUpdateSource *>(em->listenEventProcessor(name,us_id));

  return true;
}

bool MotionUpdateHeadSummary::update(ulong time,const EventList *events)
{
  return true;
}

void MotionUpdateHeadSummary::modifyMoveToHeadCoords(Motion::MotionLocalizationUpdate *move) {
  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);
  
  local_neck_offset = local_neck_offset.rotate(rot_angle);
  local_neck_offset -= move->body.neck_offset;
  
  move->pos_delta += local_neck_offset;
}

const std::vector<Motion::MotionLocalizationUpdate> *MotionUpdateHeadSummary::get(ulong time)
{
  if(time!=timestamp){
    timestamp = time;

    updates.clear();

    Motion::MotionLocalizationUpdateQueue *queue;
    queue = update_source->get(time);

    bool done = false;
    while(!done){
      ulong buffer_timestamp=queue->buffer[last_idx].timestamp;
      if(last_queue_timestamp < buffer_timestamp && buffer_timestamp <= time){
        Motion::MotionLocalizationUpdate update;

        update = queue->buffer[last_idx];
        modifyMoveToHeadCoords(&update);
        updates.push_back(update);
        
        last_queue_timestamp=buffer_timestamp;      
        last_idx = (last_idx+1) % Motion::MotionLocalizationUpdateQueueBufferSize;
      } else {
        done=true;
      }
    }
  }

  return &updates;
}

REGISTER_EVENT_PROCESSOR(MotionUpdateHeadSummary,MotionUpdateHeadSummary::name,MotionUpdateHeadSummary::create);

// ===== MotionUpdateVelocitySummary implementation ================================ //

std::string MotionUpdateVelocitySummary::name("MotionUpdateVelocitySummary");

MotionUpdateVelocitySummary::MotionUpdateVelocitySummary()
{
  updateSource = NULL;

  lastQueueTimestamp = 0UL;
  lastIdx = 0;

  velocity.set(0.0,0.0,0.0);
}

MotionUpdateVelocitySummary::~MotionUpdateVelocitySummary()
{
}

bool MotionUpdateVelocitySummary::initConnections()
{
  EventManager *em;
  uint us_id;

  em = EventManager::getManager();
  us_id = em->getEventProcessorId(MotionUpdateSource::name);
  updateSource = dynamic_cast<MotionUpdateSource *>(em->listenEventProcessor(name,us_id));

  return true;
}

bool MotionUpdateVelocitySummary::update(ulong time,const EventList *events)
{
  cacheTracker.updateAvailable(time);

  return true;
}

vector3d MotionUpdateVelocitySummary::get(ulong time)
{
  int queue_size = Motion::MotionLocalizationUpdateQueueBufferSize;

  if(cacheTracker.shouldUpdate(time)){
    Motion::MotionLocalizationUpdateQueue *queue;
    queue = updateSource->get(time);
    if(queue==NULL){
      pprintf(TextOutputStream,"no update queue\n");
      return velocity;
    }

    bool done = false;
    while(!done){
      ulong buffer_timestamp = queue->buffer[lastIdx].timestamp;
      if(buffer_timestamp <= time){
        lastQueueTimestamp = buffer_timestamp;      
        lastIdx = (lastIdx+1) % queue_size;
      } else {
        done=true;
      }
    }

    Motion::MotionLocalizationUpdate *upd_new,*upd_old,*upd_cur;
    double time_diff;
    vector2d pos_delta;
    double angle_delta;

    upd_new = &queue->buffer[lastIdx];
    upd_old = &queue->buffer[(lastIdx + queue_size - 3) % queue_size];
    time_diff = TimeToSec(upd_new->timestamp - upd_old->timestamp);

    pos_delta.set(0.0,0.0);
    angle_delta = 0.0;

    int idx=(lastIdx + queue_size - 3) % queue_size;
    do{
      idx = (idx+1) % queue_size;

      upd_cur = &queue->buffer[idx];
      angle_delta += upd_cur->heading_delta.angle();
      pos_delta   += upd_cur->pos_delta.rotate(angle_delta);
    }while(idx!=lastIdx);

    if(time_diff > 0.0){
      velocity.x = pos_delta.x / time_diff;
      velocity.y = pos_delta.y / time_diff;
      velocity.z = angle_delta / time_diff;
        
      cacheTracker.cacheUpdated(time);
    }
  }


  return velocity;
}

REGISTER_EVENT_PROCESSOR(MotionUpdateVelocitySummary,MotionUpdateVelocitySummary::name,MotionUpdateVelocitySummary::create);