robotrt-action-core 0.1.0-beta.2

use core_types::{ActionGoalId, ErrorCode, ErrorDomain, RtError, Timestamp};

use crate::message::{ActionFeedback, ActionResult, ActionSchema, GoalAck, GoalStatus};

use super::BasicActionServer;
use crate::client_server::ActionServer;

impl<G, F, R> ActionServer<G, F, R> for BasicActionServer<G, F, R>
where
    G: Send + 'static,
    F: Send + 'static,
    R: Send + 'static,
{
    fn action_name(&self) -> &str {
        &self.action_name
    }

    fn schema(&self) -> &ActionSchema {
        &self.schema
    }

    fn recv_goal(&mut self) -> Result<Option<(ActionGoalId, G)>, RtError> {
        if self.closed {
            return Err(RtError::new(
                ErrorCode::InvalidState,
                ErrorDomain::Action,
                false,
                "server is closed",
            ));
        }
        if let Some(ch) = &self.channel {
            return Ok(ch.goals.lock().unwrap().pop_front());
        }
        Ok(self.injected.pop_front())
    }

    fn accept_goal(&mut self, goal_id: ActionGoalId) -> Result<(), RtError> {
        let ack = GoalAck {
            accepted: true,
            reason: None,
        };
        if let Some(ch) = &self.channel {
            ch.acks.lock().unwrap().insert(goal_id.0, ack);
            ch.statuses
                .lock()
                .unwrap()
                .insert(goal_id.0, GoalStatus::Executing);
            ch.heartbeats
                .lock()
                .unwrap()
                .insert(goal_id.0, Timestamp::now());
        }
        Ok(())
    }

    fn reject_goal(
        &mut self,
        goal_id: ActionGoalId,
        reason: impl Into<String>,
    ) -> Result<(), RtError> {
        let ack = GoalAck {
            accepted: false,
            reason: Some(reason.into()),
        };
        if let Some(ch) = &self.channel {
            ch.acks.lock().unwrap().insert(goal_id.0, ack);
            ch.statuses
                .lock()
                .unwrap()
                .insert(goal_id.0, GoalStatus::Rejected);
            ch.heartbeats.lock().unwrap().remove(&goal_id.0);
            ch.results.lock().unwrap().insert(
                goal_id.0,
                ActionResult {
                    status: GoalStatus::Rejected,
                    value: None,
                    error: None,
                },
            );
            ch.result_timestamps
                .lock()
                .unwrap()
                .insert(goal_id.0, Timestamp::now());
        }
        Ok(())
    }

    fn publish_feedback(&mut self, goal_id: ActionGoalId, feedback: F) -> Result<(), RtError> {
        if self.closed {
            return Err(RtError::new(
                ErrorCode::InvalidState,
                ErrorDomain::Action,
                false,
                "server is closed",
            ));
        }
        let fb = ActionFeedback {
            timestamp: Timestamp::now(),
            value: feedback,
        };
        if let Some(ch) = &self.channel {
            ch.feedbacks
                .lock()
                .unwrap()
                .entry(goal_id.0)
                .or_default()
                .push_back(fb);
            ch.heartbeats
                .lock()
                .unwrap()
                .insert(goal_id.0, Timestamp::now());
            ch.feedback_timestamps
                .lock()
                .unwrap()
                .insert(goal_id.0, Timestamp::now());
        }
        Ok(())
    }

    fn heartbeat(&mut self, goal_id: ActionGoalId) -> Result<(), RtError> {
        if self.closed {
            return Err(RtError::new(
                ErrorCode::InvalidState,
                ErrorDomain::Action,
                false,
                "server is closed",
            ));
        }
        if let Some(ch) = &self.channel {
            ch.heartbeats
                .lock()
                .unwrap()
                .insert(goal_id.0, Timestamp::now());
        }
        Ok(())
    }

    fn succeed(&mut self, goal_id: ActionGoalId, result: R) -> Result<(), RtError> {
        if let Some(ch) = &self.channel {
            ch.statuses
                .lock()
                .unwrap()
                .insert(goal_id.0, GoalStatus::Succeeded);
            ch.heartbeats.lock().unwrap().remove(&goal_id.0);
            ch.results.lock().unwrap().insert(
                goal_id.0,
                ActionResult {
                    status: GoalStatus::Succeeded,
                    value: Some(result),
                    error: None,
                },
            );
            ch.result_timestamps
                .lock()
                .unwrap()
                .insert(goal_id.0, Timestamp::now());
        }
        Ok(())
    }

    fn fail(&mut self, goal_id: ActionGoalId, reason: impl Into<String>) -> Result<(), RtError> {
        if let Some(ch) = &self.channel {
            ch.statuses
                .lock()
                .unwrap()
                .insert(goal_id.0, GoalStatus::Failed);
            ch.heartbeats.lock().unwrap().remove(&goal_id.0);
            ch.results.lock().unwrap().insert(
                goal_id.0,
                ActionResult {
                    status: GoalStatus::Failed,
                    value: None,
                    error: Some(reason.into()),
                },
            );
            ch.result_timestamps
                .lock()
                .unwrap()
                .insert(goal_id.0, Timestamp::now());
        }
        Ok(())
    }

    fn poll_cancel_request(&mut self) -> Option<ActionGoalId> {
        let goal_id = self.channel.as_ref()?.cancels.lock().unwrap().pop_front()?;
        if let Some(ch) = &self.channel {
            ch.statuses
                .lock()
                .unwrap()
                .insert(goal_id.0, GoalStatus::Canceling);
            ch.heartbeats
                .lock()
                .unwrap()
                .insert(goal_id.0, Timestamp::now());
        }
        Some(goal_id)
    }

    fn confirm_cancel(&mut self, goal_id: ActionGoalId) -> Result<(), RtError> {
        if let Some(ch) = &self.channel {
            ch.statuses
                .lock()
                .unwrap()
                .insert(goal_id.0, GoalStatus::Canceled);
            ch.heartbeats.lock().unwrap().remove(&goal_id.0);
            ch.results.lock().unwrap().insert(
                goal_id.0,
                ActionResult {
                    status: GoalStatus::Canceled,
                    value: None,
                    error: None,
                },
            );
            ch.result_timestamps
                .lock()
                .unwrap()
                .insert(goal_id.0, Timestamp::now());
        }
        Ok(())
    }

    fn close(&mut self) -> Result<(), RtError> {
        self.closed = true;
        Ok(())
    }
}