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

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

char const * const Camera::state_names[Camera::NumStates] = {
  "JOYSTICK",
  "WAIT_FOR_PIC",
  "TOOK_PIC",
  "WRITE_LOG",
  "WAIT_FOR_WRITE"
};

Camera::Camera()
{
  char design[256];
  OPENR::GetRobotDesign(design);
  if(strcmp(design,"ERS-210")==0){
    kin = Kinematics(Kinematics::ERS210);
  }else{
    kin = Kinematics(Kinematics::ERS7);
  }

  fsm.init(state_names,NumStates,JOYSTICK,16,16);

  fs_id = ~0;
  fs = NULL;

  sd_id = ~0;
  sd = NULL;
}

Camera::~Camera()
{
}

void
Camera::reset(ulong time) {

  fsm.setState(JOYSTICK,0,"Reset",time);
}

double
Camera::operator()(FeatureSet *FS, 
		      SensorData *SD,
		      MotionCommand *command) {
  
  bool done=false;

  command->tail_cmd = TAIL_FOLLOW;
      
  double tilt_norm, pan_norm; // range from [-1,1]
  
  tilt_norm =  
    kin.calcTailTiltNorm(SD->getFrame(0)->tailAngles[TailTiltReading]);
  pan_norm  = 
    kin.calcTailPanNorm(SD->getFrame(0)->tailAngles[TailPanReading ]);
  
  command->head_cmd = HEAD_ANGLES;
  command->head_tilt = 
    .5*(kin.robot.head_tilt_min + kin.robot.head_tilt_max
        + (kin.robot.head_tilt_max- kin.robot.head_tilt_min)*tilt_norm);
  
  command->head_pan  =
    .5*(kin.robot.head_pan_min  + kin.robot.head_pan_max  +
	(kin.robot.head_pan_max - kin.robot.head_pan_min )*-pan_norm );

  command->head_roll = 0.0;
  command->head_tilt2 = RAD(20.0);
  
  fsm.startLoop(FS->current_time);
  while(!done && fsm.error==0){
    switch(fsm.getState()){
      case JOYSTICK:
        if((SD->getFrame(0)->button & LogButton)!=0) {
          TRANS_CONT(fsm, WRITE_LOG, 5, "LogButtonDown");
        }

        if((SD->getFrame(0)->button & HeadFrontButton)!=0) {
          TRANS_CONT(fsm, WAIT_FOR_PIC, 6, "PictureButtonDown");
        }
      
        // Create a motion command
        command->motion_cmd = MOTION_STAND_NEUTRAL;
        command->addBinLEDs(LED_MIDDLE_LEFT | LED_MIDDLE_RIGHT); // pretty lights

        // We did not transition to a new state, so set the done flag.
        done = true;
        break;
 
      case WAIT_FOR_PIC:
        if((SD->getFrame(0)->button & HeadFrontButton)==0)
          TRANS_CONT(fsm, JOYSTICK, 5, "PictureButtonUp");

        if(fsm.timeInState() > SecToTime(0.50)) {

          // Log RLE and raw vision frames
          VisionInterface::SendRLEImage(RobotMain::GetObject().getVision());
          VisionInterface::SendRawImage(RobotMain::GetObject().getVision());

          TRANS_CONT(fsm, TOOK_PIC, 6, "TookPicture");
        }

        command->motion_cmd = MOTION_STAND_NEUTRAL;
        command->addBinLEDs(LED_NONE);

        done = true;
        break;
     
      case TOOK_PIC:
        if((SD->getFrame(0)->button & HeadFrontButton)==0)
          TRANS_CONT(fsm, JOYSTICK, 5, "PictureButtonUp");

        command->motion_cmd = MOTION_STAND_NEUTRAL;
        command->addBinLEDs(LED_ALL);

        done = true;
        break;
     
      case WRITE_LOG:    
        if(LogControlStream!=NULL) {
          pprintf(TextOutputStream,"writing log\n");
          LogControl control;
          control.command = LogControl::WriteLog;
          LogControlStream->writeBinary((const uchar *)&control,
                                        (int)sizeof(control));
        }

        TRANS_CONT(fsm, WAIT_FOR_WRITE, 1, "LogWriteTriggered");
        done = true;
        break;

      case WAIT_FOR_WRITE:
        command->motion_cmd = MOTION_STAND_NEUTRAL;
        command->addBinLEDs(LED_LOWER_LEFT | LED_MIDDLE_RIGHT | LED_UPPER_LEFT);
        command->head_cmd = HEAD_SCAN_BALL;

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

bool Camera::initConnections()
{
  EventManager *event_mgr;
  EventProcessor *fs_ep;
  EventProcessor *sd_ep;

  event_mgr = EventManager::getManager();
  fs_id = event_mgr->getEventProcessorId("FeatureSet");
  sd_id = event_mgr->getEventProcessorId("SensorData");

  fs_ep = event_mgr->listenEventProcessor(beh_name,fs_id);
  sd_ep = event_mgr->listenEventProcessor(beh_name, sd_id);

  fs = (FeatureSet *)(fs_ep);
  sd = (SensorData *)(sd_ep);
  return true;
}

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

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

const MotionCommand *Camera::get(ulong time)
{
  FeatureSet *lfs=NULL; // local feature set
  SensorData *lsd = NULL;

  mzero(out_command);

  if(fs!=NULL && sd!=NULL){
    lfs = fs->get(time);
    lsd = sd->get(time);

    (*this)(lfs, lsd, &out_command);
  }
  
  return &out_command;
}

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