use super::{Gradient, vertex::*, mesh::*, PrimitiveType};
use color::{ToRgba,ToRgb,Rgba};
use rin_math::{
    Pnt2, zero, scalar::SubsetOf, Unit, ToVec, iwrap,
    RealField, Vec2, one, Rect, vec2, Rad, pnt2, Deg, vec3, Vec3, Matrix3x2, convert, Angle,
    Float, NumCast, cast, FloatPnt, NumPnt, ToPnt
};
use std::{f32, f64};
use std::mem;
use std::iter;
use std::ops::Neg;

/// Represents a line segment in 2D space
#[derive(Debug,Clone)]
pub struct Line{
    pub p1: Pnt2,
    pub p2: Pnt2,
}

impl Line{
    /// Returns the line segment as a mesh with only position
    pub fn to_mesh(self) -> Mesh<Vec2>{
        mesh(
            vec![
                self.p1.to_vec(),
                self.p2.to_vec(),
            ],
            PrimitiveType::Lines
        )
    }

    /// Returns the line segment as a mesh with position and color
    pub fn to_color_mesh<C: ToRgba>(self, color: &C) -> Mesh<Vertex2DColor>{
        mesh(
            vec![
    	    	vertex2dcolor(self.p1.to_vec(), color),
    	    	vertex2dcolor(self.p2.to_vec(), color),
    	    ],
    	    PrimitiveType::Lines
    	)
    }
}

pub trait RectToMesh{
    fn to_mesh(self) -> Mesh<Vec2>;
    fn to_color_mesh<C: ToRgba>(self, color: &C) -> Mesh<Vertex2DColor>;
    fn to_tex_mesh(self, tex: Rect<f32>) -> Mesh<Vertex2DTex>;
    fn to_gradient_mesh<C: ToRgba + ToRgb>(self, gradient: &Gradient<C>) -> Mesh<Vertex2DColor>;

}

// This needs to be an Ext trait
impl RectToMesh for Rect<f32>{
    fn to_mesh(self) -> Mesh<Vec2>{
        mesh(
            vec![
            	self.pos.to_vec(),
            	self.top_right().to_vec(),
        		self.bottom_right().to_vec(),
        		self.bottom_left().to_vec()
        	],
        	PrimitiveType::TriangleFan
        )
    }

    fn to_color_mesh<C: ToRgba>(self, color: &C) -> Mesh<Vertex2DColor>{
        let rgba = color.to_rgba::<f32>();
        mesh(
            vec![
            	vertex2dcolor(self.pos.to_vec(), &rgba),
            	vertex2dcolor(self.top_right().to_vec(), &rgba),
        		vertex2dcolor(self.bottom_right().to_vec(), &rgba),
        		vertex2dcolor(self.bottom_left().to_vec(), &rgba),
        	],
        	PrimitiveType::TriangleFan
        )
    }

    fn to_tex_mesh(self, tex: Rect<f32>) -> Mesh<Vertex2DTex>{
        mesh(
            vec![
                vertex2dtex(self.pos.to_vec(), tex.pos.to_vec()),
                vertex2dtex(self.top_right().to_vec(), tex.top_right().to_vec()),
                vertex2dtex(self.bottom_right().to_vec(), tex.bottom_right().to_vec()),
                vertex2dtex(self.bottom_left().to_vec(), tex.bottom_left().to_vec()),
            ],
            PrimitiveType::TriangleFan
        )
    }

    fn to_gradient_mesh<C: ToRgba + ToRgb>(self, gradient: &Gradient<C>) -> Mesh<Vertex2DColor>{
        match gradient{
            &Gradient::Linear{..} => mesh(
                vec![
            		vertex2dcolor::<Rgba<f32>>(self.pos.to_vec(), &gradient.rgba_at(vec2(0.0,0.0))),
            		vertex2dcolor::<Rgba<f32>>(self.top_right().to_vec(), &gradient.rgba_at(vec2(1.0,0.0))),
            		vertex2dcolor::<Rgba<f32>>(self.bottom_right().to_vec(), &gradient.rgba_at(vec2(1.0,1.0))),
            		vertex2dcolor::<Rgba<f32>>(self.bottom_left().to_vec(), &gradient.rgba_at(vec2(0.0,1.0))),
            	],
            	PrimitiveType::TriangleFan
            ),
            &Gradient::Circular{ref start_color, ref end_color} => {
        		let center = self.pos.to_vec() + vec2(self.width / 2., self.height / 2.);
        		let resolution = 32usize; // circular gradient resolution
        		let angle_bisector = Rad::two_pi() / (resolution as f32 * 2.);
        		let small_r = vec2(self.width / 2., self.height / 2.).norm();
        		let big_r = small_r / angle_bisector.cos();
                let two_pi: Rad<f32> = Rad::two_pi();
                let vertices = iter::once(vertex2dcolor(center, start_color))
                    .chain((0..resolution+1).map(|i|{
            			let theta:f32 = i as f32 * two_pi.value() / resolution as f32;
                        let pos = center + vec2(theta.sin(), theta.cos()) * big_r;
            			vertex2dcolor(pos, end_color)
        		    }));
                Mesh::from_iter_and_type(vertices, PrimitiveType::TriangleFan)
            }
        }
    }
}

/// Represents an ellipse in 2D space
#[derive(Debug,Clone)]
pub struct Ellipse{
    pub center: Pnt2,
    pub width: f32,
    pub height: f32,
}

impl Ellipse{
    /// Returns the ellipse as a filled mesh with only position info
    pub fn to_mesh(self, resolution: u32) -> Mesh<Vec2>{
        let pos = self.center;
        let w = self.width * 0.5;
        let h = self.height * 0.5;
        let vertices = arc_iter(pos, w, h, Rad(0.), Rad::two_pi(), resolution).map(|v| v.to_vec());
        Mesh::from_iter_and_type(vertices, PrimitiveType::TriangleFan)
    }

    /// Returns the ellipse as a contour mesh with only position info
    pub fn to_outline(self, resolution: u32) -> Mesh<Vec2>{
        let pos = self.center;
        let w = self.width * 0.5;
        let h = self.height * 0.5;
        let vertices = arc_iter(pos, w, h, Rad(0.), Rad::two_pi(), resolution).map(|v| v.to_vec());
        Mesh::from_iter_and_type(vertices, PrimitiveType::LineLoop)
    }

    /// Returns the ellipse as a filled mesh with position and color info
    pub fn to_mesh_color<C:ToRgba>(self, color: &C, resolution: u32) -> Mesh<Vertex2DColor>{
        let pos = self.center;
        let w = self.width * 0.5;
        let h = self.height * 0.5;
        let vertices = arc_iter(pos, w, h, Rad(0.), Rad::two_pi(), resolution)
            .map(|p| vertex2dcolor(p.to_vec(), color));
        Mesh::from_iter_and_type(vertices, PrimitiveType::TriangleFan)
    }

    /// Returns the ellipse as a contour mesh with position and color info
    pub fn to_outline_color<C:ToRgba>(self, color: &C, resolution: u32) -> Mesh<Vertex2DColor>{
        let pos = self.center;
        let w = self.width * 0.5;
        let h = self.height * 0.5;
        let vertices = arc_iter(pos, w, h, Rad(0.), Rad::two_pi(), resolution)
            .map(|p| vertex2dcolor(p.to_vec(), color));
        Mesh::from_iter_and_type(vertices, PrimitiveType::LineLoop)
    }

    /// Returns the ellipse as a filled mesh with position and texture coordinates info
    pub fn to_mesh_texcoords(self, resolution: u32) -> Mesh<Vertex2DTex>{
        let pos = self.center;
        let w = self.width * 0.5;
        let h = self.height * 0.5;
        let size = vec2(self.width, self.height);
        let vertices = arc_iter(pos, w, h, Rad(0.), Rad::two_pi(), resolution)
            .map(|p| {
                let tex = p - pos;
                let tex = vec2(tex.x / size.x, tex.y / size.y);
                Vertex2DTex{ position: p.to_vec(), texcoord: tex }
            });
        Mesh::from_iter_and_type(vertices, PrimitiveType::TriangleFan)
    }
}

/// Represents an circle in 2D space
#[derive(Debug,Clone)]
pub struct Circle{
    pub center: Pnt2,
    pub radius: f32,
}

impl Circle{
    /// Returns the circle as a filled mesh with only position info
    pub fn to_mesh(self, resolution: u32) -> Mesh<Vec2>{
        Ellipse{
            center: self.center,
            width: self.radius * 2.,
            height: self.radius * 2.,
        }.to_mesh(resolution)
    }

