/* 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 "../Motion/Kinematics.h"
#include "../headers/Util.h"
#include "../Main/RobotMain.h"

#include <list>

#include "FollowBall.h"

using namespace Motion;
using namespace std;

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

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


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

  fsm.init(state_names,NumStates,CORRECTING,16,16);
  kin = Kinematics(Kinematics::ERS7);
  //lastpixelcnts = new list<float>();
}

FollowBall::~FollowBall()
{
}

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

bool FollowBall::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 FollowBall::setupEventMgr(){
  return true;
};

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

  return true;
}

double FollowBall::operator()(ulong time,Motion::MotionCommand *command)
{
  bool done=false;
  
  float servo_error_x = vision->findVisualServoingErrorX();
  float servo_error_y = vision->findVisualServoingErrorY();
  float pixel_cnt = vision->findPixelCnt();

  cout << "doing avg count" << endl;
  lastpixelcnts.push_front(pixel_cnt);
  if(lastpixelcnts.size() > 30)
    {
      lastpixelcnts.pop_back();
    }
  float sum = 0.0;
  for(list<float>::iterator i = lastpixelcnts.begin(); i != lastpixelcnts.end(); i++)
    {
      sum += *i;
    }
  float avgpixelcnt = sum /= lastpixelcnts.size();
  
  pixel_cnt = avgpixelcnt;

  cout << "avgpixelcnt over last 30 iterations = " << avgpixelcnt << endl;

  		//cout << "Servo error is " << servo_error_x << " on X, " << servo_error_y << " on Y" << endl;
 		 cout << "Pixel count is " << pixel_cnt << endl;

  fsm.startLoop(time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){      
      case CORRECTING:
        {
	  double rangeX = kin.robot.head_pan_max - 
	    kin.robot.head_pan_min;
	  double rangeY = kin.robot.head_tilt2_max - 
	    kin.robot.head_tilt2_min;
	  double centerMinX = kin.robot.head_pan_max +
	    kin.robot.head_pan_min - (.15 * rangeX);
	  double centerMaxX = kin.robot.head_pan_max +
	    kin.robot.head_pan_min + (.15 * rangeX);
		
          if(fabs(servo_error_x) < .05 && fabs(servo_error_y) < .05 
	     && pixel_cnt > 10000 && command->head_pan > centerMinX && command->head_pan < centerMaxX)
            TRANS_CONT(fsm,SASTISFIED,5,"ErrorOk");
	  
	  if(servo_error_x == -1.0 && servo_error_y == -1.0)
	    TRANS_CONT(fsm, WANDERING, 6, "LostBall");
	  
	  //cout << "Correcting" << endl;
	  
          float vx = 0.0, va = 0.0;
	  
	  double displacementX = rangeX * .03 * fabs(servo_error_x * servo_error_x);
	  displacementX *= servo_error_x > 0 ? 1 : -1;
	  double displacementY = rangeY * .07 * fabs(servo_error_y * servo_error_y);
	  displacementY *= servo_error_y > 0 ? 1 : -1;
	  
	  command->head_cmd = HEAD_ANGLES;
	  command->head_pan += displacementX;
	  if(command->head_pan < kin.robot.head_pan_min)
	    command->head_pan = kin.robot.head_pan_min;
	  if(command->head_pan > kin.robot.head_pan_max)
	    command->head_pan = kin.robot.head_pan_max;
	  
	  command->head_tilt2 += displacementY; 
	  if(command->head_tilt2 < kin.robot.head_tilt2_min)
	    command->head_tilt2 = kin.robot.head_tilt2_min;
	  if(command->head_tilt2 > kin.robot.head_tilt2_max)
	    command->head_tilt2 = kin.robot.head_tilt2_max;
	  

	  
	  if(pixel_cnt < 1750)
	    vx = (((2000 - pixel_cnt) / 2000) * MaxDX);
	  
	  if(command->head_pan < centerMinX && displacementX < (rangeX * .2))
            va = -.4 * MaxDA;
          if(command->head_pan > centerMaxX && displacementX > -(rangeX * .2))
            va = .4 * MaxDA;
	  
	  
	  if(va != 0.0 || vx != 0.0)
	    {
	      command->motion_cmd = MOTION_WALK_TROT;
	      command->vx = vx;
	      command->vy = 0.0;
	      command->va = va;
	    }
	  else
	    {
	      
	      command->motion_cmd = MOTION_STAND_NEUTRAL;
	      command->vy = 0.0;
	      command->vx = 0.0;
	      command->va = 0.0;
	    }


	  command->head_roll = 0.0;
	  command->head_tilt = kin.robot.head_tilt_max; //RAD(20.0);
	  
          done = true;
	  
        }
        break;
      case SASTISFIED:
        {


          if(fabs(servo_error_x) > .2 || fabs(servo_error_y) > .2){
            TRANS_CONT(fsm,CORRECTING,5,"ErrorTooBig");
          }
	  if(servo_error_x == -1.0 && servo_error_y == -1.0)
	    TRANS_CONT(fsm, WANDERING, 6, "LostBall");
          //cout << "Satisfied" << endl;
          command->motion_cmd = MOTION_STAND_NEUTRAL;
          command->vy = 0.0;
          command->vx = 0.0;
          command->va = 0.0;
	  
          done = true;
        }
        break;
case WANDERING:
  {
    if(servo_error_x != -1.0 && servo_error_y != -1.0)
      TRANS_CONT(fsm, CORRECTING, 5, "FoundBall");
    
	//cout << "Wandering" << endl;
    command->motion_cmd = MOTION_WALK_TROT;
    command->vy = 0.0;
    command->vx = 0.0;
    command->va = 0.4 * MaxDA;
    done = true;
  }
  break;
    }
  }
  if(fsm.error!=0){
    fsm.handleErr(command);
  }
  fsm.endLoop();
  return 1.0;
}

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

    cache_tracker.cacheUpdated(time);
  }

  return command;
}

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