use rin_math::{
    Pnt2, Unit, Rad, Vec2, vec3, Rotation2, UnitQuat, vec2, Vec3, pnt3, ToVec, AsVec, Angle,
    distance_squared, UlpsEq
};
use rin_graphics::{Mesh, Vertex3D, vertex3d, PrimitiveType, Polyline, Node, NodeParts,};
use rin_events as events;
use color::consts::*;
use rinecs::EntitiesExt;
use rinecs::{
    Entities, Resources, Write, ReadOption, Read, Ref, Entity, Not, Component, NToOneComponent,
    Filter, system, WriteOption, StorageRef,
};
use crate::time::Clock;
use crate::DeferredScene;
use crate::scene::CreationContext;
use crate::components::Name;
use crate::transformation::SkinningUpToDate;
use rin_material::{BasicMaterialBuilder, Property};
use crate::geometry::{GeometryRef, Geometry};
use std::f32;
use serde_derive::{Serialize, Deserialize};

#[derive(Component, Clone, Debug, Serialize, Deserialize)]
pub struct Path{
    pub path: Polyline,
    pub final_direction: Option<Unit<Vec2>>,
    pub initial_direction: Option<Unit<Vec2>>,
    pub max_angular_vel: Option<Rad<f32>>,
}

impl Path{
    fn next_non_zero_segment_length(&mut self, from: usize) -> Option<f32>{
        self.path.next_non_zero_segment(from)
            .and_then(|idx| self.path.segment_length(idx))
    }

    fn next_non_zero_segment_start(&self, from: usize) -> Option<&Pnt2>{
        self.path.next_non_zero_segment(from)
            .map(|idx| &self.path[idx])
    }

    fn next_non_zero_segment_end(&self, from: usize) -> Option<&Pnt2>{
        self.path.next_non_zero_segment(from)
            .map(|idx| &self.path[idx])
    }
}

#[derive(Component, Clone, Debug, Serialize, Deserialize)]
pub struct Speed(f32);

impl Speed{
    pub fn set(&mut self, d: f32){
        self.0 = d
    }
}

#[derive(Component, Clone, Debug, Serialize, Deserialize)]
pub struct Velocity(Option<Vec2>);


#[derive(Component, Clone, Debug, Serialize, Deserialize)]
pub struct Delta(f32);

impl Delta{
    pub fn set(&mut self, d: f32){
        self.0 = d
    }
}

#[derive(NToOneComponent, Clone, Debug, Serialize, Deserialize)]
struct PathDebugGeometry(Entity);

#[derive(NToOneComponent, Clone, Debug, Serialize, Deserialize)]
struct DesiredLocationDebugGeometry(Entity);

#[derive(NToOneComponent, Clone, Debug, Serialize, Deserialize)]
struct PredictedLocationDebugGeometry(Entity);

#[derive(Component, Clone, Debug, Serialize, Deserialize)]
pub struct ReynoldsPathInfo{
    radius: f32,
    arrival_radius: f32,
}

#[derive(Component, Clone, Debug, Serialize, Deserialize)]
pub struct CurrentPosition{
    idx: usize,
    pct: f64
}

impl CurrentPosition{
    pub fn idx(&self) -> usize{
        self.idx
    }

    pub fn fidx(&self) -> f64{
        self.idx as f64 + self.pct
    }

    pub fn pct_current_segment(&self) -> f64{
        self.pct
    }

    pub fn reset(&mut self){
        self.idx = 0;
        self.pct = 0.;
    }
}

#[derive(Component, Clone, Debug, Serialize, Deserialize)]
pub enum PathLookUpDistance{
    Absolute(f32),
    CurrentSegmentPct(f32),
}

#[derive(Component, Debug, Serialize)]
pub struct Parameters{
    pub doloop: events::Parameter<'static, bool>,
}

#[derive(Copy, Clone)]
pub struct PathFollower{
    tight: bool,
    reynolds: bool,
}

impl PathFollower{
    pub fn new() -> PathFollower{
        PathFollower{
            tight: true,
            reynolds: false,
        }
    }

    // pub fn enable_tight_path_following(&mut self) -> &mut PathFollower{
    //     self.tight = true;
    //     self
    // }

    pub fn enable_reynolds_path_following(&mut self) -> &mut PathFollower{
        self.reynolds = true;
        self
    }

