/*
 * copyright (c) 2013, yujin robot.
 * all rights reserved.
 *
 * redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * neither the name of yujin robot nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * this software is provided by the copyright holders and contributors "as is"
 * and any express or implied warranties, including, but not limited to, the
 * implied warranties of merchantability and fitness for a particular purpose
 * are disclaimed. in no event shall the copyright owner or contributors be
 * liable for any direct, indirect, incidental, special, exemplary, or
 * consequential damages (including, but not limited to, procurement of
 * substitute goods or services; loss of use, data, or profits; or business
 * interruption) however caused and on any theory of liability, whether in
 * contract, strict liability, or tort (including negligence or otherwise)
 * arising in any way out of the use of this software, even if advised of the
 * possibility of such damage.
 */
/**
 * @file /kobuki_driver/src/driver/dock_drive.cpp
 *
 **/

/*****************************************************************************
** includes
*****************************************************************************/

#include "kobuki_dock_drive/dock_drive.h"

/*****************************************************************************
** defines
*****************************************************************************/

#define sign(x) (x>0?+1:x<0?-1:0)
#define stringfy(x) #x
#define setState(x) {state=x;}
#define setStateVel(x,v,w) {setState(x);setVel(v,w);}

#pragma GCC diagnostic ignored "-Wunused-variable"

/*****************************************************************************
** Implementation
*****************************************************************************/
DockDrive::DockDrive() :
  is_enabled(false), can_run(false)
  , state(RobotDockingState::IDLE), state_str("IDLE")
  , vx(0.0), wz(0.0)
  , signal_window(20)
  , bump_remainder(0)
  , dock_stabilizer(0)
  , dock_detector(0)
  , rotated(0.0)
  , min_abs_v(0.01)
  , min_abs_w(0.1)
  , ROBOT_STATE_STR(13)
{
  // Debug messages
  ROBOT_STATE_STR[0] = "IDLE";
  ROBOT_STATE_STR[1] = "DONE";
  ROBOT_STATE_STR[2] = "DOCKED_IN";
  ROBOT_STATE_STR[3] = "BUMPED_DOCK";
  ROBOT_STATE_STR[4] = "BUMPED";
  ROBOT_STATE_STR[5] = "SCAN";
  ROBOT_STATE_STR[6] = "FIND_STREAM";
  ROBOT_STATE_STR[7] = "GET_STREAM";
  ROBOT_STATE_STR[8] = "ALIGNED";
  ROBOT_STATE_STR[9] = "ALIGNED_FAR";
  ROBOT_STATE_STR[10] = "ALIGNED_NEAR";
  ROBOT_STATE_STR[11] = "UNKNOWN";
  ROBOT_STATE_STR[12] = "LOST";
}

DockDrive::~DockDrive(){;}

void DockDrive::setVel(double v, double w)
{
  vx = sign(v) * std::max(std::abs(v), min_abs_v);
  wz = sign(w) * std::max(std::abs(w), min_abs_w);
}

/**
 * @brief Updates the odometry from firmware stamps and encoders.
 *
 * Really horrible - could do with an overhaul.
 *
 * @param dock_ir signal
 * @param bumper sensor
 * @param charger sensor
 * @param current pose
 */
void DockDrive::update(const std::vector<uint8_t> &signal
                , const unsigned char &bumper
                , const unsigned char &charger
                , const CPose & pose) {

  CPose pose_update;
  std::vector<uint8_t> signal_filt(signal.size(), 0);
  std::string debug_str;

  // process bumper and charger event first
  // docking algorithm terminates here
  if(bumper || charger) {
    processBumpChargeEvent(bumper, charger);
  }
  else {
    computePoseUpdate(pose_update, pose);
    filterIRSensor(signal_filt, signal);
    updateVelocity(signal_filt, pose_update, debug_str);
  }

  //velocityCommands(vx, wz);

  // for easy debugging
  generateDebugMessage(signal_filt, bumper, charger, pose_update, debug_str);

  return;
}

/**
 * @brief compute pose update from previouse pose and current pose
 *
 * @param pose update. this variable get filled after this function
 * @param pose - current pose
 **/
void DockDrive::computePoseUpdate(CPose& pose_update, const CPose& pose)
{
  double dx = pose.x - pose_priv.x;
  double dy = pose.y - pose_priv.y;
  pose_update.x = std::sqrt( dx*dx + dy*dy );
  pose_update.heading = pose.heading - pose_priv.heading;
  //std::cout << pose_diff << "=" << pose << "-" << pose_priv << " | " << pose_update << std::endl;
  pose_priv = pose;

}


