use crate::nalgebra::{center, Isometry2, Point2, Vector2, Vector3};
use bevy::prelude::Mesh;
use bevy_rapier2d::prelude::*;
use rand::prelude::*;
use std::collections::HashSet;
pub fn generate_random_mesh(
    num_of_verticles: u16,
    max_distance_from_verticles: f32,
    max_distance_to_create_triangle: f32,
) -> Result<Mesh, &'static str> {
    if num_of_verticles < 3 {
        return Err("Number of verticles should be greater than 3!");
    }
    // list of verticles
    let mut verts = Vec::new();
    // list of indices
    let mut indices: Vec<[usize; 3]> = Vec::new();
    // list of edges, contains points of edges, verts and middle point
    let mut edges: Vec<([Point2<f32>; 2], [usize; 2], Point2<f32>)> = Vec::new();
    //first vert
    verts.push(Point2::new(0.0, 0.0));
    //second vert
    verts.push(generate_random_point_in_a_circle(
        max_distance_from_verticles,
    ));
    //third vert
    let last_verticle = verts.last().expect("wtf").clone();
    let random_point = generate_random_point_in_a_circle(max_distance_from_verticles);
    verts.push(last_verticle + Vector2::new(random_point.x, random_point.y));
    //first triangle has been created, push indices
    indices.push([0, 1, 2]);
    //push edges for later
    edges.push(([verts[0], verts[1]], [0, 1], center(&verts[0], &verts[1])));
    edges.push(([verts[1], verts[2]], [1, 2], center(&verts[1], &verts[2])));
    edges.push(([verts[2], verts[0]], [2, 0], center(&verts[2], &verts[0])));
    //rng
    let mut rng = rand::thread_rng();
    let mut current_num_of_verticles = 3;
    //creating rest of the verticles
    while current_num_of_verticles < num_of_verticles {
        use rand::seq::SliceRandom;
        //randomly choosing one edge from verticles
        let random_edge: ([Point2<f32>; 2], [usize; 2], Point2<f32>) =
            *edges.choose(&mut rng).unwrap();
        //generating middle point of the selected edge
        let middle_of_the_edge = Vector2::new(random_edge.2.x, random_edge.2.y);
        //randomly tries n times to choose new verticle
        for _ebe in 0..5 {
            //generate new verticle by randomly translating the middle of the edge choosen
            let random_point = Point2::new(0.0, 0.0)/*generate_random_point_in_a_circle(max_distance_from_verticles)*/ + middle_of_the_edge + (perpendicular_vector(&random_edge.0[0], &random_edge.0[1])*(max_distance_from_verticles));
            //checking if this verticle is inside already generated triangles
            let is_inside = indices.iter().fold(false, |acc, curr_triangle| {
                acc || point_in_triangle(
                    &random_point,
                    &verts[curr_triangle[0]],
                    &verts[curr_triangle[1]],
                    &verts[curr_triangle[2]],
                )
            });
            //if point is not inside, then we create new triangle from this point and nearest edges
            if !is_inside {
                let mut new_vert_added = false;
                //iter of distances from new verticle to nearest edges
                let mut distances: Vec<(usize, f32)> = edges
                    .iter()
                    .map(|edge| crate::nalgebra::distance(&edge.2, &random_point))
                    .enumerate()
                    .filter(|distance| distance.1 < max_distance_to_create_triangle)
                    .collect();
                distances.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap());
                let potential_edges: Vec<([Point2<f32>; 2], [usize; 2], Point2<f32>)> =
                    distances.iter().map(|distance| edges[distance.0]).collect();
                //distances.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap());
                //now we have set of edges with whom create new triangles to the newly created verticle
                //We can create new triangle only if its two new edges do not intersect existing edges
                for potential_edge in potential_edges {
                    //checks if first possible edge intersects existing edges
                    let first_edge_intersect = edges.iter().fold(false, |acc, curr_edge| {
                        acc || line_intersection_test(
                            &random_point,
                            &potential_edge.0[0],
                            &curr_edge.0[0],
                            &curr_edge.0[1],
                        )
                    });
                    //println!("1: {:?} 2: {:?} 3: {?} 4: {?}", random_point, potential_edge.0[0]);
                    if !first_edge_intersect {
                        //println!("dont intersect");
                        //checks if second possible edge intersects
                        let second_edge_intersect = edges.iter().fold(false, |acc, curr_edge| {
                            acc || line_intersection_test(
                                &random_point,
                                &potential_edge.0[1],
                                &curr_edge.0[0],
                                &curr_edge.0[1],
                            )
                        });
                        if !second_edge_intersect {
                            //Two potential edges do not intersect, add new triangle to the mesh
                            //add new verticle
                            if !new_vert_added {
                                verts.push(random_point);
                                current_num_of_verticles += 1;
                                new_vert_added = true;
                            }
                            //add indices
                            indices.push([
                                potential_edge.1[0],
                                potential_edge.1[1],
                                verts.len() - 1,
                            ]);
                            //add to edges lists
                            edges.push((
                                [verts[verts.len() - 1], verts[potential_edge.1[0]]],
                                [verts.len() - 1, potential_edge.1[0]],
                                center(&verts[verts.len() - 1], &potential_edge.0[0]),
                            ));
                            edges.push((
                                [verts[verts.len() - 1], verts[potential_edge.1[1]]],
                                [verts.len() - 1, potential_edge.1[1]],
                                center(&verts[verts.len() - 1], &potential_edge.0[1]),
                            ));
                        }
                    }
                }
                break;
            } else {
                println!("INSIDE");
            }
        }
    }
    //now we have all verts and all triangle indices, generating mesh
    let mut mesh = Mesh::new(bevy::render::pipeline::PrimitiveTopology::LineList);
    let v_pos: Vec<[f32; 2]> = verts.iter().map(|point| [point.x, point.y]).collect();
    mesh.set_attribute(Mesh::ATTRIBUTE_POSITION, v_pos);
    let v_normal = vec![[0.0, 0.0, 1.0]; verts.len()];
    mesh.set_attribute("Vertex_Normal", v_normal);
    let uv = vec![[0.5, 0.5]; verts.len()];
    mesh.set_attribute("Vertex_Uv", uv);
    //let indices = indices.iter().flat_map(|tup| [tup.0 as u32, tup.1 as u32, tup.2 as u32].iter().cloned()).collect();
    //let indices = indices.iter().fold(Vec::with_capacity(indices.len() * 3),
    //|mut acc, p| { acc.extend(&[p.0 as u32, p.1 as u32, p.2 as u32]); acc });
    /*let mut final_indices = Vec::with_capacity(indices.len() * 3);
    for indice in indices {
        if is_triangle_ccw(&verts[indice[0]], &verts[indice[1]], &verts[indice[2]]) {
            final_indices.push(indice[0] as u32);
            final_indices.push(indice[1] as u32);
            final_indices.push(indice[2] as u32);
        } else {
            final_indices.push(indice[2] as u32);
            final_indices.push(indice[1] as u32);
            final_indices.push(indice[0] as u32);
        }
    }*/
    let mut final_indices = Vec::with_capacity(indices.len() * 6);
    for edge in edges {
        final_indices.push(edge.1[0] as u32);
        final_indices.push(edge.1[1] as u32);
    }
    mesh.set_indices(Some(bevy::render::mesh::Indices::U32(final_indices)));
    Ok(mesh)
}
pub fn generate_random_mesh2(
    num_of_verticles: u16,
    max_distance_from_verticles: f32,
    max_distance_to_create_triangle: f32,
) -> Result<(Mesh, Vec<Point2<f32>>), &'static str> {
    // list of verticles
    let mut verts = Vec::new();
    // list of indices
    let mut indices: Vec<[usize; 3]> = Vec::new();
    // list of edges, contains points of edges, verts and middle point
    let mut edges2: Vec<[usize; 2]> = Vec::new();
    // list of outer indices, to which outer edges from edges2 belong
    let mut indices: Vec<[usize; 3]> = Vec::new();
    //rng
    let mut rng = rand::thread_rng();
    //first vert
    verts.push(Point2::new(0.0, 0.0));
    //second vert
    verts.push(Point2::new(max_distance_from_verticles, 0.0));
    //third vert
    verts.push(Point2::new(
        max_distance_from_verticles / 2.0,
        max_distance_from_verticles * 0.86602,
    ));
    //first triangle has been created, push indices
    indices.push([0, 1, 2]);
    //push edges for later
    edges2.push([0, 1]);
    edges2.push([1, 2]);
    edges2.push([2, 0]);
    //rng
    let mut rng = rand::thread_rng();
    let mut current_num_of_verticles = 3;
    //creating rest of the verticles
    while current_num_of_verticles < num_of_verticles {
        //randomly choosing one edge from verticles
        let random_edge_index = (rand::random::<f32>() * edges2.len() as f32).floor() as usize;
        let random_edge = edges2[random_edge_index];
        let mut new_vert = center(&verts[random_edge[0]], &verts[random_edge[1]])
            + (perpendicular_vector(&verts[random_edge[0]], &verts[random_edge[1]])
                * (max_distance_from_verticles * 0.86602));
        let mut new_vert_index = verts.len();
        let mut distances: Vec<(usize, f32)> = verts
            .iter()
            .map(|vert| crate::nalgebra::distance(&vert, &new_vert))
            .enumerate()
            .filter(|distance| distance.1 < 0.0001)
            .collect();
        if distances.len() == 1 {
            //println!("eee");
            new_vert = verts[distances[0].0];
            new_vert_index = distances[0].0;
            edges2.remove(random_edge_index);
            //println!("{:?}, {:?}, {:?}", new_vert, verts[random_edge[0]], verts[random_edge[1]]);
            let first_edge = [new_vert_index, random_edge[1]];
            if let Some(index) = edges2
                .iter()
                .position(|edge| *edge == [random_edge[1], new_vert_index])
            {
                edges2.remove(index);
            } else {
                edges2.push(first_edge);
            }
            let second_edge = [random_edge[0], new_vert_index];
            if let Some(index) = edges2
                .iter()
                .position(|edge| *edge == [new_vert_index, random_edge[0]])
            {
                edges2.remove(index);
            } else {
                edges2.push(second_edge);
            }
            indices.push([random_edge[1], random_edge[0], new_vert_index]);
        //current_num_of_verticles = num_of_verticles;
        } else {
            verts.push(new_vert);
            let first_edge = [new_vert_index, random_edge[1]];
            let second_edge = [random_edge[0], new_vert_index];
            edges2.push(first_edge);
            edges2.push(second_edge);
            indices.push([random_edge[1], random_edge[0], new_vert_index]);
            edges2.remove(random_edge_index);
        }
        current_num_of_verticles += 1;
    }
    //softing edge
    let mut random_edge_index = 0;
    while random_edge_index < edges2.len() {
        let random_edge = edges2[random_edge_index];
        let mut new_vert = center(&verts[random_edge[0]], &verts[random_edge[1]])
            + (perpendicular_vector(&verts[random_edge[0]], &verts[random_edge[1]])
                * (max_distance_from_verticles * 0.86602));
        let mut distances: Vec<(usize, f32)> = verts
            .iter()
            .map(|vert| crate::nalgebra::distance(&vert, &new_vert))
            .enumerate()
            .filter(|distance| distance.1 < 0.0001)
            .collect();
        let mut new_vert_index = verts.len();
        if distances.len() == 1 {
            //println!("eee");
            new_vert = verts[distances[0].0];
            new_vert_index = distances[0].0;
            edges2.remove(random_edge_index);
            //println!("{:?}, {:?}, {:?}", new_vert, verts[random_edge[0]], verts[random_edge[1]]);
            let first_edge = [new_vert_index, random_edge[1]];
            if let Some(index) = edges2
                .iter()
                .position(|edge| *edge == [random_edge[1], new_vert_index])
            {
                edges2.remove(index);
            } else {
                edges2.push(first_edge);
            }
            let second_edge = [random_edge[0], new_vert_index];
            if let Some(index) = edges2
                .iter()
                .position(|edge| *edge == [new_vert_index, random_edge[0]])
            {
                edges2.remove(index);
            } else {
                edges2.push(second_edge);
            }
            indices.push([random_edge[1], random_edge[0], new_vert_index]);
            //current_num_of_verticles = num_of_verticles;
        }
        random_edge_index += 1;
    }
    let mut random_edge_index = 0;
    while random_edge_index < edges2.len() {
        let random_edge = edges2[random_edge_index];
        let mut new_vert = center(&verts[random_edge[0]], &verts[random_edge[1]])
            + (perpendicular_vector(&verts[random_edge[0]], &verts[random_edge[1]])
                * (max_distance_from_verticles * 0.86602));
        let mut distances: Vec<(usize, f32)> = verts
            .iter()
            .map(|vert| crate::nalgebra::distance(&vert, &new_vert))
            .enumerate()
            .filter(|distance| distance.1 < 0.0001)
            .collect();
        let mut new_vert_index = verts.len();
        if distances.len() == 1 {
            //println!("eee");
            new_vert = verts[distances[0].0];
            new_vert_index = distances[0].0;
            edges2.remove(random_edge_index);
            //println!("{:?}, {:?}, {:?}", new_vert, verts[random_edge[0]], verts[random_edge[1]]);
            let first_edge = [new_vert_index, random_edge[1]];
            if let Some(index) = edges2
                .iter()
                .position(|edge| *edge == [random_edge[1], new_vert_index])
            {
                edges2.remove(index);
            } else {
                edges2.push(first_edge);
            }
            let second_edge = [random_edge[0], new_vert_index];
            if let Some(index) = edges2
                .iter()
                .position(|edge| *edge == [new_vert_index, random_edge[0]])
            {
                edges2.remove(index);
            } else {
                edges2.push(second_edge);
            }
            indices.push([random_edge[1], random_edge[0], new_vert_index]);
            //current_num_of_verticles = num_of_verticles;
        }
        random_edge_index += 1;
    }
    /*let mut random_edge_index = 0;
    while random_edge_index < edges2.len() {
        let first_edge = edges2[random_edge_index];
        let second_edge = edges2[random_edge_index];
        let first_point = verts[first_edge[0]];
        let second_point = verts[second_edge[1]];
        let dist = crate::nalgebra::distance(&first_point, &second_point);
        if dist < max_distance_from_verticles+0.01 {
            let index = indices.iter().position(|indice| (*indice == [first_edge[0], first_edge[1], second_edge[1]]) || (*indice == [second_edge[1], first_edge[0], first_edge[1]]) || (*indice == [first_edge[1], second_edge[1], first_edge[0]]));
            if let Some(i) = index {
                indices.remove(i);
            }
        }
        random_edge_index += 1;
    }
    let mut random_edge_index = 0;
    while random_edge_index < edges2.len() {
        let first_edge = edges2[random_edge_index];
        let second_edge = edges2[random_edge_index];
        let first_point = verts[first_edge[0]];
        let second_point = verts[second_edge[1]];
        let dist = crate::nalgebra::distance(&first_point, &second_point);
        if dist < max_distance_from_verticles+0.01 {
            let index = indices.iter().position(|indice| (*indice == [first_edge[0], first_edge[1], second_edge[1]]) || (*indice == [second_edge[1], first_edge[0], first_edge[1]]) || (*indice == [first_edge[1], second_edge[1], first_edge[0]]));
            if let Some(i) = index {
                indices.remove(i);
            }
        }
        random_edge_index += 1;
    }*/
    //now we have all verts and all triangle indices, generating mesh
    //randomly translate verts
    //let verts: Vec<Point2<f32>> = verts.iter().map(|vert| vert+point2vec(generate_random_point_in_a_circle(max_distance_from_verticles/8.0)) ).collect();
    //let mut mesh = Mesh::new(bevy::render::pipeline::PrimitiveTopology::LineList);
    let mut mesh = Mesh::new(bevy::render::pipeline::PrimitiveTopology::TriangleList);
    let v_pos: Vec<[f32; 3]> = verts.iter().map(|point| [point.x, point.y, 0.0]).collect();
    mesh.set_attribute(Mesh::ATTRIBUTE_POSITION, v_pos);
    let v_normal = vec![[0.0, 0.0, 1.0]; verts.len()];
    mesh.set_attribute("Vertex_Normal", v_normal);
    let uv = vec![[0.5, 0.5]; verts.len()];
    mesh.set_attribute("Vertex_Uv", uv);
    let mut final_indices = Vec::with_capacity(indices.len() * 6);
    for indice in &indices {
        final_indices.push(indice[0] as u32);
        final_indices.push(indice[1] as u32);
        final_indices.push(indice[2] as u32);
    }
    mesh.set_indices(Some(bevy::render::mesh::Indices::U32(final_indices)));
    let final_outline = edges2.iter().fold(Vec::new(), |mut acc, curr_edge| {
        acc.push(verts[curr_edge[0] as usize]);
        acc.push(verts[curr_edge[1] as usize]);
        acc
    });
    Ok((mesh, final_outline))
}
pub fn generate_shape_collider_from_mesh(mesh: &Mesh) -> Option<SharedShape> {
    let points = match mesh.attribute(Mesh::ATTRIBUTE_POSITION).unwrap() {
        bevy::render::mesh::VertexAttributeValues::Float3(verts) => verts
            .iter()
            .map(|vert| nalgebra::Point2::new(vert[0], vert[1]))
            .collect(),
        _ => {
            panic!("U SUK");
            vec![]
        }
    };
    /*let new_indices = match mesh.indices().unwrap() {
        bevy::render::mesh::Indices::U32(indices) => {
                indices
                .iter()
                .zip(indices.iter().skip(1))
                .map(|tuple| [tuple.0.clone(), tuple.1.clone()])
                //.inspect(|(a, b)| println!("a: {}, b: {}", a, b)
                .collect::<Vec<_>>()
            }
        _ => {panic!("U SUK")}
    };*/
    let indices = match mesh.indices().unwrap() {
        bevy::render::mesh::Indices::U32(indices) => indices,
        _ => {
            panic!("U SUK")
        }
    };
    let positions_and_shapes: Vec<(Isometry2<f32>, SharedShape)> = indices
        .chunks(3)
        //.inspect(|chunk| println!("{} {} {}", points[chunk[0] as usize], points[chunk[1] as usize], points[chunk[2] as usize]))
        .map(|indice| {
            (
                Isometry2::new(Vector2::new(0.0, 0.0), 0.0),
                ColliderShape::convex_polyline(vec![
                    points[indice[0] as usize],
                    points[indice[1] as usize],
                    points[indice[2] as usize],
                ])
                .unwrap(),
            )
        })
        .collect();
    //println!("{}", positions_and_shapes.len());
    Some(ColliderShape::compound(positions_and_shapes))
    //Some(ColliderShape::convex_decomposition(&points, &*new_indices))
    //ColliderShape::convex_hull(&points)
}
/*pub fn generate_shape_collider_from_outline(outline: &Vec<Point2<f32>>) -> Option<SharedShape> {
}*/
pub fn point2vec(point: Point2<f32>) -> Vector2<f32> {
    Vector2::new(point.x, point.y)
}
pub fn generate_middle_point_of_triangle(
    vert1: &Point2<f32>,
    vert2: &Point2<f32>,
    vert3: &Point2<f32>,
) -> Vector2<f32> {
    (Vector2::new(vert1.x, vert1.y)
        + Vector2::new(vert2.x, vert2.y)
        + Vector2::new(vert3.x, vert3.y))
        / 3.0
}
pub fn generate_random_point_in_a_circle(radius: f32) -> Point2<f32> {
    let mut rng = thread_rng();
    let r = radius * rng.gen::<f32>().sqrt();
    let theta = rng.gen::<f32>() * 2.0 * std::f32::consts::PI;
    //converting to Cartesian
    let x = r * theta.cos();
    let y = r * theta.sin();
    Point2::new(x, y)
}
pub fn sign_of_point(point: &Point2<f32>, vert1: &Point2<f32>, vert2: &Point2<f32>) -> f32 {
    (point.x - vert2.x) * (vert1.y - vert2.y) - (vert1.x - vert2.x) * (point.y - vert2.y)
}
pub fn point_in_triangle(
    point: &Point2<f32>,
    vert1: &Point2<f32>,
    vert2: &Point2<f32>,
    vert3: &Point2<f32>,
) -> bool {
    let d1 = sign_of_point(point, vert1, vert2);
    let d2 = sign_of_point(point, vert2, vert3);
    let d3 = sign_of_point(point, vert3, vert1);
    let has_neg = (d1 < 0.0) || (d2 < 0.0) || (d3 < 0.0);
    let has_pos = (d1 > 0.0) || (d2 > 0.0) || (d3 > 0.0);
    !(has_neg && has_pos)
}
pub fn is_triangle_ccw(vert1: &Point2<f32>, vert2: &Point2<f32>, vert3: &Point2<f32>) -> bool {
    let first_vector = Vector3::from(*vert2) - Vector3::from(*vert1);
    let second_vector = Vector3::from(*vert2) - Vector3::from(*vert3);
    first_vector.cross(&second_vector)[2] < 0.0
}
pub fn cross2d(first_vector2: &Vector2<f32>, second_vector2: &Vector2<f32>) -> f32 {
    (first_vector2.x * second_vector2.y) - (first_vector2.y * second_vector2.x)
}
pub fn vector2_div2scalar(first_vector2: &Vector2<f32>, second_vector2: &Vector2<f32>) -> f32 {
    first_vector2.y / second_vector2.y
}
pub fn perpendicular_vector(first_point: &Point2<f32>, second_point: &Point2<f32>) -> Vector2<f32> {
    let vector =
        Vector2::new(first_point.x, first_point.y) - Vector2::new(second_point.x, second_point.y);
    -Vector2::new(vector.y, -vector.x).normalize()
}
//If True, then lines intersect or are collinear
pub fn line_intersection_test(
    first_line_start: &Point2<f32>,
    first_line_end: &Point2<f32>,
    second_line_start: &Point2<f32>,
    second_line_end: &Point2<f32>,
) -> bool {
    let p = Vector2::new(first_line_start.x, first_line_start.y);
    let r = Vector2::new(first_line_end.x, first_line_end.y) - p;
    let q = Vector2::new(second_line_start.x, second_line_start.y);
    let s = Vector2::new(second_line_end.x, second_line_end.y) - q;
    let det_numerator = cross2d(&(q - p), &(r));
    let det_denominator = cross2d(&r, &s);
    if det_denominator == 0.0 {
        if det_numerator == 0.0 {
            //collinear
            true
        } else {
            //parallel, not intersecting
            false
        }
    } else {
        let u = det_numerator / det_denominator;
        if u < 0.99 && u > 0.01 {
            let t = cross2d(&(q - p), &(s)) / det_denominator;
            if t < 0.99 && t > 0.01 {
                true
            } else {
                false
            }
        } else {
            false
        }
    }
}
#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn itersection_test() {
        let first_point = Point2::new(0.0, 0.0);
        let second_point = Point2::new(1.0, 0.0);
        let third_point = Point2::new(0.0, 1.0);
        let fourth_point = Point2::new(1.0, 1.0);
        //collinear
        assert_eq!(
            true,
            line_intersection_test(&first_point, &second_point, &first_point, &second_point)
        );
        //parallel, not intersecting
        assert_eq!(
            false,
            line_intersection_test(&first_point, &second_point, &third_point, &fourth_point)
        );
        //intersection
        assert_eq!(
            true,
            line_intersection_test(&first_point, &fourth_point, &third_point, &second_point)
        );
        //intersection
        assert_eq!(
            true,
            line_intersection_test(&fourth_point, &first_point, &third_point, &second_point)
        );
        //intersection
        assert_eq!(
            true,
            line_intersection_test(&first_point, &fourth_point, &second_point, &third_point)
        );
        //intersection
        assert_eq!(
            true,
            line_intersection_test(&fourth_point, &first_point, &second_point, &third_point)
        );
        //not parallel, not intersect
        assert_eq!(
            false,
            line_intersection_test(
                &first_point,
                &second_point,
                &third_point,
                &Point2::new(1.0, 0.5)
            )
        );
    }
    #[test]
    fn point_in_triangle_test() {
        let first_vert = Point2::new(0.0, 0.0);
        let second_vert = Point2::new(1.0, 0.0);
        let third_vert = Point2::new(0.0, 1.0);
        let point = Point2::new(0.5, 0.5);
        let outside_point = Point2::new(1.0, 1.0);
        assert_eq!(
            true,
            point_in_triangle(&point, &first_vert, &second_vert, &third_vert)
        );
        assert_eq!(
            true,
            point_in_triangle(&point, &first_vert, &third_vert, &second_vert)
        );
        assert_eq!(
            true,
            point_in_triangle(&point, &second_vert, &first_vert, &third_vert)
        );
        assert_eq!(
            true,
            point_in_triangle(&point, &second_vert, &third_vert, &first_vert)
        );
        assert_eq!(
            true,
            point_in_triangle(&point, &third_vert, &first_vert, &second_vert)
        );
        assert_eq!(
            true,
            point_in_triangle(&point, &third_vert, &second_vert, &first_vert)
        );
        assert_eq!(
            false,
            point_in_triangle(&outside_point, &first_vert, &second_vert, &third_vert)
        );
        assert_eq!(
            false,
            point_in_triangle(&outside_point, &first_vert, &third_vert, &second_vert)
        );
        assert_eq!(
            false,
            point_in_triangle(&outside_point, &second_vert, &first_vert, &third_vert)
        );
        assert_eq!(
            false,
            point_in_triangle(&outside_point, &second_vert, &third_vert, &first_vert)
        );
        assert_eq!(
            false,
            point_in_triangle(&outside_point, &third_vert, &first_vert, &second_vert)
        );
        assert_eq!(
            false,
            point_in_triangle(&outside_point, &third_vert, &second_vert, &first_vert)
        );
    }
}

