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)
}

/**
 * A simple segment with one alignment sampler and one elevation sampler.
 * This necessarily has exactly 2 endpoints and 1 sub-segment.
 * Despite the sub-segment being unambiguous, it shall still fail if the incorrect index is used.
 * This is also true of its float state.
 */
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: f32,
}

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

impl<A: AlignmentSampler, E: ElevationSampler> RailSegment for SimpleSegment<A, E> {
    fn get_length(&self, index: usize) -> Result<f32, rail_segment::Error> {
        if index == 0 {
            Ok(self.length)
        } else {
            Err(rail_segment::Error::IndexError(index))
        }
    }
    
    fn sample(&self, index: usize, offset: f32) -> Result<Vector3, rail_segment::Error> {
        if index != 0 {
            return Err(rail_segment::Error::IndexError(index));
        } else if offset < 0.0 || offset > self.length {
            return Err(rail_segment::Error::OffsetError(offset));
        }
        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 index != 0 {
            return Err(rail_segment::Error::IndexError(index));
        } else if offset < 0.0 || offset > self.length {
            return Err(rail_segment::Error::OffsetError(offset));
        }
        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 index != 0 {
            return Err(rail_segment::Error::IndexError(index));
        } else if offset < 0.0 || offset > self.length {
            return Err(rail_segment::Error::OffsetError(offset));
        }
        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 index != 0 {
            return Err(rail_segment::Error::IndexError(index));
        } else if offset < 0.0 || offset > self.length {
            return Err(rail_segment::Error::OffsetError(offset));
        }
        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> {
        if endpoint > 1 {
            return Err(rail_segment::Error::EndpointError(endpoint));
        } else {
            Ok(0)
        }
    }
    
    fn get_endpoint_transform(&self, endpoint: usize) -> Result<Transform3D, rail_segment::Error> {
        match endpoint {
            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(endpoint)),
        }
    }
    
    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(endpoint)),
        }
    }
    
    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::ConnectionError(endpoint))
                }
            },
            1 => {
                if let Some(ret) = &self.upper_connection {
                    Ok(ret)
                } else {
                    Err(rail_segment::Error::ConnectionError(endpoint))
                }
            },
            _ => Err(rail_segment::Error::EndpointError(endpoint)),
        }
    }
    
    fn get_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::ConnectionError(endpoint))
                }
            },
            1 => {
                if let Some(ret) = self.upper_connection {
                    Ok(ret)
                } else {
                    Err(rail_segment::Error::ConnectionError(endpoint))
                }
            },
            _ => Err(rail_segment::Error::EndpointError(endpoint)),
        }
    }
    
    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(endpoint)),
        }
    }
    
    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(endpoint)),
        }
    }
    
    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(endpoint)),
        }
    }
    
    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, state: f32) -> Result<f32, rail_segment::Error> {
        Err(rail_segment::Error::StateError(state))
    }
}

