// #![doc(html_logo_url = "https://rin.rs/logo.svg")]
// #![doc(html_favicon_url = "https://rin.rs/favicon.ico")]
/*!
 * This module mostly re-exports na (a graphics oriented wrapper for nalgebra)
 * angle (a type safe wrapper for angle measseures) and adds a few simple functions
 * useful for graphics math
 */
pub use na::*;
pub use na::{Identity, Multiplicative, Additive};
pub use angle::{Angle, Deg, Rad, cast as angle_cast};
use alga;
use std::f64;
use num_traits::{self, Float, NumCast, Zero};
use std::mem;
use std::ops::{Add,Sub,Mul,Div,ShlAssign};

pub use self::polyline::Polyline;
pub use self::rectangle::{Rect, InsideRect};

mod polyline;
mod rectangle;

/// Adds a 3d translation to a Mat4
pub fn add_translation<T: alga::general::Real + BaseNum>(mat: &Mat4<T>, t: &Vec3<T>) -> Mat4<T>{
    let trans_mat = Isometry3::from_parts(Translation::from(t.clone()), one()).to_homogeneous();
    *mat * trans_mat
}


// TODO: this should be an angle method in vec2 and pnt2
pub fn atan2<T: Float + BaseNum>(v1: &Vec2<T>, v2: &Vec2<T>) -> Rad<T>{
    Rad((v1.x * v2.y - v1.y * v2.x).atan2( v1.x * v2.x + v1.y * v2.y ))
}

/// Linear interpolation
#[inline]
pub fn lerp<U: alga::general::Real, T: Add<T, Output = T> + Mul<U, Output = T>>(p0: T, p1: T, pct: U) -> T{
    p0 * (one::<U>() - pct) + p1 * pct
}

/// Bezier interpolation from `from` to `to` using the control points
/// `cp1` and `cp2` at the normalized distance `pct`
pub fn bezier_interpolate<T, U>(from: U, cp1: U, cp2: U, to: U, pct: T) -> U
	where T: alga::general::Real + NumCast,
		  U: Sub<U, Output=U> + Mul<T, Output=U> + Add<U, Output=U> + Copy{
    let c:U = (cp1 - from) * num_traits::cast::<f64,T>(3.0).unwrap();
    let b:U = (cp2 - cp1) * num_traits::cast::<f64,T>(3.0).unwrap() - c;
    let a:U = to - from - c - b;

    let t = pct;
    let t2 = t*t;
    let t3 = t2*t;
    a*t3 + b*t2 + c*t + from
}

/// Map a value from an input range `inmin..inmax`
/// to an output range `outmin..outmax`
#[inline]
pub fn map<T: Copy>(value: T, inmin: T, inmax: T, outmin: T, outmax: T) -> T
    where T: Add<T, Output = T> + Mul<T, Output = T> + Sub<T, Output = T> + Div<T, Output = T> + Clone + Zero + PartialEq{
    if inmin == inmax {
		outmin
	} else {
		((value - inmin) / (inmax - inmin) * (outmax - outmin) + outmin)
	}
}

/// Map a value from an input range `inmin..inmax`
/// to an output range `outmin..outmax` and clamp the
/// result to be inside the output range
#[inline]
pub fn map_clamp<T: Copy>(value: T, inmin: T, inmax: T, outmin: T, outmax: T) -> T
    where T: Add<T, Output = T> + Mul<T, Output = T> + Sub<T, Output = T> + Div<T, Output = T> + Clone + Zero + PartialEq + PartialOrd{
    let out = map(value, inmin, inmax, outmin, outmax);
    if outmax > outmin {
        clamp(out, outmin, outmax)
    }else if outmax < outmin {
        clamp(out, outmax, outmin)
    }else{
        outmin
    }
}

/// Wrap a value in the range `from..to`
#[inline]
pub fn wrap<T: alga::general::Real>(value:T, from:T, to:T) -> T{
    let mut from = from;
    let mut to = to;

    if from > to{
        mem::swap(&mut from,&mut to);
    }

    let cycle = to - from;

    if cycle == zero(){
        return zero();
    }

    value - cycle * ((value - from) / cycle).floor()
}

