//! Simulation model

use {std, vec_map::VecMap};
use {component, event, force, integrator, object, Collision, Integrator};

/// A system representing the computational model for the simulation.
///
/// There are three categories of entities that are found in the system (model):
///
/// - Objects -- `Static`, `Dynamic`, `Nodetect`
/// - Forces -- `Gravity`
/// - Constraints -- 'Planar', 'Penetration' (TODO)
#[cfg_attr(feature = "derive_serdes", derive(Serialize, Deserialize))]
#[derive(Clone,Debug,Default,PartialEq)]
pub struct System <INTG : Integrator> {
  /// Step counter
  step              : u64,
  /// No integration
  objects_static    : VecMap <object::Static>,
  /// Integration and collision
  objects_dynamic   : VecMap <object::Dynamic>,
  /// Integration only; no collision detection or response
  objects_nodetect  : VecMap <object::Nodetect>,
  /// Global gravity force
  gravity           : Option <force::Gravity>,
  /// Collision subsystem
  collision         : Collision,
  #[cfg_attr(feature = "derive_serdes", serde(skip))]
  _phantom_data     : std::marker::PhantomData <INTG>
}

impl <INTG : Integrator> System <INTG> {
  #[inline]
  pub fn new() -> Self where INTG : Default {
    std::default::Default::default()
  }
  #[inline]
  pub fn step (&self) -> u64 {
    self.step
  }
  #[inline]
  pub fn objects_dynamic (&self) -> &VecMap <object::Dynamic> {
    &self.objects_dynamic
  }
  #[inline]
  pub fn objects_nodetect (&self) -> &VecMap <object::Nodetect> {
    &self.objects_nodetect
  }
  #[inline]
  pub fn objects_static (&self) -> &VecMap <object::Static> {
    &self.objects_static
  }

  /// Input event handling.
  ///
  /// - `CreateObject` -- if an object is found to be in *non-colliding* contact
  ///    with any other objects, those contacts will be registered with the
  ///    collision system as *persistent contacts* for the upcoming step
  /// - `DestroyObject`
  /// - `SetGravity`
  /// - `ClearGravity`
  /// - `Step` -- advance the simulation by a single *timestep*:
  ///     1. Derivative evaluation: forces are accumulated and acceleration
  ///        computed
  ///     2. Velocities are integrated
  ///     3. Velocity constraints are solved
  ///     4. Positions are integrated using the new velocity (semi-implicit
  ///        Euler)
  ///     5. Position constraints are solved
  ///     6. Collision detects and resolves collisions in the normalized
  ///        sub-timestep [0.0-1.0).
  ///
  ///        Note that colliding contacts detected at t==1.0 will *not* produce
  ///        a collision response event (the next step will detect and resolve this
  ///        collision), however resting or separating contacts detected
  ///        at t==1.0 will be registered as a *persistent contact* for the next
  ///        simulation step.
  pub fn handle_event (&mut self, input : event::Input) -> Vec <event::Output> {
    let mut output = Vec::new();
    match input {

      //
      // event::Input::CreateObject
      //
      event::Input::CreateObject (object, key) => {
        match object {
          object::Variant::Static (object) => {
            self.create_object_static (object, key, &mut output);
          }
          object::Variant::Dynamic (object) => {
            self.create_object_dynamic (object, key, &mut output);
          }
          object::Variant::Nodetect (object) => {
            self.create_object_nodetect (object, key);
          }
        }
      }

      //
      // event::Input::ModifyObject
      //
      event::Input::ModifyObject (object_modify_event) => {
        match object_modify_event {
          event::ObjectModify::Dynamic (key, event) => {
            self.modify_object_dynamic (key, event);
          }
          event::ObjectModify::Static  (key, event) => {
            self.modify_object_static (key, event);
          }
        }
      }

      //
      // event::Input::DestroyObject
      //
      event::Input::DestroyObject (object::Identifier { kind, key }) => {
        match kind {
          object::Kind::Static => {
            self.collision.remove_object_static (key);
            assert!(self.objects_static.remove (key.index()).is_some());
          }
          object::Kind::Dynamic => {
            self.collision.remove_object_dynamic (key);
            assert!(self.objects_dynamic.remove (key.index()).is_some());
          }
          object::Kind::Nodetect =>
            assert!(self.objects_nodetect.remove (key.index()).is_some())
        }
      }

      //
      // event::Input::SetGravity
      //
      event::Input::SetGravity (gravity) => {
        assert!(self.gravity.is_none());
        self.gravity = Some (gravity);
      }

      //
      // event::Input::ClearGravity
      //
      event::Input::ClearGravity => {
        assert!(self.gravity.is_some());
        self.gravity = None;
      }

      //
      // event::Input::Step
      //
      event::Input::Step => {
        // 1. derivative evaluation (i.e., accumulate forces and compute
        //    accelerations)
        self.derivative_evaluation();
        // 2. integrate velocity
        self.integrate_velocity();
        // 3. constrain velocities
        self.constrain_velocity();
        // 4. integrate position
        self.integrate_position();
        // 5. constrain position
        self.constrain_position();
        // 6. detect and solve collisions in a loop
        self.collision (&mut output);
        self.step += 1;
      }

    }
    output
  }