use bevy::{prelude::*, utils::Duration};
pub struct Particle {
    pub lifetime: f32,
}
pub struct ParticleLinearSizeOverTime {
    pub change: f32,
}
#[derive(Bundle)]
pub struct ParticleBundle {
    p: Particle,
    t: Transform,
    #[bundle]
    s: SpriteBundle,
}
#[derive(Default)]
pub struct ParticleMaterial(pub Handle<ColorMaterial>);
struct Emitter {
    frequency: f32,
    creation_function: FnOnce() -> ParticleBundle,
}
struct WorkingEmitter {
    time_of_start: f32,
    particle_counter: u32,
}
fn particle_lifetime_update(
    mut commands: Commands,
    mut particles: Query<(Entity, &mut Particle)>,
    time: Res<Time>,
) {
    for (entity, mut particle) in particles.iter_mut() {
        particle.lifetime -= time.delta_seconds();
        if particle.lifetime < 0.0 {
            commands.entity(entity).despawn();
        }
    }
}
fn particle_linear_size_over_time_update(
    mut particles: Query<(&mut ParticleLinearSizeOverTime, &mut Transform)>,
    time: Res<Time>,
) {
    for (mut particle, mut transform) in particles.iter_mut() {
        if transform.scale.x > 0.0 {
            transform.scale.x -= particle.change * time.delta_seconds();
        }
        if transform.scale.y > 0.0 {
            transform.scale.y -= particle.change * time.delta_seconds();
        }
    }
}
fn emit(mut commands: Commands, emitters: Query<(&Emitter, &WorkingEmitter)>, time: Res<Time>) {}
fn particle_startup(
    mut materials: ResMut<Assets<ColorMaterial>>,
    server: Res<AssetServer>,
    mut particle_mat: ResMut<ParticleMaterial>,
) {
    let texture = server.load("particle2.png");
    let particle_material = bevy::prelude::ColorMaterial {
        texture: Some(texture),
        ..Default::default()
    };
    particle_mat.0 = materials.add(particle_material);
}
pub struct ParticlePlugin;
impl Plugin for ParticlePlugin {
    fn build(&self, app: &mut AppBuilder) {
        app.init_resource::<ParticleMaterial>()
            .add_startup_system(particle_startup.system())
            .add_system(particle_lifetime_update.system())
            .add_system(particle_linear_size_over_time_update.system());
    }
}

 #![warn(
     clippy::all,
     clippy::restriction,
     clippy::pedantic,
     clippy::nursery,
     clippy::cargo,
 )]