/// Wrap an integer value in the range `from..to`
#[inline]
pub fn iwrap<T: num_traits::PrimInt + ::std::fmt::Display>(mut value:T, from:T, to:T) -> T{
    let mut from = from;
    let mut to = to;

    if from > to{
        mem::swap(&mut from,&mut to);
    }

    let cycle = to - from;

    if cycle == num_traits::zero(){
        return num_traits::zero();
    }

    if value < from {
        value = value + cycle * ((from - value) / cycle + num_traits::one());
    }

    from + (value - from) % cycle
}


/// Intersection of line segments p0 - p1 and p2 - p3
#[inline]
pub fn line_segment_intersection(p0: Pnt2, p1: Pnt2, p2: Pnt2, p3: Pnt2) -> Option<Pnt2>
{
    let s1_x = p1.x - p0.x;
    let s1_y = p1.y - p0.y;
    let s2_x = p3.x - p2.x;
    let s2_y = p3.y - p2.y;

    let s = (-s1_y * (p0.x - p2.x) + s1_x * (p0.y - p2.y)) / (-s2_x * s1_y + s1_x * s2_y);
    let t = ( s2_x * (p0.y - p2.y) - s2_y * (p0.x - p2.x)) / (-s2_x * s1_y + s1_x * s2_y);

    if s >= 0.0 && s <= 1.0 && t >= 0.0 && t <= 1.0{
        Some(Pnt2::new(p0.x + (t * s1_x), p0.y + (t * s1_y)))
    }else{
        None
    }
}

/// Convert a quaternion to euler angles
#[inline]
//TODO: Generics are gone cause both Real and Float provide asin, atan...
pub fn to_euler(q: &UnitQuat) -> (Rad<f32>, Rad<f32>, Rad<f32>){
    let q = q.quaternion();
	let test = q.i*q.j + q.k*q.w;
    let two = 2.0;
    let one = 1.0;
	if test > num_traits::cast(0.499).unwrap() { // singularity at north pole
		( Rad(two * q.i.atan2(q.w)),  // heading
		 Rad::half_pi(),              // attitude
		 num_traits::zero() )                     // bank
	} else if test < num_traits::cast(-0.499).unwrap() { // singularity at south pole
		( Rad(-two * q.i.atan2(q.w)), // heading
		  - Rad::half_pi(),           // attitude
		  num_traits::zero() )                    // bank
	} else {
		let sqx = q.i * q.i;
		let sqy = q.j * q.j;
		let sqz = q.k * q.k;
		(
            Rad((two * q.j * q.w - two * q.i * q.k).atan2(one - two*sqy - two*sqz)), // heading
		    Rad((two*test).asin()),                                                  // attitude
		    Rad((two*q.i * q.w - two* q.j * q.k).atan2(one - two*sqx - two*sqz)),    // bank
        )
	}
}

/// Convert a quaternion to tait bryan angles
#[inline]
//TODO: Generics are gone cause both Real and Float provide asin, atan...
pub fn to_tait_bryan(q: &UnitQuat) -> (Rad<f32>, Rad<f32>, Rad<f32>){
    let q = q.quaternion();
	let sq0 = q.w*q.w;
	let sq1 = q.i*q.i;
	let sq2 = q.j*q.j;
	let sq3 = q.k*q.k;
    let _2 = 2.0;

	// we can now use the same terms as in the textbook.
	let roll  =	Rad((_2 * q.j * q.k + _2 * q.w * q.i).atan2(sq3 - sq2 - sq1 + sq0));
	let pitch =	Rad(-(_2 * q.i * q.k - _2 * q.w * q.j).asin());
	let yaw	  =	Rad((_2 * q.i * q.j + _2 * q.w * q.k).atan2(sq1 + sq0 - sq3 - sq2));

	(roll,pitch,yaw)
}

pub enum RotOrder{
    XYZ,
    XZY,
    YXZ,
    YZX,
    ZXY,
    ZYX,
}

struct AxisParity{
    axis: Vec3<usize>,
    parity: bool,
}

impl RotOrder{
    fn to_axis_parity(self) -> AxisParity{
        let axis_parity = match self{
            RotOrder::XYZ => ([0usize, 1, 2], false),
            RotOrder::XZY => ([0, 2, 1], false),
            RotOrder::YXZ => ([1, 0, 2], true),
            RotOrder::YZX => ([1, 2, 0], true),
            RotOrder::ZXY => ([2, 0, 1], false),
            RotOrder::ZYX => ([2, 1, 0], true),
        };
        unsafe{ mem::transmute(axis_parity) }
    }
}