#[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);
        
        // test success
        assert!(match test_segment.get_length(0) { Ok(17.0)=>true, _=>false, },
                "Segment length was incorrect.");
                
        // test failure
        assert!(match test_segment.get_length(1) { Err(rail_segment::Error::IndexError(1))=>true, _=>false, },
                "Got length of non-existant sub-segment.");
    }
    
    #[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);
        
        // test successes
        assert_eq!(test_segment.sample(0, 0.0).unwrap(), Vector3::new(1.0, 1.0, 1.0),
                   "Segment begins at the wrong location.");
        assert_eq!(test_segment.sample(0, 17.0).unwrap(), Vector3::new(10.0, 9.0, 13.0),
                   "Segment ends at the wrong location.");
        assert_eq!(test_segment.sample(0, 8.5).unwrap(), Vector3::new(5.5, 5.0, 7.0),
                   "Segment passes through the wrong location.");
                   
        // test failures
        assert!(match test_segment.sample(1, 0.0) { Err(rail_segment::Error::IndexError(1))=>true, _=>false, },
                "Sampled non-existant sub-segment.");
        assert!(match test_segment.sample(0, -1.0) { Err(rail_segment::Error::OffsetError(-1.0))=>true, _=>false, },
                "Sampled segment with negative offset.");
        assert!(match test_segment.sample(0, 18.0) { Err(rail_segment::Error::OffsetError(18.0))=>true, _=>false, },
                "Sampled segment with excess offset.");
    }
    
    #[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(0, 0.0).unwrap();
        let epsilon = 1e-6;
        
        // test successes
        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(0, 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 right direction.");
        compare_vectors(transform2.basis.col_b(), direction_up, epsilon,
                        "Segment ends with the wrong up direction.");
        let transform3 = test_segment.sample_with_rotation(0, 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 failures
        assert!(match test_segment.sample_with_rotation(1, 0.0) { Err(rail_segment::Error::IndexError(1))=>true, _=>false, },
                "Sampled non-existant sub-segment.");
        assert!(match test_segment.sample_with_rotation(0, -1.0) { Err(rail_segment::Error::OffsetError(-1.0))=>true, _=>false, },
                "Sampled segment with negative offset.");
        assert!(match test_segment.sample_with_rotation(0, 18.0) { Err(rail_segment::Error::OffsetError(18.0))=>true, _=>false, },
                "Sampled segment with excess offset.");
    }
    
    #[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(0, 0.0).unwrap();
        let epsilon = 1e-6;
        
        // test successes
        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(0, 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 right 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(0, 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 failures
        assert!(match test_segment.sample_with_rotation_reverse(1, 0.0) { Err(rail_segment::Error::IndexError(1))=>true, _=>false, },
                "Sampled non-existant sub-segment.");
        assert!(match test_segment.sample_with_rotation_reverse(0, -1.0) { Err(rail_segment::Error::OffsetError(-1.0))=>true, _=>false, },
                "Sampled segment with negative offset.");
        assert!(match test_segment.sample_with_rotation_reverse(0, 18.0) { Err(rail_segment::Error::OffsetError(18.0))=>true, _=>false, },
                "Sampled segment with excess offset.");
    }
    
    #[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(0, 0.0).unwrap();
        let epsilon = 1e-6;
        
        // test successes
        compare_vectors(up1, direction_up, epsilon,
                        "Segment begins with the wrong up direction.");
        let up2 = test_segment.sample_up_vector(0, 17.0).unwrap();
        compare_vectors(up2, direction_up, epsilon,
                        "Segment ends with the wrong up direction.");
        let up3 = test_segment.sample_up_vector(0, 8.5).unwrap();
        compare_vectors(up3, direction_up, epsilon,
                        "Segment passes through with the wrong up direction.");
                        
        // test failures
        assert!(match test_segment.sample_up_vector(1, 0.0) { Err(rail_segment::Error::IndexError(1))=>true, _=>false, },
                "Sampled non-existant sub-segment.");
        assert!(match test_segment.sample_up_vector(0, -1.0) { Err(rail_segment::Error::OffsetError(-1.0))=>true, _=>false, },
                "Sampled segment with negative offset.");
        assert!(match test_segment.sample_up_vector(0, 18.0) { Err(rail_segment::Error::OffsetError(18.0))=>true, _=>false, },
                "Sampled segment with excess offset.");
    }
    
    #[test]
    fn test_segment_from_endpoint() {
        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);
        
        // test successes
        assert_eq!(test_segment.segment_from_endpoint(0).unwrap(), 0, "Endpoint 0 points to incorrect internal segment.");
        assert_eq!(test_segment.segment_from_endpoint(1).unwrap(), 0, "Endpoint 1 points to incorrect internal segment.");
        
        // test failures
        assert!(match test_segment.segment_from_endpoint(2) { Err(rail_segment::Error::EndpointError(2))=>true, _=>false, },
                "Endpoint 2 points to an internal segment.");
    }
    
    #[test]
    fn test_get_endpoint_transform() {
        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);
        
        // test successes
        let transform1 = test_segment.get_endpoint_transform(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(0, 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 right direction.");
        compare_vectors(transform2.basis.col_b(), direction_up, epsilon,
                        "Segment ends with the wrong up direction.");
        
        // test failures
        assert!(match test_segment.get_endpoint_transform(2) { Err(rail_segment::Error::EndpointError(2))=>true, _=>false, },
                "Endpoint 2 has a transform.");
    }
    
    #[test]
    fn test_is_endpoint_upper() {
        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);
        
        // test successes
        assert_eq!(test_segment.is_endpoint_upper(0).unwrap(), false, "Endpoint 0 is upper.");
        assert_eq!(test_segment.is_endpoint_upper(1).unwrap(), true, "Endpoint 1 is lower.");
        
        // test failures
        assert!(match test_segment.is_endpoint_upper(2) { Err(rail_segment::Error::EndpointError(2))=>true, _=>false, },
                "Endpoint 2 should not be upper or lower.");
    }
    
    #[test]
    fn test_num_endpoints() {
        // By far the most critical and intricate test.
        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.num_endpoints(), 2, "Incorrect number of endpoints.");
    }
    
    // For now, I'm not going to write tests for most of the connection methods since they're basically all getters and setters.
}