use bevy::diagnostic::{FrameTimeDiagnosticsPlugin, LogDiagnosticsPlugin};
use bevy::{
    prelude::*, render::camera::Camera, render::camera::OrthographicProjection,
    render::pass::ClearColor,
};
use bevy_egui::{egui, EguiContext, EguiPlugin};
use bevy_rapier2d::prelude::*;
use nalgebra::{Point2, Vector2};
mod util;
use crate::util::*;
mod particle;
use crate::particle::*;
fn main() {
    #[cfg(target_arch = "wasm32")]
    console_error_panic_hook::set_once();
    let mut app = App::build();
    app.insert_resource(WindowDescriptor {
        title: "Bevy game".to_string(),
        vsync: false,
        //mode: bevy::window::WindowMode::BorderlessFullscreen,
        ..Default::default()
    })
    .insert_resource(RapierConfiguration {
        gravity: Vector2::zeros(),
        timestep_mode: bevy_rapier2d::physics::TimestepMode::FixedTimestep, //physic run at fixed 60Hz
        ..Default::default()
    })
    .insert_resource(ClearColor(Color::rgb(0.0, 0.0, 0.0)))
    .add_plugins(DefaultPlugins);
    #[cfg(target_arch = "wasm32")]
    app.add_plugin(bevy_webgl2::WebGL2Plugin);
    //app.add_plugin(LogDiagnosticsPlugin::default())
    //.add_plugin(FrameTimeDiagnosticsPlugin::default())
    app.add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
        .add_plugin(EguiPlugin)
        .init_resource::<PlayerShipMesh>()
        .init_resource::<BulletMesh>()
        .init_resource::<BulletMat>()
        .init_resource::<UiState>()
        .add_startup_system(setup_player_ship.system().before("lel1").before("lel2"))
        .add_startup_system(setup_graphics.system().label("lel1"))
        .add_startup_system(setup_physics.system().label("lel2"))
        .add_system(hehe.system())
        .add_system(manipulate_rigidbody.system().label("move_playa"))
        .add_plugin(ParticlePlugin)
        //.add_system(add_air_angular_friction.system())
        .add_system(shoot.system())
        .add_system_to_stage(CoreStage::PostUpdate, camera_fix.system())
        .add_system(change_scale.system())
        //.add_system(camera_fix.system().after("rapier::transform_sync_stage"))
        .run();
}
// Note the usage of `ResMut`. Even though `ctx` method doesn't require
// mutability, accessing the context from different threads will result
// into panic if you don't enable `egui/multi_threaded` feature.
//fn ui_example(egui_context: ResMut<EguiContext>) {
//    egui::Window::new("Hello").show(egui_context.ctx(), |ui| {
//        ui.label("world");
//    });
//}
struct UiState {
    label: String,
    value: f32,
}
impl Default for UiState {
    fn default() -> Self {
        UiState {
            label: "game".to_string(),
            value: 15.0,
        }
    }
}
#[derive(Default)]
struct PlayerShipMesh {
    mesh: Handle<Mesh>,
    texture: Handle<Texture>,
}
#[derive(Default)]
struct BulletMesh(Handle<Mesh>);
#[derive(Default)]
struct BulletMat(Handle<StandardMaterial>);
struct PlayerShip {
    last_shot: f64,
}
fn setup_player_ship(
    mut meshes: ResMut<Assets<Mesh>>,
    mut materials: ResMut<Assets<StandardMaterial>>,
    mut playermesh: ResMut<PlayerShipMesh>,
    mut bulletmesh: ResMut<BulletMesh>,
    mut bulletmat: ResMut<BulletMat>,
    server: Res<AssetServer>,
) {
    let mut ship = Mesh::new(bevy::render::pipeline::PrimitiveTopology::TriangleList);
    let v_pos = vec![
        [0.5, 3.0, 0.0],
        [0.0, 2.0, 0.0],
        [1.0, 2.0, 0.0],
        [0.0, 0.0, 0.0],
        [1.0, 0.0, 0.0],
    ];
    ship.set_attribute(Mesh::ATTRIBUTE_POSITION, v_pos);
    let mut v_normal = vec![[0.0, 0.0, 1.0]];
    v_normal.extend_from_slice(&[[0.0, 0.0, 1.0]; 4]);
    ship.set_attribute("Vertex_Normal", v_normal);
    let uv = vec![[0.5, 0.5], [0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]];
    ship.set_attribute("Vertex_Uv", uv);
    let indices = vec![0, 1, 2, 1, 3, 2, 2, 3, 4];
    ship.set_indices(Some(bevy::render::mesh::Indices::U32(indices)));
    playermesh.mesh = meshes.add(ship);
    playermesh.texture = server.load("ship.png");
    bulletmesh.0 = meshes.add(Mesh::from(bevy::prelude::shape::Icosphere {
        radius: 0.25,
        subdivisions: 2,
    }));
    let bullet_mat = bevy::prelude::StandardMaterial {
        base_color: Color::rgb(1.0, 1.0, 0.0),
        emissive: Color::rgb(1.0, 1.0, 0.0),
        ..Default::default()
    };
    bulletmat.0 = materials.add(bullet_mat);
}
fn setup_graphics(mut commands: Commands, mut configuration: ResMut<RapierConfiguration>) {
    configuration.scale = 1.0;
    let mut camera = OrthographicCameraBundle::new_3d();
    camera.orthographic_projection.scale = 20.0;
    camera.transform = Transform::from_xyz(0.01, 0.01, 5.01).looking_at(Vec3::ZERO, Vec3::Y);
    let cam = commands.spawn_bundle(camera).id();
    let light = commands
        .spawn_bundle(LightBundle {
            transform: Transform::from_translation(Vec3::new(0.0, 0.0, 2000.0)),
            light: Light {
                intensity: 100_000_00.0,
                range: 6000.0,
                ..Default::default()
            },
            ..Default::default()
        })
        .id();
    commands.entity(cam).push_children(&[light]);
}
fn setup_physics(
    mut commands: Commands,
    playermesh: Res<PlayerShipMesh>,
    particle_m: Res<ParticleMaterial>,
    mut materials2: ResMut<Assets<StandardMaterial>>,
    mut meshes: ResMut<Assets<Mesh>>,
    time: Res<Time>,
) {
    /* Create the ground. */
    /*let collider = ColliderBundle {
        shape: ColliderShape::cuboid(10.0, 10.0),
        ..Default::default()
    };*/
    /* Create the random mesh! */
    //commands.spawn_bundle(collider).insert(ColliderDebugRender::default()).insert(ColliderPositionSync::Discrete);
    let asteroid_mesh = util::generate_random_mesh2(1024, 0.5, 15.0).unwrap().0;
    let asteroid_collider = util::generate_shape_collider_from_mesh(&asteroid_mesh).unwrap();
    let asteroid_rigid_body = RigidBodyBundle {
        position: Vec2::new(20.0, 0.0).into(),
        forces: RigidBodyForces {
            gravity_scale: 0.0,
            ..Default::default()
        },
        ..Default::default()
    };
    let asteroid_pbr = PbrBundle {
        mesh: meshes.add(asteroid_mesh),
        material: materials2.add(bevy::prelude::StandardMaterial {
            base_color_texture: Some(playermesh.texture.clone()),
            roughness: 1.0,
            ..Default::default()
        }),
        ..Default::default()
    };
    let asteroid_collider = ColliderBundle {
        shape: asteroid_collider,
        material: ColliderMaterial {
            restitution: 0.7,
            ..Default::default()
        },
        mass_properties: ColliderMassProps::Density(200.0),
        ..Default::default()
    };
    commands
        .spawn_bundle(asteroid_rigid_body)
        .insert_bundle(asteroid_collider)
        .insert_bundle(asteroid_pbr)
        .insert(ColliderPositionSync::Discrete);
    let asteroid_mesh = util::generate_random_mesh2(256, 0.5, 15.0).unwrap().0;
    let asteroid_collider = util::generate_shape_collider_from_mesh(&asteroid_mesh).unwrap();
    let asteroid_rigid_body = RigidBodyBundle {
        position: Vec2::new(0.0, 10.0).into(),
        forces: RigidBodyForces {
            gravity_scale: 0.0,
            ..Default::default()
        },
        ..Default::default()
    };
    let asteroid_pbr = PbrBundle {
        mesh: meshes.add(asteroid_mesh),
        material: materials2.add(bevy::prelude::StandardMaterial {
            base_color_texture: Some(playermesh.texture.clone()),
            roughness: 1.0,
            ..Default::default()
        }),
        ..Default::default()
    };
    let asteroid_collider = ColliderBundle {
        shape: asteroid_collider,
        material: ColliderMaterial {
            restitution: 0.7,
            ..Default::default()
        },
        mass_properties: ColliderMassProps::Density(200.0),
        ..Default::default()
    };
    commands
        .spawn_bundle(asteroid_rigid_body)
        .insert_bundle(asteroid_collider)
        .insert_bundle(asteroid_pbr)
        .insert(ColliderPositionSync::Discrete);
    let asteroid_mesh = util::generate_random_mesh2(128, 0.5, 15.0).unwrap().0;
    let asteroid_collider = util::generate_shape_collider_from_mesh(&asteroid_mesh).unwrap();
    let asteroid_rigid_body = RigidBodyBundle {
        position: Vec2::new(-7.0, -7.0).into(),
        forces: RigidBodyForces {
            gravity_scale: 0.0,
            ..Default::default()
        },
        ..Default::default()
    };
    let asteroid_pbr = PbrBundle {
        mesh: meshes.add(asteroid_mesh),
        material: materials2.add(bevy::prelude::StandardMaterial {
            base_color_texture: Some(playermesh.texture.clone()),
            roughness: 1.0,
            ..Default::default()
        }),
        ..Default::default()
    };
    let asteroid_collider = ColliderBundle {
        shape: asteroid_collider,
        material: ColliderMaterial {
            restitution: 0.7,
            ..Default::default()
        },
        mass_properties: ColliderMassProps::Density(200.0),
        ..Default::default()
    };
    commands
        .spawn_bundle(asteroid_rigid_body)
        .insert_bundle(asteroid_collider)
        .insert_bundle(asteroid_pbr)
        .insert(ColliderPositionSync::Discrete);
    let rigid_body = RigidBodyBundle {
        position: Vec2::new(0.0, 0.0).into(),
        forces: RigidBodyForces {
            gravity_scale: 0.0,
            ..Default::default()
        },
        ..Default::default()
    };
    let mesh = meshes.get(playermesh.mesh.clone()).unwrap();
    let ShipCollider = ColliderBundle {
        shape: util::generate_shape_collider_from_mesh(&mesh).unwrap(),
        material: ColliderMaterial {
            restitution: 0.7,
            ..Default::default()
        },
        mass_properties: ColliderMassProps::Density(200.0),
        ..Default::default()
    };
    // set up the camera
    // camera
    //commands.spawn_bundle(camera);
    commands
        .spawn_bundle(rigid_body)
        .insert_bundle(ShipCollider)
        .insert_bundle(PbrBundle {
            mesh: playermesh.mesh.clone(),
            material: materials2.add(bevy::prelude::StandardMaterial {
                base_color_texture: Some(playermesh.texture.clone()),
                roughness: 1.0,
                ..Default::default()
            }),
            ..Default::default()
        })
        .insert(ColliderPositionSync::Discrete)
        .insert(PlayerShip { last_shot: 0.0 });
    commands
        .spawn_bundle(SpriteBundle {
            material: particle_m.0.clone(),
            transform: Transform::from_scale(Vec3::new(0.02, 0.02, 0.1)),
            ..Default::default()
        })
        .insert(Particle { lifetime: 10.0 })
        .insert(ParticleLinearSizeOverTime { change: 0.01 });
}
fn hehe(
    time: Res<Time>,
    positions: Query<(&RigidBodyPosition, &RigidBodyVelocity), With<PlayerShip>>,
    egui_context: ResMut<EguiContext>,
    mut ui_state: ResMut<UiState>,
) {
    egui::Window::new("Hello").show(egui_context.ctx(), |ui| {
        let e = positions.iter().next().unwrap();
        ui.label(format!(
            "world {:.2}: time {:.2} : vel: {:.?} : pos {:.2}",
            e.1.angvel,
            time.seconds_since_startup(),
            e.1.linvel.norm(),
            e.0.position
        ));
        ui.add(egui::Slider::new(&mut ui_state.value, 10.0..=30.0).text("zoom"));
    });
}
fn manipulate_rigidbody(
    keyboard_input: Res<Input<KeyCode>>,
    q_camera: Query<(&Transform, &OrthographicProjection), With<Camera>>,
    mut bodies: Query<
        (
            &mut RigidBodyForces,
            &mut RigidBodyVelocity,
            &RigidBodyMassProps,
            &RigidBodyPosition,
        ),
        With<PlayerShip>,
    >,
    windows: Res<Windows>,
    egui_context: Res<EguiContext>,
    time: Res<Time>,
) {
    let multipler = 600.0 * time.delta_seconds();
    let (mut body_forces, mut body_vel, body_mass_prop, body_pos) = bodies.single_mut().unwrap();
    if keyboard_input.pressed(KeyCode::W) {
        let forc = Vec2::new(0.0, 8.0 * multipler);
        body_vel.apply_impulse(
            body_mass_prop,
            body_pos.position.transform_vector(&forc.into()),
        );
    }
    if keyboard_input.pressed(KeyCode::S) {
        let forc = Vec2::new(0.0, -2.0 * multipler);
        body_vel.apply_impulse(
            body_mass_prop,
            body_pos.position.transform_vector(&forc.into()),
        );
    }
    if keyboard_input.pressed(KeyCode::D) {
        let forc = Vec2::new(6.0 * multipler, 0.0);
        body_vel.apply_impulse(
            body_mass_prop,
            body_pos.position.transform_vector(&forc.into()),
        );
    }
    if keyboard_input.pressed(KeyCode::A) {
        let forc = Vec2::new(-6.0 * multipler, 0.0);
        body_vel.apply_impulse(
            body_mass_prop,
            body_pos.position.transform_vector(&forc.into()),
        );
    }
    if body_vel.linvel.norm() > 10.0 {
        body_vel.linvel.try_set_magnitude(10.0, 5.0);
    }
    let window = windows.get_primary().unwrap();
    let ang_vel = body_vel.angvel;
    // check if the cursor is in the primary window
    if !egui_context.ctx().wants_pointer_input() {
        if let Some(pos) = window.cursor_position() {
            // assuming there is exactly one main camera entity, so this is OK
            let (camera_transform, ortoprojection) = q_camera.single().unwrap();
            // get the size of the window
            //let size = Vec2::new(window.width() as f32, window.height() as f32)/2.0 - Vec2::new(window.width() as f32, window.height() as f32) * ortoprojection.scale / 1000.0;
            let size = Vec2::new(window.width() as f32, window.height() as f32);
            // the default orthographic projection is in pixels from the center;
            // just undo the translation
            //let p = (pos - size/2.0)*ortoprojection.scale / 362.55;
            let current = body_pos.position.transform_vector(&Vector2::new(0.0, 1.0));
            let p = (pos - size / 2.0).normalize();
            let angle = nalgebra::geometry::UnitComplex::rotation_between(
                &current,
                &Vector2::new(p.x, p.y),
            )
            .angle();
            // apply the camera transform
            //let pos_wld = camera_transform.compute_matrix() * p.extend(0.0).extend(1.0);
            //eprintln!("World coords: {}/{}", pos_wld.x, pos_wld.y);
            //eprintln!("mouse: {}/{} ship: {}/{} angle: {}", p.x, p.y, current.x, current.y, angle);
            let mut thrusters_power = 16.0;
            let tanh_constant = 10.0;
            let desired_angle_vel_fn = |angle: f32| {
                angle.signum()
                    * (2.0
                        * thrusters_power
                        * (1.0 / tanh_constant)
                        * (((angle * tanh_constant).abs()).cosh()).ln())
                    .sqrt()
            };
            let mut desired_angle_vel =
                desired_angle_vel_fn(angle - ang_vel * time.delta_seconds());
            // constant torque, exept for angle small enough, then became linear because of tanh. Desired angle velocity is just velocity at angle which should be obtained by this torque from zero angular velocity and zero angle.
            // https://physics.stackexchange.com/questions/320752/how-do-i-get-the-position-at-time-t-if-acceleration-depends-on-position
            let mut torque = thrusters_power * (angle * tanh_constant).tanh().abs();
            println!("{:.5} {:.5}", desired_angle_vel, ang_vel);
            torque *= body_mass_prop.mass();
            if angle < 0.0 {
                if ang_vel > desired_angle_vel {
                    body_forces.torque = -torque;
                } else {
                    body_forces.torque = torque;
                }
            } else {
                if ang_vel < desired_angle_vel {
                    body_forces.torque = torque;
                } else {
                    body_forces.torque = -torque;
                }
            }
            /*if ang_vel < angle.abs() {
                body_vel.apply_torque_impulse(body_mass_prop, 4.0);
            }
            if ang_vel > angle.abs() {
                body_vel.apply_torque_impulse(body_mass_prop, -4.0);
            }*/
        }
    }
    /*if let Some(position) = window.cursor_position() {
        let logical_position = (position / bevy::math::Vec2::new( window.physical_width() as f32, window.physical_height() as f32)) - bevy::math::Vec2::new(0.5, 0.5);
        let desired_vec = crate::nalgebra::Isometry2::from_parts( crate::nalgebra::Translation2::identity(), body_pos.position.rotation ).transform_vector(&Vector2::new(0.0, 1.0));
        //let desired_rot = UnitComplex::rotation_between(&desired_vec, &logical_position);
        println!("{} {}", logical_position, desired_vec);
    } else {
        // cursor is not inside the window
    }*/
    if keyboard_input.pressed(KeyCode::E) {
        if ang_vel < 5.0 {
            body_vel.apply_torque_impulse(body_mass_prop, 4.0);
        }
    } else if keyboard_input.pressed(KeyCode::Q) {
        if ang_vel > -5.0 {
            body_vel.apply_torque_impulse(body_mass_prop, -4.0);
        }
    }
    //damping
    /*else {
        body_vel.apply_torque_impulse(body_mass_prop,  -(ang_vel*4.0));
        //body_vel.angvel = 0.0;
    }*/
}
fn shoot(
    mut commands: Commands,
    keyboard_input: Res<Input<KeyCode>>,
    buttons: Res<Input<MouseButton>>,
    mut player_ship: Query<(&RigidBodyVelocity, &RigidBodyPosition, &mut PlayerShip)>,
    mut meshes: ResMut<Assets<Mesh>>,
    mut materials: ResMut<Assets<StandardMaterial>>,
    time: Res<Time>,
    egui_context: Res<EguiContext>,
    bulletmesh: Res<BulletMesh>,
    bulletmat: Res<BulletMat>,
    particle_m: Res<ParticleMaterial>,
) {
    if keyboard_input.pressed(KeyCode::Space)
        || (buttons.pressed(MouseButton::Left) && !egui_context.ctx().wants_pointer_input())
    {
        let (velocity, position, mut player_ships) =
            player_ship.single_mut().expect("Multiple players!");
        if time.seconds_since_startup() - player_ships.last_shot > 0.25 {
            player_ships.last_shot = time.seconds_since_startup();
            let bullet_pos = position.position * Point2::new(0.5, 3.5);
            let rigid_body = RigidBodyBundle {
                position: bullet_pos.into(),
                forces: RigidBodyForces {
                    gravity_scale: 0.0,
                    ..Default::default()
                },
                velocity: RigidBodyVelocity {
                    linvel: (velocity.linvel + position.position * Vector2::new(0.0, 50.0)).into(),
                    angvel: 0.0,
                },
                ..Default::default()
            };
            let bullet_collider = ColliderBundle {
                shape: ColliderShape::ball(0.5),
                material: ColliderMaterial {
                    restitution: 0.7,
                    ..Default::default()
                },
                mass_properties: ColliderMassProps::Density(2.0),
                ..Default::default()
            };
            let bullet_mat = bevy::prelude::StandardMaterial {
                base_color: Color::rgb(1.0, 1.0, 0.0),
                emissive: Color::rgb(1.0, 1.0, 0.0),
                ..Default::default()
            };
            let pbr_bundle = PbrBundle {
                mesh: bulletmesh.0.clone_weak(),
                //material: materials.add(Color::rgb(1.0, 1.0, 0.0).into()),
                transform: bevy::prelude::Transform::from_translation(Vec3::new(
                    bullet_pos.x,
                    bullet_pos.y,
                    0.0,
                )),
                material: bulletmat.0.clone_weak(),
                ..Default::default()
            };
            commands
                .spawn_bundle(rigid_body)
                .insert_bundle(pbr_bundle)
                .insert_bundle(bullet_collider)
                .insert(ColliderPositionSync::Discrete);
        }
    }
}
fn change_scale(mut cam: Query<&mut OrthographicProjection>, ui_state: Res<UiState>) {
    let mut cam = cam.single_mut().unwrap();
    cam.scale = ui_state.value;
}
fn camera_fix(
    mut query: QuerySet<(
        Query<&mut Transform, With<Camera>>,
        Query<(&Transform, &RigidBodyMassProps), With<PlayerShip>>,
    )>,
) {
    /*let queryy = query.q1().single().expect("wuwu");
    let offset = queryy.1.local_mprops.local_com.clone();
    let offset = Vec3::new(offset.x, offset.y, 0.0);
    let new_pos = *queryy.0*offset;
    let mut c = query.q0_mut().single_mut().expect("hehe");
    c.translation = Vec3::new(new_pos[0], new_pos[1], 0.0) + Vec3::new(0.0, 0.0, 5.01);*/
    let (playership_transform, playership_rigidbody) =
        query.q1().single().expect("Playership not found");
    let offset = playership_rigidbody.local_mprops.local_com.clone();
    let offset = Vec3::new(offset.x, offset.y, 0.0);
    let new_pos = *playership_transform * offset;
    let mut cam_transform = query.q0_mut().single_mut().expect("Camera not found");
    cam_transform.translation = Vec3::new(new_pos[0], new_pos[1], 0.0) + Vec3::new(0.0, 0.0, 5.01);
}