    /// Returns the circle as a contour mesh with only position info
    pub fn to_outline(self, resolution: u32) -> Mesh<Vec2>{
        Ellipse{
            center: self.center,
            width: self.radius * 2.,
            height: self.radius * 2.,
        }.to_outline(resolution)
    }

    /// Returns the circle as a filled mesh with position and color info
    pub fn to_mesh_color<C:ToRgba>(self, color: &C, resolution: u32) -> Mesh<Vertex2DColor>{
        Ellipse{
            center: self.center,
            width: self.radius * 2.,
            height: self.radius * 2.,
        }.to_mesh_color(color, resolution)
    }

    /// Returns the circle as a contour mesh with position and color info
    pub fn to_outline_color<C:ToRgba>(self, color: &C, resolution: u32) -> Mesh<Vertex2DColor>{
        Ellipse{
            center: self.center,
            width: self.radius * 2.,
            height: self.radius * 2.,
        }.to_outline_color(color, resolution)
    }

    /// Returns the circle as a filled mesh with position and texture coordinates info
    pub fn to_mesh_texcoords(self, resolution: u32) -> Mesh<Vertex2DTex>{
        Ellipse{
            center: self.center,
            width: self.radius * 2.,
            height: self.radius * 2.,
        }.to_mesh_texcoords(resolution)
    }
}

/// Returns an ellipse as a filled mesh with only position info
pub fn ellipse(pos: &Pnt2, w: f32, h:f32, resolution: u32) -> Mesh<Vec2>{
    let vertices = arc_iter(*pos, w*0.5, h*0.5, Deg(0.), Deg::two_pi(), resolution)
        .map(|p| p.to_vec());
    Mesh::from_iter_and_type(vertices, PrimitiveType::TriangleFan)
}

/// Returns an ellipse as a filled mesh with position and texture coordinates info
pub fn ellipse_texcoords(pos: &Pnt2, w: f32, h:f32, resolution: u32) -> Mesh<Vertex2DTex>{
    let size = pnt2(w,h);
    let vertices = arc_iter(*pos, w*0.5, h*0.5, Deg(0.), Deg::two_pi(), resolution)
        .map(|p| {
            let tex = p - pos;
            let tex = vec2(tex.x / size.x, tex.y / size.y);
            Vertex2DTex{ position: p.to_vec(), texcoord: tex }
        });
    Mesh::from_iter_and_type(vertices, PrimitiveType::TriangleFan)
}

/// Returns an ellipse as a filled mesh with position and color info
pub fn ellipse_color<C:ToRgba>(pos: &Pnt2, w: f32, h:f32, resolution: u32, color: &C) -> Mesh<Vertex2DColor>{
    let vertices = arc_iter(*pos, w*0.5, h*0.5, Deg(0.), Deg::two_pi(), resolution)
        .map(|p| vertex2dcolor(p.to_vec(), color));
    Mesh::from_iter_and_type(vertices, PrimitiveType::TriangleFan)
}

/// Returns an circle as a filled mesh with only position info
pub fn circle(pos: &Pnt2, r: f32, resolution: u32) -> Mesh<Vec2>{
    ellipse(pos, r*2.0, r*2.0, resolution)
}

/// Returns a circle as a filled mesh with position and texture coordinates info
pub fn circle_texcoords(pos: &Pnt2, r: f32, resolution: u32) -> Mesh<Vertex2DTex>{
    ellipse_texcoords(pos, r*2.0, r*2.0, resolution)
}

/// Returns a circle as a contour mesh with position and color info
pub fn circle_color<C:ToRgba>(pos: &Pnt2, r: f32, resolution: u32, color: &C) -> Mesh<Vertex2DColor>{
    ellipse_color(pos, r*2.0, r*2.0, resolution,color)
}

/// Returns a line segment as a mesh with position and color
pub fn line_color<C: ToRgba>(from: &Pnt2, to: &Pnt2, color: &C) -> Mesh<Vertex2DColor>{
    mesh(
        vec![
	    	vertex2dcolor(vec2(from.x,from.y), color),
	    	vertex2dcolor(vec2(to.x,to.y), color)
	    ],
	    PrimitiveType::Lines
	)
}

/// Returns a rectangle as a filled mesh with position
pub fn rectangle(pos: &Pnt2, w: f32, h: f32) -> Mesh<Vec2>{
    mesh(
        vec![
        	pos.to_vec(),
        	vec2(pos.x + w, pos.y),
    		vec2(pos.x + w, pos.y + h),
    		vec2(pos.x, pos.y + h)
    	],
    	PrimitiveType::TriangleFan
    )
}

/// Returns a rectangle as a filled mesh with position and texture coordinates
pub fn rectangle_texcoords(pos: &Pnt2, w: f32, h: f32, s0: f32, t0: f32, s1: f32, t1:f32) -> Mesh<Vertex2DTex>{
    mesh(
        vec![
    		vertex2dtex(pos.to_vec(),               vec2(s0,t1)),
    		vertex2dtex(vec2(pos.x + w, pos.y),     vec2(s1,t1)),
    		vertex2dtex(vec2(pos.x + w, pos.y + h), vec2(s1,t0)),
    		vertex2dtex(vec2(pos.x, pos.y + h),     vec2(s0,t0)),
    	],
    	PrimitiveType::TriangleFan
    )
}

/// Returns a rectangle as a filled mesh with position and color per vertex
pub fn rectangle_color<C: ToRgba>(pos: &Pnt2, w: f32, h: f32, color: &C) -> Mesh<Vertex2DColor>{
    mesh(
        vec![
    		vertex2dcolor(pos.to_vec(),                 color),
    		vertex2dcolor(vec2(pos.x + w, pos.y),       color),
    		vertex2dcolor(vec2(pos.x + w, pos.y + h),   color),
    		vertex2dcolor(vec2(pos.x, pos.y + h),      color),
    	],
    	PrimitiveType::TriangleFan
    )
}

/// Returns a rectangle as a filled mesh with position and color per vertex from a gradient
pub fn rectangle_gradient<C: ToRgba + ToRgb>(pos: &Pnt2, w: f32, h: f32, gradient: &Gradient<C>) -> Mesh<Vertex2DColor>{
    match gradient{
        &Gradient::Linear{..} => mesh(
            vec![
        		vertex2dcolor(pos.to_vec(),                 &gradient.rgba_at::<f32>(vec2(0.0,0.0))),
        		vertex2dcolor(vec2(pos.x + w, pos.y),       &gradient.rgba_at::<f32>(vec2(1.0,0.0))),
        		vertex2dcolor(vec2(pos.x + w, pos.y + h),   &gradient.rgba_at::<f32>(vec2(1.0,1.0))),
        		vertex2dcolor(vec2(pos.x, pos.y + h),       &gradient.rgba_at::<f32>(vec2(0.0,1.0))),
        	],
        	PrimitiveType::TriangleFan
        ),
        &Gradient::Circular{ref start_color, ref end_color} => {
    		let center = pos + vec2(w / 2., h / 2.);
            let mut vertices = vec![ vertex2dcolor(center.to_vec(), start_color) ];
    		let resolution = 32; // circular gradient resolution
    		let angle_bisector = Rad::two_pi() / (resolution as f32 * 2.);
    		let small_r = vec2(w / 2., h / 2.).norm();
    		let big_r = small_r / angle_bisector.cos();
            let two_pi: Rad<f32> = Rad::two_pi();
    		for i in 0..resolution+1 {
    			let theta:f32 = i as f32 * two_pi.value() / resolution as f32;
                let pos = center + vec2(theta.sin(), theta.cos()) * big_r;
    			vertices.push(vertex2dcolor(pos.to_vec(), end_color));
    		}
            mesh(vertices, PrimitiveType::TriangleFan)
        }
    }
}

