agent-tk 0.4.0

//! TST Simulation

use futures::future::FutureExt;
use geo::{Area, Centroid};

use crate::definitions::agent as definitions_agent;
use crate::definitions::tst as definitions_tst;
use crate::definitions::tst::SpeedCompute;
use crate::execution::tst as execution_tst;
use crate::execution::tst::TreeExecutorTrait;
use crate::{Error, Result};

mod utils;

//  ____  _                 _       _   _             ____                 _ _
// / ___|(_)_ __ ___  _   _| | __ _| |_(_) ___  _ __ |  _ \ ___  ___ _   _| | |_
// \___ \| | '_ ` _ \| | | | |/ _` | __| |/ _ \| '_ \| |_) / _ \/ __| | | | | __|
//  ___) | | | | | | | |_| | | (_| | |_| | (_) | | | |  _ |  __/\__ \ |_| | | |_
// |____/|_|_| |_| |_|\__,_|_|\__,_|\__|_|\___/|_| |_|_| \_\___||___/\__,_|_|\__|

/// Result of the simulation, with the estimated cost and final states.
#[derive(Default, Clone, serde::Serialize, serde::Deserialize)]
pub struct SimulationResult
{
  estimated_cost: f32,
  final_states: crate::states::States,
}

impl SimulationResult
{
  /// Return the estimated cost
  pub fn get_estimated_cost(&self) -> f32
  {
    self.estimated_cost
  }
  /// Get the final states, after executing the TST
  pub fn get_final_states(&self) -> &crate::states::States
  {
    &self.final_states
  }
}

//  ____  _                 _       _   _             _____                     _   _
// / ___|(_)_ __ ___  _   _| | __ _| |_(_) ___  _ __ | ____|_  _____  ___ _   _| |_(_) ___  _ __
// \___ \| | '_ ` _ \| | | | |/ _` | __| |/ _ \| '_ \|  _| \ \/ / _ \/ __| | | | __| |/ _ \| '_ \
//  ___) | | | | | | | |_| | | (_| | |_| | (_) | | | | |___ >  <  __/ (__| |_| | |_| | (_) | | | |
// |____/|_|_| |_| |_|\__,_|_|\__,_|\__|_|\___/|_| |_|_____/_/\_\___|\___|\__,_|\__|_|\___/|_| |_|

/// Simulate the execution of a task, to compute the feasibility and the associated cost.
pub async fn simulate_execution(
  initial_states: crate::states::States,
  capabilities: definitions_agent::capabilities::Capabilities,
  tst_node: definitions_tst::Node,
) -> Result<SimulationResult>
{
  let agent = Agent {
    capabilities,
    ..Default::default()
  };
  agent.simulation_result.lock().unwrap().final_states = initial_states;
  let executor =
    execution_tst::DefaultTreeExecutor::<MoveToExecutor, SearchAreaExecutor>::from_agent(&agent);
  let r = executor.execute(tst_node);
  let res = r.await;
  if let Err(e) = res
  {
    Err(e)
  }
  else
  {
    Ok(agent.simulation_result.lock().unwrap().clone())
  }
}

// Simulation executor

// Structures

#[derive(Clone, Default)]
struct Agent
{
  capabilities: definitions_agent::capabilities::Capabilities,
  simulation_result: std::sync::Arc<std::sync::Mutex<SimulationResult>>,
}

impl execution_tst::Agent for Agent {}

struct MoveToExecutor
{
  agent: Agent,
}

impl execution_tst::CreatableFromAgent<Agent, MoveToExecutor> for MoveToExecutor
{
  fn from_agent(agent: &Agent) -> MoveToExecutor
  {
    MoveToExecutor {
      agent: agent.clone(),
    }
  }
}

impl execution_tst::Executor<definitions_tst::MoveTo> for MoveToExecutor
{
  fn execute<'b>(
    &self,
    _executors: &'b impl execution_tst::Executors,
    t: definitions_tst::MoveTo,
  ) -> execution_tst::ExecutionResult<'b>
  {
    let simulation_result = self.agent.simulation_result.clone();
    let capabilities = self.agent.capabilities.clone();
    async move {
      let mut simulation_result = simulation_result.lock().unwrap();
      let current_position = simulation_result.final_states.get_position()?;

      let move_to_capability = capabilities
        .get_move()
        .map_err(|e| crate::Error::ExecutionFailed(Box::new(e)))?;
      let vel = t
        .params
        .speed
        .compute_velocity(move_to_capability.get_max_velocity());
      simulation_result.estimated_cost += utils::haversine_distance(
        current_position.longitude,
        current_position.latitude,
        t.params.waypoint.longitude,
        t.params.waypoint.latitude,
      ) as f32
        * vel;

      simulation_result
        .final_states
        .update_state(crate::states::Position {
          latitude: t.params.waypoint.latitude,
          longitude: t.params.waypoint.longitude,
        })?;
      Ok::<(), Error>(())
    }
    .boxed()
  }
}

struct SearchAreaExecutor
{
  agent: Agent,
}

impl execution_tst::CreatableFromAgent<Agent, SearchAreaExecutor> for SearchAreaExecutor
{
  fn from_agent(agent: &Agent) -> SearchAreaExecutor
  {
    SearchAreaExecutor {
      agent: agent.clone(),
    }
  }
}

impl execution_tst::Executor<definitions_tst::SearchArea> for SearchAreaExecutor
{
  fn execute<'b>(
    &self,
    _executors: &'b impl execution_tst::Executors,
    t: definitions_tst::SearchArea,
  ) -> execution_tst::ExecutionResult<'b>
  {
    let simulation_result = self.agent.simulation_result.clone();
    let capabilities = self.agent.capabilities.clone();

    async move {
      let mut simulation_result = simulation_result.lock().unwrap();
      let current_position = simulation_result.final_states.get_position()?;

      let move_to_capability = capabilities
        .get_move()
        .map_err(|e| crate::Error::ExecutionFailed(Box::new(e)))?;

      // Compute surface of polygon and center
      let mut polygon_points = Vec::<(f64, f64)>::new();

      for pt in t.params.area
      {
        polygon_points.push((pt.longitude, pt.latitude));
      }

      polygon_points.push(polygon_points[0]);

      let polygon = geo::Polygon::new(geo::LineString::from(polygon_points), vec![]);

      let center = polygon.centroid().unwrap();
      simulation_result.estimated_cost += utils::haversine_distance(
        current_position.longitude,
        current_position.latitude,
        center.x(),
        center.y(),
      ) as f32;
      simulation_result.estimated_cost +=
        polygon.unsigned_area() as f32 * move_to_capability.get_max_velocity();

      simulation_result
        .final_states
        .update_state(crate::states::Position {
          longitude: center.x(),
          latitude: center.y(),
        })?;
      Ok::<(), Error>(())
    }
    .boxed()
  }
}