//! capabilities
//! ============
//!
//! Support for defining capabilities of an agent.

use std::any::TypeId;
use std::ops::{Deref, DerefMut};

use crate::{Error, Result};

// Structures

#[derive(Clone)]
pub struct Camera;

#[derive(Clone)]
pub struct Lidar;

#[derive(Clone)]
pub enum Sensor
{
  Camera(Camera),
  Lidar(Lidar),
}

#[derive(Clone)]
pub struct Gripper;

#[derive(Clone)]
pub struct Bucket
{
  capacity: i32,
}

#[derive(Clone)]
pub struct Dropper;

#[derive(Clone)]
pub enum Actuator
{
  Gripper(Gripper),
  Bucket(Bucket),
  Dropper(Dropper),
}

pub trait CapabilityTrait
{
  const IS_UNIQUE: bool;
  fn to_capability(self) -> Capability;
}

#[derive(Clone)]
pub enum Capability
{
  Move(Move),
  Acquire(Acquire),
  Grasp(Grasp),
}

macro_rules! define_capability {
  ($type:ident, unique) => {
    impl CapabilityTrait for $type
    {
      const IS_UNIQUE: bool = true;
      fn to_capability(self) -> Capability
      {
        return Capability::$type(self);
      }
    }
  };
  ($type:ident) => {
    impl CapabilityTrait for $type
    {
      const IS_UNIQUE: bool = false;
      fn to_capability(self) -> Capability
      {
        return Capability::$type(self);
      }
    }
  };
}

// Move
#[derive(Copy, Clone)]
pub struct Move
{
  max_velocity: f32,
}

impl Move
{
  pub fn new(max_velocity: f32) -> Move
  {
    Move {
      max_velocity: max_velocity,
    }
  }
  pub fn get_max_velocity(&self) -> f32
  {
    return self.max_velocity;
  }
}

define_capability!(Move, unique);

// Acquire

#[derive(Clone)]
pub struct Acquire
{
  sensor: Sensor,
}

impl Acquire
{
  pub fn new(sensor: Sensor) -> Acquire
  {
    Acquire { sensor: sensor }
  }
}

define_capability!(Acquire);

// Grasp
#[derive(Clone)]
pub struct Grasp
{
  actuator: Actuator,
}

define_capability!(Grasp);

// Capabilities

#[derive(Default, Clone)]
pub struct Capabilities(pub Vec<Capability>);

impl Deref for Capabilities
{
  type Target = Vec<Capability>;
  fn deref(&self) -> &Self::Target
  {
    &self.0
  }
}

impl DerefMut for Capabilities
{
  fn deref_mut(&mut self) -> &mut Self::Target
  {
    &mut self.0
  }
}

macro_rules! build_capability_getter {
  ($name:tt, $type:tt) => {
    pub fn $name(&self) -> Result<$type>
    {
      let res = self.0.iter().find_map(|x| match x
      {
        Capability::$type(p) => Some(p),
        _ => None,
      });
      if res.is_none()
      {
        Err(Error::UnknownCapability(std::any::type_name::<$type>()))
      }
      else
      {
        Ok(*res.unwrap())
      }
    }
  };
}

macro_rules! build_capabilities_getter {
  ($name:tt, $type:tt) => {
    pub fn $name(&self) -> Result<Vec<$type>>
    {
      let res = self
        .0
        .iter()
        .filter_map(|x| match x
        {
          Capability::$type(p) => Some(p),
          _ => None,
        })
        .map(|x| x.clone())
        .collect::<Vec<$type>>();
      if res.is_empty()
      {
        Err(Error::UnknownCapability(std::any::type_name::<$type>()))
      }
      else
      {
        Ok(res)
      }
    }
  };
}

impl Capabilities
{
  build_capability_getter!(get_move, Move);
  build_capabilities_getter!(get_acquire, Acquire);
  pub fn add_capability<T: CapabilityTrait + 'static>(
    &mut self,
    t: T,
  ) -> crate::Result<&mut Capabilities>
  {
    // If the capability is unique, check it wasn't already added
    if T::IS_UNIQUE
    {
      let it = self
        .0
        .iter()
        .filter(|x| match x
        {
          Capability::Move(_) => TypeId::of::<Move>() == TypeId::of::<T>(),
          Capability::Acquire(_) => TypeId::of::<Acquire>() == TypeId::of::<T>(),
          Capability::Grasp(_) => TypeId::of::<Grasp>() == TypeId::of::<T>(),
        })
        .next();
      if it.is_none()
      {
        self.push(t.to_capability());
        Ok(self)
      }
      else
      {
        Err(Error::DuplicateCapability(std::any::type_name::<T>()))
      }
    }
    else
    {
      self.push(t.to_capability());
      Ok(self)
    }
  }
}