    pub fn disable_tight_path_following(&mut self) -> &mut PathFollower{
        self.tight = true;
        self
    }

    // pub fn disable_reynolds_path_following(&mut self) -> &mut PathFollower{
    //     self.reynolds = true;
    //     self
    // }
}

impl crate::Bundle for PathFollower{
    type Parameters = ();

    fn parameters(&self) -> Option<&Self::Parameters> { None }

    fn name(&self) -> &str{
        ""
    }

    fn setup(self, world: &mut DeferredScene){
        if self.tight {
            world.add_system(path_follower);
        }
        if self.reynolds {
            world.add_system(reynolds_path_follower);
        }
    }
}

#[derive(Filter)]
pub struct EntityWithPath<'a>{
    transformation: Write<'a, Node>,
    path: Write<'a, Path>,
    speed: ReadOption<'a, Speed>,
    delta: ReadOption<'a, Delta>,
    current_position: Write<'a, CurrentPosition>,
    skinning_uptodate: WriteOption<'a, SkinningUpToDate>,
    _not_reynolds: Not<'a, ReynoldsPathInfo>,

    // parameters: Read<'a, Parameters>,
}

impl<'a> EntityWithPath<'a>{
    pub fn arrived(&self) -> bool {
        self.current_position.idx == self.path.path.len() - 1
    }

    fn arrived_orientation(&self) -> bool {
        if self.path.max_angular_vel.is_some() && self.path.final_direction.is_some() {
            let direction = -self.transformation.local_y_axis().xy();
            self.arrived() && direction.ulps_eq(&self.path.final_direction.unwrap().xy(), f32::EPSILON * 2., 2)
        }else{
            self.arrived()
        }
    }

    fn next_non_zero_segment_length(&mut self) -> Option<f32>{
        self.path.next_non_zero_segment_length(self.current_position.idx)
    }
}

#[derive(Filter)]
pub struct EntityWithPathMut<'a>{
    transformation: Write<'a, Node>,
    path: Write<'a, Path>,
    speed: WriteOption<'a, Speed>,
    delta: WriteOption<'a, Delta>,
    current_position: Write<'a, CurrentPosition>,
    skinning_uptodate: WriteOption<'a, SkinningUpToDate>,
    _not_reynolds: Not<'a, ReynoldsPathInfo>,

    // parameters: Read<'a, Parameters>,
}

impl<'a> EntityWithPathMut<'a>{
    pub fn arrived(&self) -> bool {
        self.current_position.idx == self.path.path.len() - 1
    }

    pub fn speed(&self) -> Option<f32> {
        self.speed.as_ref().map(|s| ***s)
    }

    pub fn set_speed(&mut self, speed: f32) -> Result<(), rin_util::Error> {
        if let Some(current_speed) = self.speed.as_mut() {
            current_speed.set(speed);
            Ok(())
        }else{
            Err(rin_util::Error::new("This entity doesn't have speed"))
        }
    }

    pub fn reset_current_position(&mut self){
        self.current_position.reset()
    }

    pub fn path(&self) -> &Path {
        &self.path
    }

    pub fn path_mut(&mut self) -> &mut Path {
        &mut self.path
    }

    pub fn transformation(&self) -> &Node {
        &self.transformation
    }

    pub fn transformation_mut(&mut self) -> &mut Node {
        &mut self.transformation
    }
}

#[derive(Filter)]
pub struct EntityWithReynoldsPath<'a>{
    transformation: Write<'a, Node>,
    path: Write<'a, Path>,
    speed: ReadOption<'a, Speed>,
    delta: ReadOption<'a, Delta>,
    velocity: Write<'a, Velocity>,
    current_position: Write<'a, CurrentPosition>,
    lookup_distance: Read<'a, PathLookUpDistance>,
    path_info: Read<'a, ReynoldsPathInfo>,

    // parameters: Read<'a, Parameters>,
}

impl<'a> EntityWithReynoldsPath<'a>{
    pub fn arrived(&self) -> bool {
        self.current_position.idx == self.path.path.len() - 1
    }

    fn arrived_orientation(&self) -> bool {
        if self.path.max_angular_vel.is_some() && self.path.final_direction.is_some() {
            let direction = -self.transformation.local_y_axis().xy();
            self.arrived() && direction.ulps_eq(&self.path.final_direction.unwrap().xy(), f32::EPSILON * 2., 2)
        }else{
            self.arrived()
        }
    }

