use crate::prelude::*;
use crate::samplers::sampler::*;
use crate::rail_segment;
use crate::rail_segment::RailSegment;
use crate::rail_connector::RailConnector;

/**
 * Pure rust implementation of Basis::new_looking_at intended to allow unit tests.
 */
fn new_looking_at(direction: Vector3, up: Vector3, _positive_z_forward: bool) -> Basis {
    let z = -direction.normalized();
    let x = up.cross(z).normalized();
    let y = z.cross(x);
    Basis::from_cols(x, y, z)
}

pub struct SimpleSegment<A: AlignmentSampler, E: ElevationSampler> {
    alignment: A,
    elevation: E,
    
    lower_connection: Option<RailConnector>,
    upper_connection: Option<RailConnector>,
    //superelevation: Box<dyn SuperElevationSampler>, // Sometime I may get around to making this.
    
    // cache some stuff
    //endpoint0: Option<Transform3D>,
    //endpoint1: Option<Transform3D>,
    //length: Option<f32>,
}

impl<A: AlignmentSampler, E: ElevationSampler> SimpleSegment<A, E> {
    pub fn new(alignment: A, elevation: E) -> Self {
        Self {
            alignment,
            elevation,
            lower_connection: Option::None,
            upper_connection: Option::None,
            //endpoint0: None,
            //endpoint1: None,
            //length: None,
        }
    }
}

impl<A: AlignmentSampler, E: ElevationSampler> RailSegment for SimpleSegment<A, E> {
    fn get_length(&self, _index: usize) -> Result<f32, rail_segment::Error> {
        /*match self.length {
            Some(length) => Ok(length),
            None         => {
                let result = self.elevation.length();
                self.length = Some(result);
                Ok(result)
            },
        }*/
        Ok(self.elevation.length())
    }
    
    fn sample(&self, _index: usize, offset: f32) -> Result<Vector3, rail_segment::Error> {
        if offset < 0.0 || offset > self.get_length(1).unwrap() {
            return Err(rail_segment::Error::OffsetError);
        }
        let top_down_t = self.elevation.to_top_down_normalized(offset / self.elevation.length());
        let xz = self.alignment.sample_normalized(top_down_t);
        let y = self.elevation.sample_normalized(top_down_t);
        Ok(Vector3::new(xz.x, y, xz.y))
    }
    
    fn sample_with_rotation(&self, _index: usize, offset: f32) -> Result<Transform3D, rail_segment::Error> {
        if offset < 0.0 || offset > self.get_length(1).unwrap() {
            return Err(rail_segment::Error::OffsetError);
        }
        let top_down_t = self.elevation.to_top_down_normalized(offset / self.elevation.length());
        let direction_xz = self.alignment.tangent_normalized(top_down_t);
        let direction_y = self.elevation.grade_normalized(top_down_t);
        let direction = Vector3::new(direction_xz.x, direction_y, direction_xz.y);
        let basis = new_looking_at(direction, Vector3::UP, false);
        let xz = self.alignment.sample_normalized(top_down_t);
        let y = self.elevation.sample_normalized(top_down_t);
        Ok(Transform3D::new(basis, Vector3::new(xz.x, y, xz.y)))
    }
    
    fn sample_with_rotation_reverse(&self, _index: usize, offset: f32) -> Result<Transform3D, rail_segment::Error> {
        if offset < 0.0 || offset > self.get_length(1).unwrap() {
            return Err(rail_segment::Error::OffsetError);
        }
        let top_down_t = self.elevation.to_top_down_normalized(offset / self.elevation.length());
        let direction_zx = self.alignment.tangent_normalized(top_down_t);
        let direction_y = self.elevation.grade_normalized(top_down_t);
        let direction = Vector3::new(-direction_zx.x, -direction_y, -direction_zx.y);
        let basis = new_looking_at(direction, Vector3::UP, false);
        let xz = self.alignment.sample_normalized(top_down_t);
        let y = self.elevation.sample_normalized(top_down_t);
        Ok(Transform3D::new(basis, Vector3::new(xz.x, y, xz.y)))
    }
    