/// Returns a sphere as a filled mesh with position, texture coordinates and normals
pub fn sphere_texcoords(radius: f32, rings: u32, sectors: u32) -> Mesh<Vertex3DTexNormal>{
    let inv_r = 1.0 / rings as f32;
    let inv_s = 1.0 / sectors as f32;

    let ring_vertices = rings + 1;
    let sector_vertices = sectors + 1;

    let mut mesh: Mesh<Vertex3DTexNormal> = Mesh::default();

    mesh.reserve((ring_vertices*sector_vertices) as usize);
    unsafe{
        mesh.set_len((ring_vertices*sector_vertices) as usize);
    }

    mesh.indices_mut().reserve((ring_vertices * sector_vertices * 6) as usize);
    unsafe{
        mesh.indices_mut().set_len((ring_vertices * sector_vertices * 6) as usize);
    }

    let mut v = 0;
    let mut i = 0;

    sphere_vertices(rings,sectors,
        |vertex,r,s|{
            mesh[v].position = vertex * radius;
            mesh[v].texcoord = vec2(s as f32 * inv_s, r as f32 * inv_r);
            mesh[v].normal = vertex;
            v+=1;
        },
    );


    for r in 0 ..= rings {
        for s in 0 .. sectors {
            mesh.indices_mut()[i] = ((r+1) * sectors + (s+1)) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = (r * sectors + (s+1)) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = (r * sectors + s) as IndexT;
            i += 1;

            mesh.indices_mut()[i] = (r * sectors + s) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = ((r+1) * sectors + s) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = ((r+1) * sectors + (s+1)) as IndexT;
            i += 1;
        }
    }

    mesh
}

/// Returns a sphere as a filled mesh with position, color per vertex and normals
pub fn sphere_color<C:ToRgba>(radius: f32, rings: u32, sectors: u32, color: &C) -> Mesh<Vertex3DColorNormal>{
    let ring_vertices = rings+1;
    let sector_vertices = sectors+1;

    let mut mesh: Mesh<Vertex3DColorNormal> = Mesh::default();

    mesh.reserve((ring_vertices*sector_vertices) as usize);
    unsafe{
        mesh.set_len((ring_vertices*sector_vertices) as usize);
    }

    mesh.indices_mut().reserve((ring_vertices * sectors * 6) as usize);
    unsafe{
        mesh.indices_mut().set_len((ring_vertices * sectors * 6) as usize);
    }

    let mut v = 0;
    let mut i = 0;

    sphere_vertices(rings,sectors,
        |vertex,_r,_s|{
            mesh[v].position = vertex * radius;
            mesh[v].color = color.to_rgba().to_standard();
            mesh[v].normal = vertex;
            v+=1;
        },
    );


    for r in 0 .. rings+1 {
        for s in 0 .. sectors {
            mesh.indices_mut()[i] = ((r+1) * sectors + (s+1)) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = (r * sectors + (s+1)) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = (r * sectors + s) as IndexT;
            i += 1;

            mesh.indices_mut()[i] = (r * sectors + s) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = ((r+1) * sectors + s) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = ((r+1) * sectors + (s+1)) as IndexT;
            i += 1;
        }
    }

    mesh
}

/// Returns a sphere as a filled mesh with position and normals
pub fn sphere(radius: f32, rings: u32, sectors: u32) -> Mesh<Vertex3DNormal>{
    let ring_vertices = rings+1;
    let sector_vertices = sectors+1;

    let mut mesh: Mesh<Vertex3DNormal> = Mesh::default();

    mesh.reserve((ring_vertices*sector_vertices) as usize);
    unsafe{
        mesh.set_len((ring_vertices*sector_vertices) as usize);
    }

    mesh.indices_mut().reserve((ring_vertices * sectors * 6) as usize);
    unsafe{
        mesh.indices_mut().set_len((ring_vertices * sectors * 6) as usize);
    }

    let mut v = 0;
    let mut i = 0;

    sphere_vertices(rings,sectors,
        |vertex,_r,_s|{
            mesh[v].position = vertex * radius;
            mesh[v].normal = vertex;
            v+=1;
        },
    );


    for r in 0 .. rings+1 {
        for s in 0 .. sectors {
            mesh.indices_mut()[i] = ((r+1) * sectors + (s+1)) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = (r * sectors + (s+1)) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = (r * sectors + s) as IndexT;
            i += 1;

            mesh.indices_mut()[i] = (r * sectors + s) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = ((r+1) * sectors + s) as IndexT;
            i += 1;
            mesh.indices_mut()[i] = ((r+1) * sectors + (s+1)) as IndexT;
            i += 1;
        }
    }

    mesh
}

/// Returns a cone as a filled mesh with position and normals
pub fn cone(radius: f32, height: f32, segments: u32) -> Mesh<Vertex3DNormal>{
    let mut mesh: Mesh<Vertex3DNormal> = Mesh::default();
    const TWO_PI: f32 = 2.0 * f32::consts::PI;
    let height_segments = 1;
    // let cap_segments = 1;
    let radius_segments = segments;
    let angle_step = -1. * TWO_PI / radius_segments as f32;
    let height_step = 1. / height_segments as f32 * height;
    let start = vec3!(0., height, 0.);
    mesh.push(vertex3dnormal(start, Vec3::y()));
    for iy in 1 .. height_segments + 1{
        for ix in 1 .. radius_segments + 1{
            let mut r = (iy as f32 / height_segments as f32) * radius;
            let mut x = (ix as f32 * angle_step).cos() * r;
            let mut y = height_step * (iy - 1) as f32;
            let mut z = (ix as f32 * angle_step).sin() * r;
            let pos = vec3!(x,y,z);
            if iy == 0 {
                r = 1.;
                x = (ix as f32 * angle_step).cos() * r;
                y = height_step * (iy - 1) as f32;
                z = (ix as f32 * angle_step).sin() * r;
            }

            let new_pos = vec3!(x,y,z);
            let diff = new_pos - start;
            let crossed = (-Vec3::y()).cross(&new_pos);
            let normal = crossed.cross(&diff);
            mesh.push(vertex3dnormal(pos, normal.normalize()));
        }
    }

    mesh.push(vertex3dnormal(vec3!(0.), -Vec3::y()));

    // let cap_start = mesh.len() as u32;
    //
    // for iy in 0 .. cap_segments + 1 {
    //     for ix in 0 .. radius_segments + 1{
    //         let r = (1. - iy as f32 / cap_segments as f32) * radius;
    //         let x = (ix as f32 * angle_step).cos() * r;
    //         let y = 0.;
    //         let z = (ix as f32 * angle_step).sin() * r;
    //         let pos = vec3!(x,y,z);
    //         mesh.push(vertex3dnormal(pos, -Vec3::y()));
    //     }
    // }

    // let height_segments = height_segments + 1;
    // let radius_segments = radius_segments + 1;
    // let cap_segments = cap_segments + 1;

    for ix in 1 .. radius_segments {
        mesh.add_face(
            0,
            ix + 1,
            ix,
        );
    }
    mesh.add_face(
        0,
        1,
        radius_segments,
    );



    for ix in 1 .. radius_segments {
        mesh.add_face(
            radius_segments + 1,
            ix,
            ix + 1,
        );
    }
    mesh.add_face(
        radius_segments + 1,
        1,
        radius_segments,
    );


    // for iy in 1 .. height_segments - 1 {
    //     for ix in 1 .. radius_segments - 1 {
    //         // if iy < height_segments - 2 {
    //         //     mesh.add_face(
    //         //         iy * radius_segments + ix,
    //         //         iy * radius_segments + ix + 1,
    //         //         (iy + 1) * radius_segments + ix,
    //         //     );
    //         // }
    //         mesh.add_face(
    //             iy * radius_segments + ix + 1,
    //             (iy + 1) * radius_segments + ix + 1,
    //             (iy + 1) * radius_segments + ix,
    //         );
    //     }
    // }

    // if cap_segments > 0 {
    //     for iy in 0 .. cap_segments - 1 {
    //         for ix in 0 .. radius_segments - 1 {
    //             mesh.add_face(
    //                 iy * radius_segments + ix + cap_start,
    //                 iy + radius_segments + ix + 1 + cap_start,
    //                 (iy + 1) * radius_segments + ix + cap_start,
    //             );
    //
    //             if iy < cap_segments - 2{
    //                 mesh.add_face(
    //                     iy * radius_segments + ix + 1 + cap_start,
    //                     (iy + 1) * radius_segments + ix + 1 + cap_start,
    //                     (iy + 1) * radius_segments + ix + cap_start,
    //                 );
    //             }
    //         }
    //     }
    // }

    mesh
}

/// Returns a cube as a wireframe mesh with position
pub fn cube_wireframe(size: f32) -> Mesh<Vertex3D>{
    let half = size * 0.5;
	Mesh::new(
        vec![
    		vertex3d(vec3( -half,-half,-half )),
            vertex3d(vec3( -half, half,-half )),
            vertex3d(vec3( half,-half,-half )),
            vertex3d(vec3( half, half,-half )),
    		vertex3d(vec3( -half,-half, half )),
            vertex3d(vec3( -half, half, half )),
            vertex3d(vec3( half,-half, half )),
            vertex3d(vec3( half, half, half )),
        ],
        vec![
            0,1, 1,3, 3,2, 2,0,
            0,4, 1,5, 2,6, 3,7,
            4,5, 5,7, 7,6, 6,4,
        ],
        PrimitiveType::Lines
    )
}