/// Convert euler angles with a certain rotation order
/// into a quaternion
pub fn euler_to_quaternion(rot: &Vec3, rot_order: RotOrder) -> UnitQuat{
    let r = rot_order.to_axis_parity();
    let i = r.axis.x;
    let j = r.axis.y;
    let k = r.axis.z;

    let ti = rot[i] * 0.5;
    let tj = rot[j] * if r.parity { -0.5 } else { 0.5 };
    let th = rot[k] * 0.5;

    let ci = ti.cos();
    let cj = tj.cos();
    let ch = th.cos();
    let si = ti.sin();
    let sj = tj.sin();
    let sh = th.sin();

    let cc = ci * ch;
    let cs = ci * sh;
    let sc = si * ch;
    let ss = si * sh;

    let mut a: [f32;3] = unsafe{ mem::uninitialized() };
    a[i] = cj * sc - sj * cs;
    a[j] = cj * ss + sj * cc;
    a[k] = cj * cs - sj * sc;

    let w = cj * cc + sj * ss;
    let x = a[0];
    let y = a[1];
    let z = a[2];

    let mut q = Quaternion::new(w,x,y,z);

    if r.parity { q[j + 1] = -q[j + 1] };

    UnitQuat::from_quaternion(q)
}

/// Trait to check if a point is inside a polygon
pub trait InsidePolygon<T: alga::general::Real>{
    fn inside_polygon(&self, polygon: &Polyline<T>, bound: bool) -> bool;
}

impl<T: alga::general::Real + NumCast> InsidePolygon<T> for Pnt2<T>{
    fn inside_polygon(&self, polygon: &Polyline<T>, bound: bool) -> bool{
        //cross points count of x
        let mut count = 0;

        //left vertex
        let mut p1 = polygon[0];

        //check all rays
        for i in 0 .. polygon.len()+1
        {
            //point is an vertex
            if *self == p1 { return bound; }

            //right vertex
            let p2 = polygon[i % polygon.len()];

            //ray is outside of our interests
            if self.y < p1.y.min(p2.y) || self.y > p1.y.max(p2.y){
                //next ray left point
                p1 = p2; continue;
            }

            //ray is crossing over by the algorithm (common part of)
            if self.y > p1.y.min(p2.y) && self.y < p1.y.max(p2.y){
                //x is before of ray
                if self.x <= p1.x.max(p2.x){
                    //overlies on a horizontal ray
                    if p1.y == p2.y && self.x >= p1.x.min(p2.x) { return bound; }

                    //ray is vertical
                    if p1.x == p2.x {
                        //overlies on a ray
                        if p1.x == self.x { return bound; }
                        //before ray
                        else { count += 1; }
                    }else{
                        //cross point on the left side
                        //cross point of x
                        let xinters = (self.y - p1.y) * (p2.x - p1.x) / (p2.y - p1.y) + p1.x;

                        //overlies on a ray
                        if self.x == xinters { return bound; }

                        //before ray
                        if self.x < xinters { count += 1; }
                    }
                }
            }else{
                //special case when ray is crossing through the vertex
                //p crossing over p2
                if self.y == p2.y && self.x <= p2.x {
                    //next vertex
                    let p3 = polygon[(i+1) % polygon.len()];

                    //p.y lies between p1.y & p3.y
                    if self.y >= p1.y.min(p3.y) && self.y <= p1.y.max(p3.y){
                        count += 1;
                    }else{
                        count += 2;
                    }
                }
            }
            //next ray left point
            p1 = p2;
        }

        //EVEN
        if count % 2 == 0{
            false
        }else{
        //ODD
            true
        }
    }
}

/// Next power of two
///
/// 63 -> 64
/// 200 -> 256
/// ...
pub fn next_pow2<T: BaseNum + PartialOrd + ShlAssign<T>>(v: T) -> T{
    let mut rval= one();
	while rval<v{
        rval<<=one();
    }
	rval
}

pub fn next_multiple<T: BaseNum>(value: T, multiple: T) -> T{
    let rem = value % multiple;
    if rem != zero() {
        let add = multiple - rem;
        value + add
    }else{
        value
    }
}