    fn next_non_zero_segment_start(&self) -> Option<&Pnt2>{
        self.path.next_non_zero_segment_start(self.current_position.idx)
    }

    fn next_non_zero_segment_end(&self) -> Option<&Pnt2>{
        self.path.next_non_zero_segment_end(self.current_position.idx)
    }
}

#[derive(Filter)]
struct DebugGeometry<'a>{
    path_debug_geom: Ref<'a, PathDebugGeometry, Read<'a, GeometryRef>>,
    predicted_debug_geom: Ref<'a, PredictedLocationDebugGeometry, Read<'a, GeometryRef>>,
    desired_debug_geom: Ref<'a, DesiredLocationDebugGeometry, Read<'a, GeometryRef>>,
}

fn closest_pointto_line_segment(p: &Pnt2, start: &Pnt2, end: &Pnt2) -> Pnt2{
    let a = p - start.to_vec();
    let b = end - start;
    let b = b.normalize();
    let adotb = a.as_vec().dot(&b);
    let b = b * adotb;
    let normal_point = start + b;
    let lambda_s = (p.xy() - start).dot(&(end - start)) / ((end - start).dot(&(end - start)));

    if lambda_s <= 0. {
        *start
    }else if lambda_s >= 1.{
        *end
    }else{
        normal_point
    }
}

fn closest_point_to_polyline_segment(p: &Pnt2, polyline: &Polyline, segment: usize) -> (Pnt2, usize){
    let last = polyline[segment];
    let start = polyline.next_non_zero_segment(segment).map(|idx| &polyline[idx]);
    if start.is_none(){
        return (last, segment);
    }
    let start = start.unwrap();
    let end = polyline.next_non_zero_segment(segment + 1).map(|idx| &polyline[idx]);
    if end.is_none(){
        return (*start, segment);
    }
        let end = end.unwrap();
    let mut nearest = closest_pointto_line_segment(p, start, end);
    let mut nearest_distance = distance_squared(&nearest, p);
    let mut nearest_segment = segment;
    for (segment, start) in polyline.iter().enumerate().skip(segment + 1) {
        let end = polyline.next_non_zero_segment(segment + 1).map(|idx| &polyline[idx]);
        if end.is_none(){
            break;
        }
        let end = end.unwrap();
        let a = p - start.to_vec();
        let b = end - start;
        let b = b.normalize();
        let adotb = a.as_vec().dot(&b);
        let b = b * adotb;
        let normal_point = start + b;
        let lambda_s = (p.xy() - start).dot(&(end - start)) / ((end - start).dot(&(end - start)));
        if lambda_s <= 0. {
            let dist =  distance_squared(start, p);
            if dist < nearest_distance{
                nearest = *start;
                nearest_distance = dist;
                nearest_segment = segment;
            }
        }else if lambda_s >= 1.{
            let dist =  distance_squared(end, p);
            if dist < nearest_distance{
                nearest = *end;
                nearest_distance = dist;
                nearest_segment = segment;
            }
        }else{
            let dist =  distance_squared(&normal_point, p);
            if dist < nearest_distance{
                nearest = normal_point;
                nearest_distance = dist;
                nearest_segment = segment;
            }
        }
    }
    (nearest, nearest_segment)
}