/// Returns a cube as a filled mesh with position and normals
pub fn cube(side: f32) -> Mesh<Vertex3DNormal>{
    let top =     Vec3::y();
    let bottom = -Vec3::y();
    let left =   -Vec3::x();
    let right =   Vec3::x();
    let front =   Vec3::z();
    let back =   -Vec3::z();

    let vertices = vec![
        vertex3dnormal(vec3(-0.5, -0.5,  0.5) * side, front),
        vertex3dnormal(vec3( 0.5, -0.5,  0.5) * side, front),
        vertex3dnormal(vec3( 0.5,  0.5,  0.5) * side, front),
        vertex3dnormal(vec3(-0.5,  0.5,  0.5) * side, front),

        vertex3dnormal(vec3(-0.5, -0.5, -0.5) * side, bottom),
        vertex3dnormal(vec3( 0.5, -0.5, -0.5) * side, bottom),
        vertex3dnormal(vec3( 0.5, -0.5,  0.5) * side, bottom),
        vertex3dnormal(vec3(-0.5, -0.5,  0.5) * side, bottom),

        vertex3dnormal(vec3(-0.5, -0.5, -0.5) * side, left),
        vertex3dnormal(vec3(-0.5, -0.5,  0.5) * side, left),
        vertex3dnormal(vec3(-0.5,  0.5,  0.5) * side, left),
        vertex3dnormal(vec3(-0.5,  0.5, -0.5) * side, left),

        vertex3dnormal(vec3( 0.5, -0.5,  0.5) * side, right),
        vertex3dnormal(vec3( 0.5, -0.5, -0.5) * side, right),
        vertex3dnormal(vec3( 0.5,  0.5, -0.5) * side, right),
        vertex3dnormal(vec3( 0.5,  0.5,  0.5) * side, right),

        vertex3dnormal(vec3( 0.5,  0.5, -0.5) * side, top),
        vertex3dnormal(vec3(-0.5,  0.5, -0.5) * side, top),
        vertex3dnormal(vec3(-0.5,  0.5,  0.5) * side, top),
        vertex3dnormal(vec3( 0.5,  0.5,  0.5) * side, top),

        vertex3dnormal(vec3( 0.5, -0.5, -0.5) * side, back),
        vertex3dnormal(vec3(-0.5, -0.5, -0.5) * side, back),
        vertex3dnormal(vec3(-0.5,  0.5, -0.5) * side, back),
        vertex3dnormal(vec3( 0.5,  0.5, -0.5) * side, back),
    ];

    let indices = (0 .. vertices.len())
        .collect::<Vec<_>>()
        .chunks(4)
        .flat_map(|v|{
            vec![
                v[0] as u32, v[1] as u32, v[2] as u32,
                v[0] as u32, v[2] as u32, v[3] as u32
            ]
        })
        .collect::<Vec<_>>();

    Mesh::from_vertices_indices(vertices, indices)
}


/// Returns a cube as a filled mesh with position, colors per vertex and normals
pub fn cube_color<C:ToRgba>(side: f32, color: &C) -> Mesh<Vertex3DColorNormal>{
    let top =     Vec3::y();
    let bottom = -Vec3::y();
    let left =   -Vec3::x();
    let right =   Vec3::x();
    let front =   Vec3::z();
    let back =   -Vec3::z();

    let vertices = vec![
        vertex3dcolornormal(vec3(-0.5, -0.5,  0.5) * side, color, front),
        vertex3dcolornormal(vec3( 0.5, -0.5,  0.5) * side, color, front),
        vertex3dcolornormal(vec3( 0.5,  0.5,  0.5) * side, color, front),
        vertex3dcolornormal(vec3(-0.5,  0.5,  0.5) * side, color, front),

        vertex3dcolornormal(vec3(-0.5, -0.5, -0.5) * side, color, bottom),
        vertex3dcolornormal(vec3( 0.5, -0.5, -0.5) * side, color, bottom),
        vertex3dcolornormal(vec3( 0.5, -0.5,  0.5) * side, color, bottom),
        vertex3dcolornormal(vec3(-0.5, -0.5,  0.5) * side, color, bottom),

        vertex3dcolornormal(vec3(-0.5, -0.5, -0.5) * side, color, left),
        vertex3dcolornormal(vec3(-0.5, -0.5,  0.5) * side, color, left),
        vertex3dcolornormal(vec3(-0.5,  0.5,  0.5) * side, color, left),
        vertex3dcolornormal(vec3(-0.5,  0.5, -0.5) * side, color, left),

        vertex3dcolornormal(vec3( 0.5, -0.5,  0.5) * side, color, right),
        vertex3dcolornormal(vec3( 0.5, -0.5, -0.5) * side, color, right),
        vertex3dcolornormal(vec3( 0.5,  0.5, -0.5) * side, color, right),
        vertex3dcolornormal(vec3( 0.5,  0.5,  0.5) * side, color, right),

        vertex3dcolornormal(vec3( 0.5,  0.5, -0.5) * side, color, top),
        vertex3dcolornormal(vec3(-0.5,  0.5, -0.5) * side, color, top),
        vertex3dcolornormal(vec3(-0.5,  0.5,  0.5) * side, color, top),
        vertex3dcolornormal(vec3( 0.5,  0.5,  0.5) * side, color, top),

        vertex3dcolornormal(vec3( 0.5, -0.5, -0.5) * side, color, back),
        vertex3dcolornormal(vec3(-0.5, -0.5, -0.5) * side, color, back),
        vertex3dcolornormal(vec3(-0.5,  0.5, -0.5) * side, color, back),
        vertex3dcolornormal(vec3( 0.5,  0.5, -0.5) * side, color, back),
    ];

    let indices = (0 .. vertices.len())
        .collect::<Vec<_>>()
        .chunks(4)
        .flat_map(|v|{
            vec![
                v[0] as u32, v[1] as u32, v[2] as u32,
                v[0] as u32, v[2] as u32, v[3] as u32
            ]
        })
        .collect::<Vec<_>>();

    Mesh::from_vertices_indices(vertices, indices)
}


/// Returns a cuboid as a filled mesh with position and normals
pub fn cuboid(extents: Vec3) -> Mesh<Vertex3DNormal>{
    let top =     Vec3::y();
    let bottom = -Vec3::y();
    let left =   -Vec3::x();
    let right =   Vec3::x();
    let front =   Vec3::z();
    let back =   -Vec3::z();

    let vertices = vec![
        vertex3dnormal(vec3(-0.5, -0.5,  0.5).component_mul(&extents), front),
        vertex3dnormal(vec3( 0.5, -0.5,  0.5).component_mul(&extents), front),
        vertex3dnormal(vec3( 0.5,  0.5,  0.5).component_mul(&extents), front),
        vertex3dnormal(vec3(-0.5,  0.5,  0.5).component_mul(&extents), front),

        vertex3dnormal(vec3(-0.5, -0.5, -0.5).component_mul(&extents), bottom),
        vertex3dnormal(vec3( 0.5, -0.5, -0.5).component_mul(&extents), bottom),
        vertex3dnormal(vec3( 0.5, -0.5,  0.5).component_mul(&extents), bottom),
        vertex3dnormal(vec3(-0.5, -0.5,  0.5).component_mul(&extents), bottom),

        vertex3dnormal(vec3(-0.5, -0.5, -0.5).component_mul(&extents), left),
        vertex3dnormal(vec3(-0.5, -0.5,  0.5).component_mul(&extents), left),
        vertex3dnormal(vec3(-0.5,  0.5,  0.5).component_mul(&extents), left),
        vertex3dnormal(vec3(-0.5,  0.5, -0.5).component_mul(&extents), left),

        vertex3dnormal(vec3( 0.5, -0.5,  0.5).component_mul(&extents), right),
        vertex3dnormal(vec3( 0.5, -0.5, -0.5).component_mul(&extents), right),
        vertex3dnormal(vec3( 0.5,  0.5, -0.5).component_mul(&extents), right),
        vertex3dnormal(vec3( 0.5,  0.5,  0.5).component_mul(&extents), right),

        vertex3dnormal(vec3( 0.5,  0.5, -0.5).component_mul(&extents), top),
        vertex3dnormal(vec3(-0.5,  0.5, -0.5).component_mul(&extents), top),
        vertex3dnormal(vec3(-0.5,  0.5,  0.5).component_mul(&extents), top),
        vertex3dnormal(vec3( 0.5,  0.5,  0.5).component_mul(&extents), top),

        vertex3dnormal(vec3( 0.5, -0.5, -0.5).component_mul(&extents), back),
        vertex3dnormal(vec3(-0.5, -0.5, -0.5).component_mul(&extents), back),
        vertex3dnormal(vec3(-0.5,  0.5, -0.5).component_mul(&extents), back),
        vertex3dnormal(vec3( 0.5,  0.5, -0.5).component_mul(&extents), back),
    ];

    let indices = (0 .. vertices.len())
        .collect::<Vec<_>>()
        .chunks(4)
        .flat_map(|v|{
            vec![
                v[0] as u32, v[1] as u32, v[2] as u32,
                v[0] as u32, v[2] as u32, v[3] as u32
            ]
        })
        .collect::<Vec<_>>();

    Mesh::from_vertices_indices(vertices, indices)
}


