//
// Created by Johannes Mey on 12/10/21.
//

#include "../include/PandaUtil.h"

#include "franka_msgs/ErrorRecoveryAction.h"
#include "franka_msgs/FrankaState.h"
#include "franka_msgs/Errors.h"
#include <actionlib/client/simple_action_client.h>

bool PandaUtil::recoverFromErrors()
{
  ros::NodeHandle n;
  bool hasErrors;
  bool receivedState = false;
  ros::Subscriber state_subscription = n.subscribe<franka_msgs::FrankaState>(
      "/franka_state_controller/franka_states", 1000, [&receivedState, &hasErrors, &state_subscription](auto& s) {
        receivedState = true;
        uint8_t robot_mode = s->robot_mode;
        hasErrors = robot_mode == franka_msgs::FrankaState::ROBOT_MODE_OTHER ||
                    robot_mode == franka_msgs::FrankaState::ROBOT_MODE_REFLEX;
        state_subscription.shutdown();
      });

  for (int maxSpins = 10; !receivedState && maxSpins && !state_subscription.getNumPublishers(); maxSpins--)
  {
    ros::Duration(.5).sleep();
  }

  if (!receivedState && !state_subscription.getNumPublishers())
  {
    ROS_WARN_STREAM("No robot publishes a state. Aborting error recovery.");
    return false;
  }

  for (int maxSpins = 10; maxSpins && !receivedState; maxSpins--)
  {
    ros::Duration(.05).sleep();
    ros::spinOnce();
  }
  if (!receivedState)
  {
    ROS_WARN_STREAM("Unable to obtain state from robot. Aborting error recovery.");
    return false;
  }

  if (!hasErrors)
  {
    ROS_WARN_STREAM("Robot has no errors. Skipping error recovery.");
    return false;
  }

  actionlib::SimpleActionClient<franka_msgs::ErrorRecoveryAction> ac("/franka_control/error_recovery", true);
  if (!ac.waitForServer(ros::Duration(10.0)))
  {
    ROS_ERROR("Panda error recovery failed (action server not available).");
    return false;
  }
  franka_msgs::ErrorRecoveryGoal goal;
  ac.sendGoal(goal);
  if (ac.waitForResult(ros::Duration(5.0)))
  {
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO_STREAM("Panda error recovery finished: " << state.toString());
    return state == actionlib::SimpleClientGoalState::SUCCEEDED;
  }
  else
  {
    ROS_ERROR("Panda error recovery failed (with timeout).");
    return false;
  }
}