#[system(name = "reynolds path_follower")]
#[needs("Speed", "Delta", "Path")] // TODO: Remove initial_direction from Path so it can be Read?
#[updates("NodeParts", "CurrentPosition")]
fn reynolds_path_follower(mut entities: Entities, resources: Resources){
    let tdelta = resources.get::<Clock>()
        .unwrap()
        .game_time_delta()
        .as_seconds();

    let entities = entities.as_storages();
    let mut entities_with_paths = entities
        .storage_for::<(EntityWithReynoldsPath, DebugGeometry)>()
        .unwrap();
    let mut debug_geometries = entities.storage_for::<Write<Geometry<Vertex3D>>>().unwrap();
    for (mut entity, debug) in entities_with_paths.iter_mut() {
        if entity.path.path.is_empty() || (entity.arrived() && !entity.path.path.is_closed()){
            continue
        }

        let delta = if let Some(delta) = *entity.delta {
            delta.0
        }else{
            tdelta as f32 * entity.speed.unwrap().0
        };
        let max_speed = if let Some(delta) = *entity.delta {
            delta.0 / tdelta as f32
        }else{
            entity.speed.unwrap().0
        };

        if delta == 0. {
            continue;
        }

        let end = entity.next_non_zero_segment_end();

        if end.is_none() {
            continue;
        }
        let end = end.unwrap();

        if (entity.transformation.position().xy() - end).norm() <= entity.path_info.arrival_radius {
            entity.current_position.idx += 1;
            if entity.path.path.is_closed() {
                entity.current_position.idx = entity.path.path
                    .wrap_index(entity.current_position.idx as isize)
                    .unwrap();
            }
        }

        let start = entity.next_non_zero_segment_start().unwrap();
        let end = entity.next_non_zero_segment_end();

        if end.is_none() {
            continue;
        }

        let end = end.unwrap();

        debug_geometries.get_mut(&debug.path_debug_geom)
            .unwrap()
            .set_vertices(vec![
                vertex3d(vec3!(start.to_vec(), 0.2)),
                vertex3d(vec3!(end.to_vec(), 0.2)),
            ]);

        let position = entity.transformation.position();
        let direction = -entity.transformation.local_y_axis().xy();
        let velocity = if let Some(velocity) = entity.velocity.0 {
            velocity
        }else{
            direction * max_speed
        };
        // let predict_len = match *entity.lookup_distance{
        //     PathLookUpDistance::CurrentSegmentPct(pct) => (end - start).norm() * pct,
        //     PathLookUpDistance::Absolute(l) => l,
        // };
        let predict_loc = position.xy() + velocity * tdelta as f32;


        let predicted_verts = vec![
            vertex3d(vec3!(position.to_vec().xy(), 0.2)),
            vertex3d(vec3!(predict_loc.to_vec(), 0.2)),
        ];
        debug_geometries.get_mut(&debug.path_debug_geom)
            .unwrap()
            .set_vertices(predicted_verts);

        // let closest_point = closest_pointto_line_segment(&predict_loc, start, end);
        let (closest_point, closest_segment) = closest_point_to_polyline_segment(
            &predict_loc,
            &entity.path.path,
            entity.current_position.idx);

        let distance = (predict_loc - closest_point.to_vec()).as_vec().norm();
        let desired_vert;
        let steer = if distance > entity.path_info.radius {
            let b = match *entity.lookup_distance{
                PathLookUpDistance::CurrentSegmentPct(pct) => (end - start) * pct,
                PathLookUpDistance::Absolute(l) => (end - start).normalize() * l,
            };
            // let target = closest_pointto_line_segment(&(closest_point + b), start, end);
            let (target,_) = closest_point_to_polyline_segment(
                &(closest_point + b),
                &entity.path.path,
                entity.current_position.idx);
            let desired = target - position.xy();
            let desired = desired.normalize();
            let desired = desired * max_speed;
            desired_vert = target;
            desired - velocity
        }else{
            desired_vert = closest_point;
            vec2!(0.)
        };

        let desired_verts = vec![
            vertex3d(vec3!(position.to_vec().xy(), 0.2)),
            vertex3d(vec3!(desired_vert.to_vec(), 0.2)),
        ];
        debug_geometries.get_mut(&debug.path_debug_geom)
            .unwrap()
            .set_vertices(desired_verts);

        let new_velocity = velocity + steer * tdelta as f32;
        let new_velocity = if new_velocity.norm() > max_speed{
            new_velocity.normalize() * max_speed
        }else{
            new_velocity
        };
        // let new_velocity = new_velocity.normalize() * max_speed;
        entity.velocity.0 = Some(new_velocity);
        let velocity = vec3!(entity.velocity.0.unwrap(), 0.);
        let position = position + velocity * tdelta as f32;
        entity.transformation.set_position(position);

        let next_direction = entity.velocity.0.unwrap().normalize();
        if next_direction.norm() > 0. {
            // let look_at = position + new_forward;
            // entity.transformation.look_at(&look_at, &Vec3::z());

            let delta_orientation = Rotation2::rotation_between(&direction, &next_direction);
            let mut delta_angle = Rad(delta_orientation.angle());
            if let Some(max_angular_vel) = entity.path.max_angular_vel{
                let angular_vel = delta_angle / tdelta as f32;
                if angular_vel.abs() > max_angular_vel{
                    delta_angle = max_angular_vel * tdelta as f32 * angular_vel.value().signum();
                }
            }
            let delta_orientation = UnitQuat::from_axis_angle(&Vec3::z_axis(), delta_angle.value());
            entity.transformation.append_orientation(&delta_orientation);
        }

        entity.current_position.idx = closest_segment;
    }
}