/**
 * @breif pushing into signal into signal window. and go through the signal window to find what has detected
 *
 * @param signal_filt - this get filled out after the function.
 * @param signal - the raw data from robot
 **/

void DockDrive::filterIRSensor(std::vector<uint8_t>& signal_filt,const std::vector<uint8_t> &signal)
{
  //dock_ir signals filtering
  past_signals.push_back(signal);
  while (past_signals.size() > signal_window) {
    past_signals.erase( past_signals.begin(), past_signals.begin() + past_signals.size() - signal_window);
  }

  for ( unsigned int i = 0; i < past_signals.size(); i++) {
    if (signal_filt.size() != past_signals[i].size())
      continue;
    for (unsigned int j = 0; j < signal_filt.size(); j++)
      signal_filt[j] |= past_signals[i][j];
  }
}


void DockDrive::velocityCommands(const double &vx_, const double &wz_)
{
  // vx: in m/s
  // wz: in rad/s
  vx = vx_;
  wz = wz_;
}

/****************************************************
 * @brief process bumper and charge event. If robot is charging, terminates auto dokcing process. If it bumps something, Set the next state as bumped and go backward
 *
 * @bumper - indicates whether bumper has pressed
 * @charger - indicates whether robot is charging
 *
 ****************************************************/
void DockDrive::processBumpChargeEvent(const unsigned char& bumper, const unsigned char& charger) {
  RobotDockingState::State new_state;
  if(charger && bumper) {
    new_state = RobotDockingState::BUMPED_DOCK;
    setStateVel(new_state, -0.01, 0.0);
  }
  else if(charger) {
    if(dock_stabilizer++ == 0) {
      new_state = RobotDockingState::DOCKED_IN;
      setStateVel(new_state, 0.0, 0.0);
    }
    else if(dock_stabilizer > 20) {
      dock_stabilizer = 0;
      is_enabled = false;
      can_run = false;
      new_state = RobotDockingState::DONE;
      setStateVel(new_state, 0.0, 0.0);
    }
    else {
      new_state = RobotDockingState::DOCKED_IN;
      setStateVel(new_state, 0.0, 0.0);
    }
  }
  else if(bumper) {
    new_state = RobotDockingState::BUMPED;
    setStateVel(new_state, -0.05, 0.0);
    bump_remainder = 0;
  }
  state_str = ROBOT_STATE_STR[new_state];
}

/*************************
 * @breif processing. algorithms; transforma to velocity command
 *
 * @param dock_ir signal
 * @param bumper sensor
 * @param charger sensor
 * @param pose_update
 *
 *************************/
void DockDrive::updateVelocity(const std::vector<uint8_t>& signal_filt, const CPose& pose_update, std::string& debug_str)
{
  std::ostringstream oss;
  RobotDockingState::State current_state, new_state;
  double new_vx = 0.0;
  double new_wz = 0.0;

  // determine the current state based on ir and the previous state
  // common transition. idle -> scan -> find_stream -> get_stream -> scan -> aligned_far -> aligned_near -> docked_in -> done

  current_state = new_state = state;
  switch((unsigned int)current_state) {
    case RobotDockingState::IDLE:
      idle(new_state, new_vx, new_wz);
      break;
    case RobotDockingState::SCAN:
      scan(new_state, new_vx, new_wz, signal_filt, pose_update, debug_str);
      break;
    case RobotDockingState::FIND_STREAM:
      find_stream(new_state, new_vx, new_wz, signal_filt);
      break;
    case RobotDockingState::GET_STREAM:
      get_stream(new_state, new_vx, new_wz, signal_filt);
      break;
    case RobotDockingState::ALIGNED:
    case RobotDockingState::ALIGNED_FAR:
    case RobotDockingState::ALIGNED_NEAR:
      aligned(new_state, new_vx, new_wz, signal_filt, debug_str);
      break;
    case RobotDockingState::BUMPED:
      bumped(new_state, new_vx, new_wz, bump_remainder);
      break;
    default:
      oss << "Wrong state : " << current_state;
      debug_str = oss.str();
      break;
  }

  setStateVel(new_state, new_vx, new_wz);
  state_str = ROBOT_STATE_STR[new_state];
}

/*************************
 * @breif Check if any ir sees the given state signal from dock station
 *
 * @param filtered signal
 * @param dock ir state
 *
 * @ret true or false
 *************************/
bool DockDrive::validateSignal(const std::vector<uint8_t>& signal_filt, const unsigned int state)
{
  unsigned int i;
  for(i = 0; i < signal_filt.size(); i++)
  {
    if(signal_filt[i] & state)
      return true;
  }
  return false;
}