/// Returns a cuboid as a filled mesh with position, colors per vertex and normals
pub fn cuboid_color<C:ToRgba>(extents: Vec3, color: &C) -> Mesh<Vertex3DColorNormal>{
    let top =     Vec3::y();
    let bottom = -Vec3::y();
    let left =   -Vec3::x();
    let right =   Vec3::x();
    let front =   Vec3::z();
    let back =   -Vec3::z();

    let vertices = vec![
        vertex3dcolornormal(vec3(-0.5, -0.5,  0.5).component_mul(&extents), color, front),
        vertex3dcolornormal(vec3( 0.5, -0.5,  0.5).component_mul(&extents), color, front),
        vertex3dcolornormal(vec3( 0.5,  0.5,  0.5).component_mul(&extents), color, front),
        vertex3dcolornormal(vec3(-0.5,  0.5,  0.5).component_mul(&extents), color, front),

        vertex3dcolornormal(vec3(-0.5, -0.5, -0.5).component_mul(&extents), color, bottom),
        vertex3dcolornormal(vec3( 0.5, -0.5, -0.5).component_mul(&extents), color, bottom),
        vertex3dcolornormal(vec3( 0.5, -0.5,  0.5).component_mul(&extents), color, bottom),
        vertex3dcolornormal(vec3(-0.5, -0.5,  0.5).component_mul(&extents), color, bottom),

        vertex3dcolornormal(vec3(-0.5, -0.5, -0.5).component_mul(&extents), color, left),
        vertex3dcolornormal(vec3(-0.5, -0.5,  0.5).component_mul(&extents), color, left),
        vertex3dcolornormal(vec3(-0.5,  0.5,  0.5).component_mul(&extents), color, left),
        vertex3dcolornormal(vec3(-0.5,  0.5, -0.5).component_mul(&extents), color, left),

        vertex3dcolornormal(vec3( 0.5, -0.5,  0.5).component_mul(&extents), color, right),
        vertex3dcolornormal(vec3( 0.5, -0.5, -0.5).component_mul(&extents), color, right),
        vertex3dcolornormal(vec3( 0.5,  0.5, -0.5).component_mul(&extents), color, right),
        vertex3dcolornormal(vec3( 0.5,  0.5,  0.5).component_mul(&extents), color, right),

        vertex3dcolornormal(vec3( 0.5,  0.5, -0.5).component_mul(&extents), color, top),
        vertex3dcolornormal(vec3(-0.5,  0.5, -0.5).component_mul(&extents), color, top),
        vertex3dcolornormal(vec3(-0.5,  0.5,  0.5).component_mul(&extents), color, top),
        vertex3dcolornormal(vec3( 0.5,  0.5,  0.5).component_mul(&extents), color, top),

        vertex3dcolornormal(vec3( 0.5, -0.5, -0.5).component_mul(&extents), color, back),
        vertex3dcolornormal(vec3(-0.5, -0.5, -0.5).component_mul(&extents), color, back),
        vertex3dcolornormal(vec3(-0.5,  0.5, -0.5).component_mul(&extents), color, back),
        vertex3dcolornormal(vec3( 0.5,  0.5, -0.5).component_mul(&extents), color, back),
    ];

    let indices = (0 .. vertices.len())
        .collect::<Vec<_>>()
        .chunks(4)
        .flat_map(|v|{
            vec![
                v[0] as u32, v[1] as u32, v[2] as u32,
                v[0] as u32, v[2] as u32, v[3] as u32
            ]
        })
        .collect::<Vec<_>>();

    Mesh::from_vertices_indices(vertices, indices)
}

/// Returns a cube as a wireframe mesh with position
pub fn cuboid_wireframe(extents: Vec3) -> Mesh<Vertex3D>{
    let half_w = extents.x * 0.5;
    let half_h = extents.y * 0.5;
    let half_d = extents.z * 0.5;
	Mesh::new(
        vec![
    		vertex3d(vec3( -half_w,-half_h,-half_d )),
            vertex3d(vec3( -half_w, half_h,-half_d )),
            vertex3d(vec3(  half_w,-half_h,-half_d )),
            vertex3d(vec3(  half_w, half_h,-half_d )),
    		vertex3d(vec3( -half_w,-half_h, half_d )),
            vertex3d(vec3( -half_w, half_h, half_d )),
            vertex3d(vec3(  half_w,-half_h, half_d )),
            vertex3d(vec3(  half_w, half_h, half_d )),
        ],
        vec![
            0,1, 1,3, 3,2, 2,0,
            0,4, 1,5, 2,6, 3,7,
            4,5, 5,7, 7,6, 6,4,
        ],
        PrimitiveType::Lines
    )
}

fn plane_indices(rows: IndexT, cols: IndexT) -> Vec<IndexT>{
    (0..rows-1).flat_map(move |row|{
        (0..cols-1).flat_map(move |col|{
            vec![
                row * cols + col,
                row * cols + col + 1,
                (row + 1) * cols + col,

                row * cols + (col + 1),
                (row + 1) * cols + (col + 1),
                (row + 1) * cols + col,
            ]
        })
    }).collect()
}

/// Returns a plane as a filled mesh with position and normals
pub fn plane(size: Vec2, rows: IndexT, cols: IndexT) -> Mesh<Vertex3DNormal>{
    let half = size * 0.5;
    let rows = rows + 1;
    let cols = cols + 1;
    let vertices = (0..rows).flat_map(move |row|{
        (0..cols).map(move |col|{
            let x = col as f32 / (cols - 1) as f32;
            let y = row as f32 / (rows - 1) as f32;
            let pos = vec2(x, y);
            let pos = vec3!(pos.component_mul(&size) - half, 0.);
            vertex3dnormal(pos, Vec3::z())
        })
    }).collect();

    let indices = plane_indices(rows, cols);

    Mesh::from_vertices_indices(vertices, indices)
}

/// Returns a plane as a filled mesh with position, colors per vertex and normals
pub fn plane_color<C: ToRgba>(size: Vec2, rows: IndexT, cols: IndexT, color: &C) -> Mesh<Vertex3DColorNormal>{
    let half = size * 0.5;
    let rows = rows + 1;
    let cols = cols + 1;
    let vertices = (0..rows).flat_map(move |row|{
        (0..cols).map(move |col|{
            let x = col as f32 / (cols - 1) as f32;
            let y = row as f32 / (rows - 1) as f32;
            let pos = vec2(x, y);
            let pos = vec3!(pos.component_mul(&size) - half, 0.);
            vertex3dcolornormal(pos, color, Vec3::z())
        })
    }).collect();

    let indices = plane_indices(rows, cols);

    Mesh::from_vertices_indices(vertices, indices)
}


/// Returns a plane as a filled mesh with position, texture coordinates and normals
pub fn plane_texcoords(size: Vec2, rows: IndexT, cols: IndexT) -> Mesh<Vertex3DTexNormal>{
    let half = size * 0.5;
    let rows = rows + 1;
    let cols = cols + 1;
    let vertices = (0..rows).flat_map(move |row|{
        (0..cols).map(move |col|{
            let x = col as f32 / (cols - 1) as f32;
            let y = row as f32 / (rows - 1) as f32;
            let texcoord = vec2(x,y);
            let pos = vec3!(texcoord.component_mul(&size) - half, 0.);
            vertex3dtexnormal(pos, texcoord, Vec3::z())
        })
    }).collect();

    let indices = plane_indices(rows, cols);

    Mesh::from_vertices_indices(vertices, indices)
}