#[system(name = "path_follower")]
#[needs("Speed", "Delta")] // TODO: Remove initial_direction from Path so it can be Read?
#[updates("NodeParts", "CurrentPosition", "Path")]
#[writes("Node", "SkinningUpToDate")]
#[reads(Clock)]
pub fn path_follower(mut entities: Entities, resources: Resources){
    let tdelta = resources.get::<Clock>()
        .unwrap()
        .game_time_delta()
        .as_seconds();

    for mut entity in entities.iter_for_mut::<EntityWithPath>(){
        // If empty path or arrived continue to next
        if entity.path.path.is_empty()
            || (entity.arrived_orientation() && !entity.path.path.is_closed())
            || !entity.skinning_uptodate.as_ref().map(|u| u.0).unwrap_or(true)
        {
            continue
        }

        if let Some(skinning_uptodate) = entity.skinning_uptodate.as_mut() {
            skinning_uptodate.0 = false
        }

        // Get delta from entity delta or it's speed
        let delta = if let Some(delta) = *entity.delta {
            delta.0 as f64
        }else{
            tdelta * entity.speed.unwrap().0 as f64
        };

        let at_end = entity.arrived() && !entity.path.path.is_closed();

        if delta == 0. && !at_end {
            continue;
        }

        let next_direction;
        if at_end {
            next_direction = entity.path.final_direction.map(|dir| *dir);
        }else{
            // Distance to next segment
            let segment_distance = entity.next_non_zero_segment_length();

            if segment_distance.is_none() {
                continue;
            }

            let segment_distance = segment_distance.unwrap();

            // Increment pct with new delta over segment length
            let next_pct = entity.current_position.pct + delta / segment_distance as f64;
            if next_pct > 1. {
                // If past segment end go to next segment
                entity.current_position.idx += 1;
                if entity.path.path.is_closed() {
                    // If closed path wrap around
                    entity.current_position.idx = entity.path.path
                        .wrap_index(entity.current_position.idx as isize)
                        .unwrap();
                }
                if !entity.arrived() {
                    // If not arrived calculate remainder for next segment and go there
                    let rem = (1. - entity.current_position.pct) * segment_distance as f64;
                    if let Some(segment_distance) = entity.next_non_zero_segment_length(){
                        let rem_delta = delta - rem;
                        entity.current_position.pct = rem_delta / segment_distance as f64;
                    }else if !entity.path.path.is_closed() {
                        entity.current_position.idx = entity.path.path.len() - 1;
                        entity.current_position.pct = 0.;
                    }else{
                        continue;
                    }
                }else{
                    // If arrived reset pct and initial direction
                    entity.current_position.pct = 0.;
                    entity.path.initial_direction = None;
                }
            }else{
                entity.current_position.pct = next_pct;
            }

            let next_position;
            let fidx = entity.current_position.idx as f64 + entity.current_position.pct;
            if entity.path.path.len() > 1 &&
                entity.current_position.idx == entity.path.path.len() - 2 &&
                entity.path.final_direction.is_some()
            {
                next_position = entity.path.path.lerped_point_at(fidx as f32).unwrap();
                let prev_direction = if entity.current_position.idx == 0 {
                    if entity.path.initial_direction.is_none(){
                        let dir = Unit::new_normalize(-entity.transformation.local_y_axis().xy());
                        entity.path.initial_direction = Some(dir);
                    }
                    entity.path.initial_direction
                }else{
                    entity.path.path.tangent_at(entity.current_position.idx)
                        .map(|dir| Unit::new_normalize(dir))
                };
                let dst_dir = entity.path.final_direction.as_ref().unwrap();
                let pct = entity.current_position.pct as f32;
                next_direction = prev_direction.map(|dir| dir.lerp(dst_dir, pct));
            }else if !entity.arrived() || entity.path.path.is_closed() {
                next_position = entity.path.path.lerped_point_at(fidx as f32).unwrap();
                next_direction = entity.path.path.lerped_tangent_at(fidx as f32);
            }else{
                next_position = *entity.path.path.last().unwrap();
                next_direction = entity.path.path.tangent_at(entity.path.path.len() - 1);
            }

            entity.transformation.set_position(pnt3!(next_position, 0.));
        }

        if let Some(next_direction) = next_direction {
            let curr_direction = -entity.transformation.local_y_axis().xy().normalize();
            let delta_orientation = Rotation2::rotation_between(&curr_direction, &next_direction);
            let mut delta_angle = Rad(delta_orientation.angle());
            if let Some(max_angular_vel) = entity.path.max_angular_vel{
                let angular_vel = delta_angle / tdelta as f32;
                if angular_vel.abs() > max_angular_vel{
                    delta_angle = max_angular_vel * tdelta as f32 * angular_vel.value().signum();
                }
            }
            let delta_orientation = UnitQuat::from_axis_angle(&Vec3::z_axis(), delta_angle.value());
            let dst_orientation = delta_orientation * entity.transformation.orientation();
            entity.transformation.set_orientation(dst_orientation);
        }
    }
}