  fn create_object_static (&mut self,
    object : object::Static,
    key    : object::Key,
    output : &mut Vec <event::Output>
  ) {
    let object_id = object::Identifier {
      kind: object::Kind::Static, key
    };

    // try to add to collision system: detects intersections in case of failure
    match self.collision.try_add_object_static (
      &self.objects_dynamic, &object, key
    ) {
      Ok  (()) =>   // object successfully added
        assert!(self.objects_static.insert (key.index(), object).is_none()),
      Err (intersections) => {
        debug_assert!(!intersections.is_empty());
        output.push (event::CreateObjectResult::Intersection (
          object_id.clone(), intersections
        ).into());
      }
    }
  }

  fn create_object_dynamic (&mut self,
    object : object::Dynamic,
    key    : object::Key,
    output : &mut Vec <event::Output>
  ) {
    let object_id = object::Identifier {
      kind: object::Kind::Static, key
    };
    // try to add to collision system: detects intersections in case of failure
    match self.collision.try_add_object_dynamic (
      &self.objects_static, &self.objects_dynamic, &object, key
    ) {
      Ok  (()) => // object successfully added
        assert!(self.objects_dynamic.insert (key.index(), object).is_none()),
      Err (intersections) => {
        debug_assert!(!intersections.is_empty());
        output.push (event::CreateObjectResult::Intersection (
          object_id.clone(), intersections
        ).into());
      }
    }
  }

  fn create_object_nodetect (&mut self,
    object : object::Nodetect,
    key    : object::Key,
  ) {
    assert!(self.objects_nodetect.insert (key.index(), object).is_none());
  }

  /// Panics if object with the given key is not present
  fn modify_object_dynamic (&mut self,
    key   : object::Key,
    event : event::ObjectModifyDynamic
  ) {
    let object = &mut self.objects_dynamic[key.index()];
    match event {
      event::ObjectModifyDynamic::ApplyImpulse (impulse) => {
        object.derivatives.velocity += impulse;
      }
    }
  }

  /// Panics if object with the given key is not present
  fn modify_object_static (&mut self,
    key   : object::Key,
    event : event::ObjectModifyStatic
  ) {
    let object = &mut self.objects_static[key.index()];
    match event {
      event::ObjectModifyStatic::Move (move_vector) => {
        // object will be resorted at the beginning of the next collision broad
        // phase
        let component::Position (ref mut position) = object.position;
        *position += move_vector;
      }
    }
  }

  /// Computes accelerations from accumulated forces
  fn derivative_evaluation (&mut self) {
    // clear forces
    for_sequence!{
      (_, object) in (
        self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
      ) {
        object.derivatives.force = [0.0, 0.0, 0.0].into();
      }
    }

    // accumulate forces
    if let Some (ref gravity) = &self.gravity {
      for_sequence!{
        (_, object) in (
          self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
        ) {
          if object.derivatives.force_flags.contains (force::Flag::Gravity) {
            use {Force};
            let impulse = gravity.impulse (object, self.step, 1.0);
            object.derivatives.force += impulse;
          }
        }
      }
    }

    // compute accelerations from accumulated forces
    for_sequence!{
      (_, object) in (
        self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
      ) {
        use object::Inertial;
        object.compute_acceleration_inplace();
      }
    }
  }

  /// Integrates velocity from acceleration
  fn integrate_velocity (&mut self) {
    for_sequence!{
      (_, object) in (
        self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
      ) {
        INTG::integrate_velocity (object);
      }
    }
  }

  /// Solve velocity constraints
  fn constrain_velocity (&mut self) {
    // TODO: constrain velocities
  }

  /// Integrates position from velocity
  fn integrate_position (&mut self) {
    for_sequence!{
      (_, object) in (
        self.objects_dynamic.iter_mut(), self.objects_nodetect.iter_mut()
      ) {
        INTG::integrate_position (object);
      }
    }
  }

  /// Solve position constraints
  fn constrain_position (&mut self) {
    // TODO: constrain position
  }

  /// Collision detection and response
  fn collision (&mut self, output : &mut Vec <event::Output>) {
    self.collision.detect_resolve_loop (
      &mut self.objects_static, &mut self.objects_dynamic, self.step, output);
  }

}

/// Print system size information
pub fn report_sizes() {
  use std::mem::size_of;
  println!("system report sizes...");

  println!("  size of System <integrator::SemiImplicitEuler>: {}",
    size_of::<System <integrator::SemiImplicitEuler>>());

  println!("...system report sizes");
}