    fn sample_up_vector(&self, _index: usize, offset: f32) -> Result<Vector3, rail_segment::Error> {
        if offset < 0.0 || offset > self.get_length(1).unwrap() {
            return Err(rail_segment::Error::OffsetError);
        }
        let top_down_t = self.elevation.to_top_down_normalized(offset / self.elevation.length());
        let direction_xz = self.alignment.tangent_normalized(top_down_t);
        let direction_y = self.elevation.grade_normalized(top_down_t);
        let direction = Vector3::new(direction_xz.x, direction_y, direction_xz.y);
        let normal = Vector3::new(direction_xz.y, 0.0, -direction_xz.x);
        Ok(direction.cross(normal).normalized())
    }
    
    fn segment_from_endpoint(&self, _endpoint: usize) -> Result<usize, rail_segment::Error> {
        Ok(1)
    }
    
    fn get_endpoint_transform(&self, endpoint: usize) -> Result<Transform3D, rail_segment::Error> {
        match endpoint {
            /*0 => {
                match self.endpoint0 {
                    Some(result) => Ok(result),
                    None         => {
                        let result = self.sample_with_rotation_reverse(0, 0.0).unwrap();
                        self.endpoint0 = Some(result);
                        Ok(result)
                    },
                }
            },
            1 => {
                match self.endpoint1 {
                    Some(result) => Ok(result),
                    None         => {
                        let result = self.sample_with_rotation(0, self.get_length(1).unwrap()).unwrap();
                        self.endpoint1 = Some(result);
                        Ok(result)
                    },
                }
            },*/
            0 => self.sample_with_rotation_reverse(0, 0.0),
            1 => self.sample_with_rotation(0, self.get_length(1).unwrap()),
            _ => Err(rail_segment::Error::EndpointError),
        }
    }
    
    fn is_endpoint_upper(&self, endpoint: usize) -> Result<bool, rail_segment::Error> {
        match endpoint {
            0 => Ok(false),
            1 => Ok(true),
            _ => Err(rail_segment::Error::EndpointError),
        }
    }
    
    fn num_endpoints(&self) -> usize {
        2
    }
    
    fn borrow_connection(&self, endpoint: usize) -> Result<&RailConnector, rail_segment::Error> {
        match endpoint {
            0 => {
                if let Some(ret) = &self.lower_connection {
                    Ok(ret)
                } else {
                    Err(rail_segment::Error::NeighborError)
                }
            },
            1 => {
                if let Some(ret) = &self.upper_connection {
                    Ok(ret)
                } else {
                    Err(rail_segment::Error::NeighborError)
                }
            },
            _ => Err(rail_segment::Error::EndpointError),
        }
    }
    
    fn check_connection(&self, endpoint: usize) -> Result<bool, rail_segment::Error> {
        match endpoint {
            0 => {
                match &self.lower_connection {
                    Some(_) => Ok(true),
                    None    => Ok(false),
                }
            },
            1 => {
                match &self.upper_connection {
                    Some(_) => Ok(true),
                    None    => Ok(false),
                }
            },
            _ => Err(rail_segment::Error::EndpointError),
        }
    }
    
    fn connection_iter(&self) -> rail_segment::ConnectionIter {
        rail_segment::ConnectionIter::new(self)
    }
    
    fn add_connection(&mut self, endpoint: usize, connector: RailConnector) -> Result<bool, rail_segment::Error> {
        match endpoint {
            0 => {
                self.lower_connection = Option::Some(connector);
                Ok(true)
            },
            1 => {
                self.upper_connection = Option::Some(connector);
                Ok(true)
            },
            _ => Err(rail_segment::Error::EndpointError),
        }
    }
    
    fn remove_connection(&mut self, endpoint: usize) -> Result<bool, rail_segment::Error> {
        match endpoint {
            0 => {
                self.lower_connection = Option::None;
                Ok(true)
            },
            1 => {
                self.upper_connection = Option::None;
                Ok(true)
            },
            _ => Err(rail_segment::Error::EndpointError),
        }
    }
    