pub trait PathFollowerBuilderExt<C>{
    fn add_path_follower(&mut self, entity: &Entity) -> PathFollowerBuilder<C>;
}

impl<'a, C: CreationContext<'a>> PathFollowerBuilderExt<C> for C{
    fn add_path_follower(&mut self, entity: &Entity) -> PathFollowerBuilder<Self>{
        PathFollowerBuilder{
            entity: *entity,
            path: None,
            speed: None,
            max_angular_vel: None,
            add_debug_geometry: false,
            context: self,
        }
    }
}

pub struct PathFollowerBuilder<'a, C>{
    entity: Entity,
    path: Option<Polyline>,
    speed: Option<f32>,
    max_angular_vel: Option<Rad<f32>>,
    add_debug_geometry: bool,
    context: &'a mut C,
}

impl<'a, 'c, C: CreationContext<'c> + EntitiesExt<'c>> PathFollowerBuilder<'a, C>{
    pub fn path(mut self, path: Polyline) -> PathFollowerBuilder<'a,C>{
        self.path = Some(path);
        self
    }

    pub fn speed(mut self, speed: f32) -> PathFollowerBuilder<'a,C>{
        self.speed = Some(speed);
        self
    }

    pub fn max_angular_vel(mut self, max_angular_vel: Rad<f32>) -> PathFollowerBuilder<'a,C>{
        self.max_angular_vel = Some(max_angular_vel);
        self
    }

    // http://www.red3d.com/cwr/steer/
    // https://natureofcode.com/book/chapter-6-autonomous-agents/#chapter06_section8
    fn reynolds(self) -> ReynoldsPathFollowerBuilder<'a, C>{
        ReynoldsPathFollowerBuilder{
            entity: self.entity,
            path: self.path,
            speed: self.speed,
            max_angular_vel: self.max_angular_vel,
            add_debug_geometry: true,
            context: self.context,
            path_radius: 0.2,
            arrival_radius: 0.4,
            lookup_distance: PathLookUpDistance::CurrentSegmentPct(0.2),
        }
    }

    pub fn build(self) {
        let entity = &self.entity;
        let context = self.context;
        let path = self.path.unwrap_or(Polyline::new());
        context.add_component_to(entity, Path{
            path,
            final_direction: None,
            initial_direction: None,
            max_angular_vel: self.max_angular_vel,
        });
        if let Some(speed) = self.speed {
            context.add_component_to(entity, Speed(speed));
        }else{
            context.add_component_to(entity, Delta(0.));
        }
        context.add_component_to(entity, Velocity(None));
        context.add_component_to(entity, CurrentPosition{idx: 0, pct: 0.});

        if self.add_debug_geometry {
            add_debug_geometry(context, entity)
        }
    }
}


pub struct ReynoldsPathFollowerBuilder<'a, C>{
    entity: Entity,
    path: Option<Polyline>,
    path_radius: f32,
    arrival_radius: f32,
    speed: Option<f32>,
    lookup_distance: PathLookUpDistance,
    max_angular_vel: Option<Rad<f32>>,
    add_debug_geometry: bool,
    context: &'a mut C,
}



