/* 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 "../Motion/MotionInterface.h"
#include "../Vision/Vision.h"

#include "VisualServo.h"

using namespace Motion;

char const * const VisualServo::beh_name = "VisualServo";

char const * const VisualServo::state_names[VisualServo::NumStates] = {
  "CORRECTING",
  "SASTIFIED"
};


VisualServo::VisualServo()
{
  command = new MotionCommand;
  mzero(*command);

  fsm.init(state_names,NumStates,CORRECTING,16,16);
}

VisualServo::~VisualServo()
{
}

void
VisualServo::reset(ulong time)
{
  fsm.setState(CORRECTING,0,"Reset",time);
}

bool VisualServo::initConnections()
{
  EventManager *event_mgr;
  event_mgr = EventManager::getManager();

  vis_id = event_mgr->getEventProcessorId(Vision::name);
  vision = dynamic_cast<Vision *>(event_mgr->listenEventProcessor(beh_name,vis_id));

  return (vision!=NULL);
}

bool VisualServo::setupEventMgr(){
  return true;
};

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

  return true;
}

double VisualServo::operator()(ulong time,Motion::MotionCommand *command)
{
  bool done=false;

  // calculate the positional error in the image
  float servo_error;
  servo_error = vision->findVisualServoingError();

  // look at a point a bit ahead of us and to the left
  command->head_cmd = HEAD_LOOKAT;
  command->head_lookat.set(150.0,200.0,0.0);

  fsm.startLoop(time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){      
      case CORRECTING:
        {
          float vy,va;
          
          if(fabs(servo_error) < .02){
            TRANS_CONT(fsm,SASTISFIED,5,"ErrorOk");
          }

          // move at a speed proportional to the amount of error
          // this is a proportional controller with a gain of 1.0
          vy =  servo_error * MaxDY;
          // correct for tendency of motions to twist while moving sideways
          va =  servo_error * 0.1;

          command->motion_cmd = MOTION_WALK_TROT;
          command->vx = 0.0;
          command->vy = vy;
          command->va = va;

          done = true;
        }
        break;
      case SASTISFIED:
        {
          if(fabs(servo_error) > .2){
            TRANS_CONT(fsm,CORRECTING,5,"ErrorTooBig");
          }
          
          command->motion_cmd = MOTION_STAND_NEUTRAL;

          done = true;
        }
        break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();
  
  return 1.0;
}

const Motion::MotionCommand *VisualServo::get(ulong time)
{
  if(cache_tracker.shouldUpdate(time)){
    // update info
    (*this)(time,command);

    cache_tracker.cacheUpdated(time);
  }

  return command;
}

REGISTER_EVENT_PROCESSOR(VisualServo,VisualServo::beh_name,VisualServo::create);