    fn remove_all_connections(&mut self) {
        self.lower_connection = Option::None;
        self.upper_connection = Option::None;
    }
    
    fn get_state(&self) -> f32 {
        0.0
    }
    
    fn set_state(&mut self, _: f32) {
        // This does nothing for simple segments.
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::samplers::tangent_sampler::*;
    
    fn compare_vectors(vec1: Vector3, vec2: Vector3, epsilon: f32, message: &str) {
        assert!((vec1.x - vec2.x).abs() <= epsilon && (vec1.y - vec2.y).abs() <= epsilon,
                "{message} Expected {vec2}, received {vec1}.")
    }
    
    /*fn compare_floats(float1: f32, float2: f32, epsilon: f32, message: &str) {
        assert!((float1 - float2).abs() <= epsilon,
                "{message} Expected {float2}, received {float1}.")
    }*/
    
    #[test]
    fn test_basis_new_looking_at() {
        let test_basis = new_looking_at(Vector3::FORWARD, Vector3::UP, false);
        assert_eq!(test_basis.col_a(), Vector3::RIGHT,
                   "Column A was incorrect.");
        assert_eq!(test_basis.col_b(), Vector3::UP,
                   "Column B was incorrect.");
        assert_eq!(test_basis.col_c(), Vector3::BACK,
                   "Column C was incorrect.");
    }
    
    #[test]
    fn test_get_length() {
        let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0));
        let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0);
        let test_segment = SimpleSegment::new(alignment, elevation);
        assert_eq!(test_segment.get_length(1).unwrap(), 17.0,
                   "Segment length was incorrect.");
    }
    
    #[test]
    fn test_sample() {
        let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0));
        let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0);
        let test_segment = SimpleSegment::new(alignment, elevation);
        assert_eq!(test_segment.sample(1, 0.0).unwrap(), Vector3::new(1.0, 1.0, 1.0),
                   "Segment begins at the wrong location.");
        assert_eq!(test_segment.sample(1, 17.0).unwrap(), Vector3::new(10.0, 9.0, 13.0),
                   "Segment ends at the wrong location.");
        assert_eq!(test_segment.sample(1, 8.5).unwrap(), Vector3::new(5.5, 5.0, 7.0),
                   "Segment passes through the wrong location.");
    }
    
    #[test]
    fn test_sample_with_rotation() {
        let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0));
        let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0);
        let test_segment = SimpleSegment::new(alignment, elevation);
        let direction_forward = Vector3::new(-9.0 / 17.0, -8.0 / 17.0, -12.0 / 17.0);
        let direction_right = Vector3::new(-0.8, 0.0, 0.6);
        let direction_up = direction_forward.cross(direction_right);
        let transform1 = test_segment.sample_with_rotation(1, 0.0).unwrap();
        let epsilon = 1e-6;
        assert_eq!(transform1.origin, Vector3::new(1.0, 1.0, 1.0),
                   "Segment begins at the wrong location.");
        compare_vectors(transform1.basis.col_c(), direction_forward, epsilon,
                        "Segment begins facing the wrong direction.");
        compare_vectors(transform1.basis.col_a(), direction_right, epsilon,
                        "Segment begins with the wrong right direction.");
        compare_vectors(transform1.basis.col_b(), direction_up, epsilon,
                        "Segment begins with the wrong up direction.");
        let transform2 = test_segment.sample_with_rotation(1, 17.0).unwrap();
        assert_eq!(transform2.origin, Vector3::new(10.0, 9.0, 13.0),
                   "Segment ends at the wrong location.");
        compare_vectors(transform2.basis.col_c(), direction_forward, epsilon,
                        "Segment ends facing the wrong direction.");
        compare_vectors(transform2.basis.col_a(), direction_right, epsilon,
                        "Segment ends with the wrong righ direction.");
        compare_vectors(transform2.basis.col_b(), direction_up, epsilon,
                        "Segment ends with the wrong up direction.");
        let transform3 = test_segment.sample_with_rotation(1, 8.5).unwrap();
        assert_eq!(transform3.origin, Vector3::new(5.5, 5.0, 7.0),
                   "Segment passes through the wrong location.");
        compare_vectors(transform3.basis.col_c(), direction_forward, epsilon,
                        "Segment passes through facing the wrong direction.");
        compare_vectors(transform3.basis.col_a(), direction_right, epsilon,
                        "Segment passes through with the wrong righ direction.");
        compare_vectors(transform3.basis.col_b(), direction_up, epsilon,
                        "Segment passes through with the wrong up direction.");
    }
    
    #[test]
    fn test_sample_with_rotation_reverse() {
        let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0));
        let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0);
        let test_segment = SimpleSegment::new(alignment, elevation);
        let direction_forward = Vector3::new(9.0 / 17.0, 8.0 / 17.0, 12.0 / 17.0);
        let direction_right = Vector3::new(0.8, 0.0, -0.6);
        let direction_up = direction_forward.cross(direction_right);
        let transform1 = test_segment.sample_with_rotation_reverse(1, 0.0).unwrap();
        let epsilon = 1e-6;
        assert_eq!(transform1.origin, Vector3::new(1.0, 1.0, 1.0),
                   "Segment begins at the wrong location.");
        compare_vectors(transform1.basis.col_c(), direction_forward, epsilon,
                        "Segment begins facing the wrong direction.");
        compare_vectors(transform1.basis.col_a(), direction_right, epsilon,
                        "Segment begins with the wrong right direction.");
        compare_vectors(transform1.basis.col_b(), direction_up, epsilon,
                        "Segment begins with the wrong up direction.");
        let transform2 = test_segment.sample_with_rotation_reverse(1, 17.0).unwrap();
        assert_eq!(transform2.origin, Vector3::new(10.0, 9.0, 13.0),
                   "Segment ends at the wrong location.");
        compare_vectors(transform2.basis.col_c(), direction_forward, epsilon,
                        "Segment ends facing the wrong direction.");
        compare_vectors(transform2.basis.col_a(), direction_right, epsilon,
                        "Segment ends with the wrong righ direction.");
        compare_vectors(transform2.basis.col_b(), direction_up, epsilon,
                        "Segment ends with the wrong up direction.");
        let transform3 = test_segment.sample_with_rotation_reverse(1, 8.5).unwrap();
        assert_eq!(transform3.origin, Vector3::new(5.5, 5.0, 7.0),
                   "Segment passes through the wrong location.");
        compare_vectors(transform3.basis.col_c(), direction_forward, epsilon,
                        "Segment passes through facing the wrong direction.");
        compare_vectors(transform3.basis.col_a(), direction_right, epsilon,
                        "Segment passes through with the wrong righ direction.");
        compare_vectors(transform3.basis.col_b(), direction_up, epsilon,
                        "Segment passes through with the wrong up direction.");
    }
    
    #[test]
    fn test_sample_up_vector() {
        let alignment = TangentAlignmentSampler::new(Vector2::new(1.0, 1.0), Vector2::new(10.0, 13.0));
        let elevation = TangentElevationSampler::new(1.0, 9.0, 15.0);
        let test_segment = SimpleSegment::new(alignment, elevation);
        let direction_forward = Vector3::new(-9.0 / 17.0, -8.0 / 17.0, -12.0 / 17.0);
        let direction_right = Vector3::new(-0.8, 0.0, 0.6);
        let direction_up = direction_forward.cross(direction_right);
        let up1 = test_segment.sample_up_vector(1, 0.0).unwrap();
        let epsilon = 1e-6;
        compare_vectors(up1, direction_up, epsilon,
                        "Segment begins with the wrong up direction.");
        let up2 = test_segment.sample_up_vector(1, 17.0).unwrap();
        compare_vectors(up2, direction_up, epsilon,
                        "Segment ends with the wrong up direction.");
        let up3 = test_segment.sample_up_vector(1, 8.5).unwrap();
        compare_vectors(up3, direction_up, epsilon,
                        "Segment passes through with the wrong up direction.");
    }
}