impl<'a, C: CreationContext<'a> + EntitiesExt<'a>> ReynoldsPathFollowerBuilder<'a, C>{
    pub fn path(mut self, path: Polyline) -> ReynoldsPathFollowerBuilder<'a,C>{
        self.path = Some(path);
        self
    }

    pub fn path_radius(mut self, path_radius: f32) -> ReynoldsPathFollowerBuilder<'a,C>{
        self.path_radius = path_radius;
        self
    }

    pub fn arrival_radius(mut self, arrival_radius: f32) -> ReynoldsPathFollowerBuilder<'a,C>{
        self.arrival_radius = arrival_radius;
        self
    }

    pub fn speed(mut self, speed: f32) -> ReynoldsPathFollowerBuilder<'a,C>{
        self.speed = Some(speed);
        self
    }

    pub fn lookup_distance(mut self, lookup_distance: PathLookUpDistance) -> ReynoldsPathFollowerBuilder<'a,C>{
        self.lookup_distance = lookup_distance;
        self
    }

    pub fn max_angular_vel(mut self, max_angular_vel: Rad<f32>) -> ReynoldsPathFollowerBuilder<'a,C>{
        self.max_angular_vel = Some(max_angular_vel);
        self
    }

    pub fn build(self){
        let entity = &self.entity;
        let context = self.context;
        let path = self.path.unwrap_or(Polyline::new());
        context.add_component_to(entity, Path{
            path,
            final_direction: None,
            initial_direction: None,
            max_angular_vel: self.max_angular_vel,
        });
        if let Some(speed) = self.speed {
            context.add_component_to(entity, Speed(speed));
        }else{
            context.add_component_to(entity, Delta(0.));
        }
        context.add_component_to(entity, Velocity(None));
        context.add_component_to(entity, CurrentPosition{idx: 0, pct: 0.});
        context.add_component_to(entity, self.lookup_distance);
        context.add_component_to(entity, ReynoldsPathInfo{
            radius: self.path_radius,
            arrival_radius: self.arrival_radius,
        });

        if self.add_debug_geometry {
            add_debug_geometry(context, entity)
        }
    }
}


fn add_debug_geometry<'a, C: CreationContext<'a> + EntitiesExt<'a>>(context: &mut C, entity: &Entity){
    let mut path_geom: Mesh<Vertex3D> = Mesh::default();
    path_geom.set_primitive_type(PrimitiveType::LineStrip);
    let path_geom = context.register_mesh(path_geom);

    let path_material = BasicMaterialBuilder::new()
        .base_color(&WHITE)
        .properties(vec![Property::DepthTest(false)])
        .build();

    let path_material = context.register_material("__PATHFOLLOWER_PATH_MATERIAL", path_material);

    let name = context.component_for::<Name>(entity)
        .map(|n| n.0.clone())
        .unwrap_or_else(|| "".to_owned());
    let path = context.add_model(&format!("Path{}", name))
        .material(path_material)
        .geometry(path_geom)
        .build();
    context.add_component_to(entity, PathDebugGeometry(path));


    let mut desired_location_geom: Mesh<Vertex3D> = Mesh::default();
    desired_location_geom.set_primitive_type(PrimitiveType::Lines);
    let desired_location_geom = context.register_mesh(desired_location_geom);
    let desired_location_material = BasicMaterialBuilder::new()
        .base_color(&RED)
        .properties(vec![Property::DepthTest(false)])
        .build();

    let desired_location_material = context
        .register_material("__PATHFOLLOWER_DESIRED_LOC_MATERIAL", desired_location_material);
    let desired_location = context.add_model(&format!("DesiredLocation{}", name))
        .material(desired_location_material)
        .geometry(desired_location_geom)
        .build();
    context.add_component_to(entity, DesiredLocationDebugGeometry(desired_location));

    let mut predicted_location_geom: Mesh<Vertex3D> = Mesh::default();
    predicted_location_geom.set_primitive_type(PrimitiveType::Lines);
    let predicted_location_geom = context.register_mesh(predicted_location_geom);
    let predicted_location_material = BasicMaterialBuilder::new()
        .base_color(&GREEN)
        .properties(vec![Property::DepthTest(false)])
        .build();

    let predicted_location_material = context
        .register_material("__PATHFOLLOWER_PREDICTED_LOC_MATERIAL", predicted_location_material);
    let predicted_location = context.add_model(&format!("PredictedLocation{}", name))
        .material(predicted_location_material)
        .geometry(predicted_location_geom)
        .build();
    context.add_component_to(entity, PredictedLocationDebugGeometry(predicted_location));
}