/// Returns a plane as a filled mesh with position and normals but no indices
pub fn plane_no_indices(size: Vec2, rows: IndexT, cols: IndexT) -> Mesh<Vertex3DNormal>{
    let plane = plane(size, rows, cols);
    plane.indices().iter().map(|i| plane[*i as usize]).collect()
}

/// Returns a plane as a filled mesh with position texture coordinates and normals but no indices
pub fn plane_texcoords_no_indices(size: Vec2, rows: IndexT, cols: IndexT) -> Mesh<Vertex3DTexNormal>{
    let plane = plane_texcoords(size, rows, cols);
    plane.indices().iter().map(|i| plane[*i as usize]).collect()
}

/// Returns a plane as a wireframe mesh with position only
pub fn plane_wireframe(size: Vec2, rows: IndexT, cols: IndexT) -> Mesh<Vertex3D>{
    let half = size * 0.5;
    let rows = rows + 1;
    let cols = cols + 1;
    let vertices = (0..rows).flat_map(move |row|{
        (0..cols).map(move |col|{
            let x = col as f32 / (cols - 1) as f32;
            let y = row as f32 / (rows - 1) as f32;
            let pos = vec2(x, y);
            let pos = vec3!(pos.component_mul(&size) - half, 0.);
            vertex3d(pos)
        })
    }).collect();

    let indices = (0..rows-1).flat_map(move |row|{
        (0..cols-1).flat_map(move |col|{
            vec![
                row * cols + col,
                row * cols + col +1,

                row * cols + col +1,
                (row + 1) * cols + (col + 1),


                (row + 1) * cols + (col + 1),
                (row + 1) * cols + col,

                (row + 1) * cols + col,
                row * cols + col,
            ]
        })
    }).collect();

    Mesh::new(vertices, indices, PrimitiveType::Lines)
}

pub fn bezier<Point, F>(from: Point, cp1: Point, cp2: Point, to: Point, resolution: u32, mut f: F)
where
    Point: FloatPnt + Copy,
    <Point as NumPnt>::Field: RealField + NumCast,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates> + Copy,
    F: FnMut(Point)
{
    let _3 = cast(3.0).unwrap();
    let c = (cp1 - from) * _3;
    let b = (cp2 - cp1) * _3 - c;
    let a = to - from - c - b;

    for i in 0 .. resolution+1{
        let t = cast(i as f32 / resolution as f32).unwrap();
        let t2 = t*t;
        let t3 = t2*t;
        let p = from + (a*t3 + b*t2 + c*t);
        f(p);
    }
}

pub struct BezierIter<Point: FloatPnt>
where
    <Point as NumPnt>::Field: RealField,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates>
{
    a: <Point as NumPnt>::Coordinates,
    b: <Point as NumPnt>::Coordinates,
    c: <Point as NumPnt>::Coordinates,
    from: Point,
    i: u32,
    resolution: u32,
}

impl<Point> Iterator for BezierIter<Point>
where
    Point: FloatPnt + Copy,
    <Point as NumPnt>::Field: RealField + NumCast,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates> + Copy
{
    type Item = Point;
    fn next(&mut self) -> Option<Point>{
        if self.i <= self.resolution{
            let t = cast(self.i as f64 / self.resolution as f64).unwrap();
            let t2 = t*t;
            let t3 = t2*t;
            self.i += 1;
            Some(self.from + (self.a*t3 + self.b*t2 + self.c*t))
        }else{
            None
        }
    }
}

/// Returns a bezier iterator with the passed parameter
///
/// The iterator can be used for eaxmple on a loop to go through all the vertices of the bezier
pub fn bezier_iter<Point>(from: Point, cp1: Point, cp2: Point, to: Point, resolution: u32) -> BezierIter<Point>
where
    <Point as NumPnt>::Field: RealField + NumCast + Copy,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates> + Copy,
    Point: FloatPnt + Copy
{
    let _3 = cast(3.0).unwrap();
    let c = (cp1 - from) * _3;
    let b = (cp2 - cp1) * _3 - c;
    let a = to - from - c - b;
    BezierIter{
        i: 0,
        c: c,
        b: b,
        a: a,
        from,
        resolution: resolution,
    }
}

// primitives
pub fn quad_bezier<Point, F>(from: Point, cp1: Point, to: Point, resolution: u32, mut f: F)
where
    <Point as NumPnt>::Field: RealField + NumCast,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates>,
    Point: FloatPnt + Copy,
    F: FnMut(Point)
{
    let _2: <Point as NumPnt>::Field = cast(2.0).unwrap();
    let _1: <Point as NumPnt>::Field = one();
    for i in 0 .. resolution+1{
        let t = cast(i as f32 / resolution as f32).unwrap();
        let one_minus_t = _1 - t;
        let a = one_minus_t * one_minus_t;
        let b = (_2 - t) * one_minus_t;
        let c = t*t;
        let p = from * a + (cp1 * b).coordinates() + (to * c).coordinates();
        f(p)
    }
}

struct CubicPolynomial<T>{
    a: T,
    b: T,
    c: T,
    d: T,
}

impl<Point> CubicPolynomial<Point>
where
Point: FloatPnt + Copy,
<Point as NumPnt>::Field: RealField + NumCast + Float,
<Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates> + Copy + ToPnt<Point>,
{
    fn calc(&self, t: <Point as NumPnt>::Field) -> Point {
        let t2 = t * t;
        let t3 = t2 * t;
        self.a + (self.b * t).coordinates() + (self.c * t2).coordinates() + (self.d * t3).coordinates()
    }

    fn new(x0: Point, x1: Point, t0: <Point as NumPnt>::Coordinates, t1: <Point as NumPnt>::Coordinates) -> CubicPolynomial<Point> {
        let three: <Point as NumPnt>::Field = cast(3).unwrap();
        CubicPolynomial{
            a: x0,
            b: t0.to_pnt(),
            c: x0 * cast(-3).unwrap() + (x1 * three).coordinates() - t0 * cast(2).unwrap() - t1,
            d: (x0 * cast(2).unwrap() -  x1 * cast(2).unwrap() + t0 + t1).to_pnt(),
        }
    }

    fn non_uniform_catmullrom(
        x0: Point,
        x1: Point,
        x2: Point,
        x3: Point,
        dt0: <Point as NumPnt>::Field,
        dt1: <Point as NumPnt>::Field,
        dt2: <Point as NumPnt>::Field) -> CubicPolynomial<Point>
    {
        let mut t1 = (x1 - x0) / dt0 - (x2 - x0) / (dt0 + dt1) + (x2 - x1) / dt1;
        let mut t2 = (x2 - x1) / dt1 - (x3 - x1) / (dt1 + dt2) + (x3 - x2) / dt2;

        t1 *= dt1;
        t2 *= dt1;


        CubicPolynomial::new(x1, x2, t1, t2)
    }

    fn uniform_catmullrom(x0: Point, x1: Point, x2: Point, x3: Point, alpha: <Point as NumPnt>::Field) -> CubicPolynomial<Point>{
        CubicPolynomial::new(
            x1,
            x2,
            (x2 - x0) * alpha,
            (x3 - x1) * alpha,
        )
    }
}

pub struct CatmullRomIter<'a, Point: FloatPnt>
where
    <Point as NumPnt>::Field: RealField,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates>
{
    points: &'a [Point],
    closed: bool,
    t: <Point as NumPnt>::Field,
    curve_ty: CatmullRom<<Point as NumPnt>::Field>,
    resolution: u32,
    prev_i: Option<usize>,
    polynomial: Option<CubicPolynomial<Point>>,
}

impl<'a, Point> Iterator for CatmullRomIter<'a, Point>
where
    Point: FloatPnt + Copy,
    <Point as NumPnt>::Field: RealField + NumCast + Float,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates> + Copy + ToPnt<Point>,
{
    type Item = Point;
    fn next(&mut self) -> Option<Point>{
        if self.t > one() {
            return None
        }

        let l: <Point as NumPnt>::Field = cast(self.points.len()).unwrap();
        let fi = (l -  if self.closed{ zero() } else { one() }) * self.t;
        let i = fi.floor();
        let weight = fi - i;
        let i: usize = cast(i).unwrap();

        if Some(i) != self.prev_i {
            let p0 = if self.closed || i > 0 {
                self.points[(i - 1) % self.points.len()]
            }else{
                (self.points[0] - self.points[1] + self.points[0].coordinates()).to_pnt()
            };
            let p1 = self.points[i % self.points.len()];
            let p2 = self.points[(i + 1) % self.points.len()];
            let p3 = if self.closed || i + 2 < self.points.len() {
                self.points[(i + 2) % self.points.len()]
            }else if i + 1 < self.points.len(){
                (self.points[i + 1]  - self.points[i] + self.points[i + 1].coordinates()).to_pnt()
            }else{
                self.points[i]
            };
            let alpha = self.curve_ty.alpha();

            if self.curve_ty.is_uniform() {
                self.polynomial = Some(CubicPolynomial::uniform_catmullrom(p0, p1, p2, p3, alpha));
            }else{
                let mut dt0 = p0.distance_squared(&p1).powf(alpha);
                let mut dt1 = p1.distance_squared(&p2).powf(alpha);
                let mut dt2 = p2.distance_squared(&p3).powf(alpha);

                if dt1 < <Point as NumPnt>::Field::epsilon() {
                    dt1 = cast(1.).unwrap();
                }
                if dt0 < <Point as NumPnt>::Field::epsilon() {
                    dt0 = dt1;
                }
                if dt2 < <Point as NumPnt>::Field::epsilon() {
                    dt2 = dt1;
                }
                self.polynomial = Some(CubicPolynomial::non_uniform_catmullrom(p0, p1, p2, p3, dt0, dt1, dt2));
            }
        }

        self.t += one::<<Point as NumPnt>::Field>() / cast(self.resolution).unwrap();
        Some(self.polynomial.as_ref().unwrap().calc(weight))
    }
}

#[derive(Clone,Copy,Debug)]
pub enum CatmullRom<T>{
    Centripetal,
    Chordal,
    CustomNonUniform(T),
    Uniform(T),
}

impl<T: NumCast + RealField> CatmullRom<T>{
    pub fn is_uniform(self) -> bool {
        match self{
            CatmullRom::Centripetal | CatmullRom::Chordal | CatmullRom::CustomNonUniform(_) => false,
            CatmullRom::Uniform(_) => true,
        }
    }

    pub fn alpha(self) -> T {
        match self{
            CatmullRom::Centripetal => cast(0.25).unwrap(),
            CatmullRom::Chordal => cast(0.5).unwrap(),
            CatmullRom::CustomNonUniform(alpha) => alpha * cast(0.5).unwrap(),
            CatmullRom::Uniform(alpha) => alpha,
        }
    }
}

pub fn catmull_rom<Point>(p0: &Point, p1: &Point, p2: &Point, p3: &Point, alpha: <Point as NumPnt>::Field, pct: <Point as NumPnt>::Field) -> Point
where
    Point: FloatPnt + Copy,
    <Point as NumPnt>::Field: RealField + NumCast + Float,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates> + Copy + ToPnt<Point>,
{
    let mut dt0 = p0.distance_squared(p1).powf(alpha);
    let mut dt1 = p1.distance_squared(p2).powf(alpha);
    let mut dt2 = p2.distance_squared(p3).powf(alpha);

    if dt1 < <Point as NumPnt>::Field::epsilon() {
        dt1 = cast(1.).unwrap();
    }
    if dt0 < <Point as NumPnt>::Field::epsilon() {
        dt0 = dt1;
    }
    if dt2 < <Point as NumPnt>::Field::epsilon() {
        dt2 = dt1;
    }
    let polynomial = CubicPolynomial::non_uniform_catmullrom(*p0, *p1, *p2, *p3, dt0, dt1, dt2);
    polynomial.calc(pct)
}

pub fn catmull_rom_vertices<Point, F>(p0: &Point, p1: &Point, p2: &Point, p3: &Point, alpha: <Point as NumPnt>::Field, resolution: u32, mut f: F)
where
    Point: FloatPnt + Copy,
    <Point as NumPnt>::Field: RealField + NumCast + Float,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates> + Copy + ToPnt<Point>,
    F: FnMut(Point)
{
    let polynomial = CubicPolynomial::uniform_catmullrom(*p0, *p1, *p2, *p3, alpha);

    let fres: <Point as NumPnt>::Field = cast(resolution).unwrap();
    for i in 0 ..= resolution {
        let fi: <Point as NumPnt>::Field  = cast(i).unwrap();
        let pct = fi / fres;
        f(polynomial.calc(pct))
    }
}

pub fn catmull_rom_iter<Point>(points: &[Point], curve_ty: CatmullRom<<Point as NumPnt>::Field>, resolution: u32) -> Option<CatmullRomIter<Point>>
where
    Point: FloatPnt + Copy,
    <Point as NumPnt>::Field: RealField + NumCast,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates> + Copy + ToPnt<Point>
{
    if points.len() < 2 {
        None
    }else{
        Some(CatmullRomIter{
            points,
            closed: false,
            t: zero(),
            curve_ty,
            resolution,
            polynomial: None,
            prev_i: None,
        })
    }
}

pub fn star_vertices<Point, F>(
    center: Point,
    r: <Point as NumPnt>::Field,
    ri: <Point as NumPnt>::Field,
    points: u32,
    mut f: F)
where
    u32: SubsetOf<<Point as NumPnt>::Field>,
    Point: FloatPnt + Copy,
    <Point as NumPnt>::Field: RealField + NumCast + Float,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates>,
    F: FnMut(Point)
{
    let two_pi: Rad<<Point as NumPnt>::Field> = Rad::two_pi();
    let points_two: <Point as NumPnt>::Field = convert(points*2);
    let alpha: Rad<<Point as NumPnt>::Field> = two_pi / points_two;
    let mut yr = Point::origin().coordinates();
    yr[1] = From::from(r);
    f(center + yr);

    for i in (1 .. points*2).rev() {
        let ra = if i % 2 == 1 {ri} else {r};

        let float_i: <Point as NumPnt>::Field = cast(i as f32).unwrap();
        let omega = alpha * float_i;
        let mut v = Point::origin().coordinates();
        v[0] = From::from(ra * omega.sin());
        v[1] = From::from(ra * omega.cos());
        f(center + v);
    }
}

pub struct ArcIter<Point: FloatPnt>
where
    <Point as NumPnt>::Field: RealField,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates>
{
    i: u32,
    resolution: u32,
    center:  Point,
    rx: <Point as NumPnt>::Field,
    ry: <Point as NumPnt>::Field,
    alpha: Rad<<Point as NumPnt>::Field>,
    offset: Rad<<Point as NumPnt>::Field>,
}

impl<Point> Iterator for ArcIter<Point>
where
    <Point as NumPnt>::Field: RealField + Float + NumCast,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates>,
    Point: FloatPnt + Copy,
{
    type Item = Point;
    fn next(&mut self) -> Option<Point>{
        if self.i <= self.resolution {
            let float_i: <Point as NumPnt>::Field = cast(self.i).unwrap();
            let omega = self.offset + self.alpha * float_i;
            let mut v = Point::origin().coordinates();
            v[0] = From::from(self.rx * omega.sin());
            v[1] = From::from(self.ry * omega.cos());
            self.i += 1;
            Some(self.center + v)
        }else{
            None
        }
    }
}

pub fn arc_iter<Point, A1, A2>(
    center:  Point,
    rx: <Point as NumPnt>::Field,
    ry: <Point as NumPnt>::Field,
    init_angle: A1,
    angle: A2,
    resolution: u32) -> ArcIter<Point>

where
    Point: FloatPnt,
    <Point as NumPnt>::Field: RealField + NumCast,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates>,
    A1: Angle<<Point as NumPnt>::Field>,
    A2: Angle<<Point as NumPnt>::Field>,
{
    let float_resolution: <Point as NumPnt>::Field = cast(resolution).unwrap();
    let alpha = angle.to_rad() / float_resolution;
    let offset = init_angle.to_rad() + Rad::half_pi();
    ArcIter{
        i: 0,
        resolution: resolution,
        center: center,
        rx: rx,
        ry: ry,
        alpha: alpha,
        offset: offset,
    }
}

pub fn arc_vertices<Point, F> (
    center:  Point,
    rx: <Point as NumPnt>::Field,
    ry: <Point as NumPnt>::Field,
    init_angle: Deg<<Point as NumPnt>::Field>,
    angle: Deg<<Point as NumPnt>::Field>,
    resolution: u32,
    mut f: F)

where
    <Point as NumPnt>::Field: RealField + Float + NumCast,
    <Point as NumPnt>::Coordinates: Neg<Output = <Point as NumPnt>::Coordinates>,
    Point: FloatPnt + Copy,
    F: FnMut(Point)
{
    let resolution = resolution + 1;
    let float_resolution: <Point as NumPnt>::Field = cast((resolution - 1) as f32).unwrap();
    let alpha = angle / float_resolution;
    let offset = init_angle + Deg::half_pi();

    for i in (0 .. resolution).rev() {
        let float_i: <Point as NumPnt>::Field = cast(i as f32).unwrap();
        let omega = offset + alpha * float_i;
        let mut v = Point::origin().coordinates();
        v[0] = From::from(rx * omega.sin());
        v[1] = From::from(ry * omega.cos());
        f(center + v);
    }
}

pub fn sphere_vertices<F>(rings: u32, sectors: u32, mut emit_vertex: F)
	where F: FnMut(Vec3,u32,u32) {
    let inv_r = 1.0/(rings) as f64;
    let inv_s = 1.0/(sectors) as f64;

    for r in 0 .. rings+1{
        let y = ( -f64::consts::PI*0.5 + f64::consts::PI * r as f64 * inv_r ).sin();
        let tr = ( f64::consts::PI * r as f64 * inv_r ).sin();
        for s in 0 .. sectors {
            let x = ( 2.0*f64::consts::PI * s as f64 * inv_s).cos() * tr;
            let z = ( 2.0*f64::consts::PI * s as f64 * inv_s).sin() * tr;

            emit_vertex( vec3(x as f32, y as f32, z as f32),r,s );
        }
        emit_vertex( vec3(tr as f32, y as f32, 0.0),r,sectors );
    }
}


pub fn cone_vertices<F>(radius_segments: u32, height_segments: u32, mut emit_vertex: F)
    where F: FnMut(Vec3,u32,u32)
{
    const TWO_PI: f32 = 2.0 * f32::consts::PI;
    let angle_step = -1. * TWO_PI / (radius_segments - 1) as f32;
    let height_step = 1. / (height_segments - 1) as f32;
    for iy in 0 .. height_segments {
        for ix in 0 .. radius_segments {
            let r = iy as f32 / (height_segments - 1) as f32;
            let x = (ix as f32 * angle_step) * r;
            let y = (height_step * iy as f32).cos() - 0.5;
            let z = (ix as f32 * angle_step) * r;
            emit_vertex(vec3!(x,y,z),ix,iy);
        }
    }
}

pub fn cone_cap_vertices<F>(radius_segments: u32, cap_segments: u32, mut emit_vertex: F)
    where F: FnMut(Vec3,u32,u32)
{
    const TWO_PI: f32 = 2.0 * f32::consts::PI;
    let angle_step = -1. * TWO_PI / (radius_segments - 1) as f32;
    for iy in 0 .. cap_segments {
        for ix in 0 .. radius_segments {
            let r = 1. - iy as f32 / (cap_segments - 1) as f32;
            let x = (ix as f32 * angle_step).cos() * r;
            let y = 0.5;
            let z = (ix as f32 * angle_step).sin() * r;
            emit_vertex(vec3(x,y,z), ix, iy);
        }
    }
}

pub fn lathed(
    axis: &Unit<Vec3>,
    arm1: &Unit<Vec3>,
    arm2: &Unit<Vec3>,
    resolution: u32,
    points: &[Vec2]) -> Mesh<Vec3>
{
    const TAU: f32 = 6.28318530718;
    let mut mesh = Mesh::default();
    for i in 0 ..= resolution {
        let angle = (i % resolution) as f32 * TAU / resolution as f32 + TAU / 8.;
        let c = angle.cos();
        let s = angle.sin();
        let mat: Matrix3x2<f32> = Matrix3x2::from_columns(&[
            axis.into_inner(),
            arm1.into_inner() * c + arm2.into_inner() * s
        ]);
        let vertices = points.iter().map(|p| mat * p);
        mesh.extend(vertices);
        if i > 0 {
            let indices = (1 .. points.len() as u32).flat_map(|j| {
                let i0 = (i - 1) * points.len() as u32 + (j - 1);
                let i1 = (i - 0) * points.len() as u32 + (j - 1);
                let i2 = (i - 0) * points.len() as u32 + (j - 0);
                let i3 = (i - 1) * points.len() as u32 + (j - 0);
                vec![i0, i1, i2, i0, i2, i3]
            });
            mesh.indices_mut().extend(indices);
        };
    }
    mesh
}

fn recalculate_smooth_normals(mesh: &mut Mesh<Vertex3DNormal>){
    for v in mesh.iter_mut(){
        v.normal = zero();
    }

    for face in mesh.faces_mut(){
        let a = face[1].position - face[0].position;
        let b = face[2].position - face[0].position;
        let normal = a.cross(&b);
        face[0].normal += normal;
        face[1].normal += normal;
        face[2].normal += normal;
    }

    for v in mesh.iter_mut(){
        let normal = v.normal.normalize();
        v.normal = normal;
    }
}

fn _recalculate_flat_normals(mesh: &mut Mesh<Vertex3DNormal>){
    for face in mesh.faces_mut(){
        let a = face[1].position - face[0].position;
        let b = face[2].position - face[0].position;
        let normal = a.cross(&b);
        face[0].normal = normal;
        face[1].normal = normal;
        face[2].normal = normal;
    }
}

pub fn torus(radius_inner: f32, radius_outter: f32, segment_resolution: u32, segments: u32) -> Mesh<Vertex3DNormal>{
    let circle_r = (radius_outter - radius_inner) * 0.5;
    let circle_y = radius_inner + circle_r;
    let circle: Vec<Vec2> = circle(&pnt2(0., circle_y), circle_r, segment_resolution).into();
    let mesh = lathed(
        &Vec3::z_axis(),
        &Vec3::x_axis(),
        &Vec3::y_axis(),
        segments,
        &circle);
    let (vertices, indices) = mesh.into();
    let vertices = vertices.into_iter()
        .map(|v| vertex3dnormal(v, unsafe{ mem::MaybeUninit::uninit().assume_init() }))
        .collect();
    let mut mesh = Mesh::from_vertices_indices(vertices, indices);
    recalculate_smooth_normals(&mut mesh);
    mesh
}

pub fn cylinder(radius: f32, height: f32, segments_radius: u32, segments_height: u32) -> Mesh<Vertex3DNormal>{
    assert!(segments_radius > 0);
    assert!(segments_height > 0);

    let circle = arc_iter(
        pnt2!(0.),
        radius,
        radius,
        Deg(0.),
        Deg::two_pi(),
        segments_radius
    ).collect::<Vec<_>>();

    let bottom_center = vec3!(0.);
    let top_center = vec3!(0., 0., height);

    let vertices = (0..=segments_height)
        .zip(std::iter::repeat(&circle))
        .flat_map(move |(i, circle)| {
            let z = height * (i as f32 / segments_height as f32);
            circle.iter().map(move |v| vertex3dnormal(vec3!(v.x, v.y, z), vec3!(v.x, v.y, 0.).normalize()))
        })
        .chain(Some(vertex3dnormal(bottom_center, Vec3::z())))
        .chain(circle.iter().map(|v| vertex3dnormal(vec3!(v.x, v.y, 0.), Vec3::z())))
        .chain(Some(vertex3dnormal(top_center, -Vec3::z())))
        .chain(circle.iter().map(|v| vertex3dnormal(vec3!(v.x, v.y, height), -Vec3::z())))
        .collect();

    let indices = (0..segments_height).flat_map(move |s| {
        let min = segments_radius * s;
        let max = segments_radius * (s + 1);
        (0..=segments_radius).flat_map(move |i| vec![
            i + min,
            iwrap(i as i32 - 1, min as i32, max as i32) as IndexT + min,
            i + segments_radius + min,

            i + min,
            i + segments_radius + min,
            iwrap(i as i32 + 1, min as i32, max as i32) as IndexT + segments_radius + min,
        ])
    }).chain((0..=segments_radius).flat_map(|i| {
        let bottom_cap_offset =  segments_radius * segments_height;
        vec![
            bottom_cap_offset,
            i + 1 + bottom_cap_offset,
            iwrap(i as i32, 1 as i32, segments_radius as i32 + 1) as IndexT + bottom_cap_offset,
        ]
    })).chain((0..=segments_radius).flat_map(|i| {
        let top_cap_offset =  segments_radius * (segments_height + 1) + 1;
        vec![
            top_cap_offset,
            i + 1 + top_cap_offset,
            iwrap(i as i32, 1 as i32, segments_radius as i32 + 1) as IndexT + top_cap_offset,
        ]
    }))
    .collect();

    Mesh::new(vertices, indices, PrimitiveType::Triangles)
}