use crate::prelude::*;
use crate::rail_segment::RailSegment;
use crate::rail_connector::RailConnector;
//use std::collections::HashMap;
use std::vec::Vec;
//use std::string::String;

pub struct RailGraph {
    segments: Vec<Option<Box<dyn RailSegment>>>,
    // segment_names: HashMap<String, usize>,
    
    min_available_index: usize,
}

impl RailGraph {
    pub fn new() -> Self {
        Self {
            segments: Vec::<Option::<Box::<dyn RailSegment>>>::new(),
            //segment_names: {},
            min_available_index: 0,
        }
    }
    
    pub fn get_segment(&self, index: usize) -> Option<&Box<dyn RailSegment>> {
        match &self.segments.get(index) {
            Some(ret) => ret.as_ref(),
            None      => None,
        }
    }
    
    pub fn add_segment(&mut self, segment: Box<dyn RailSegment>) -> usize {
        let ret = self.min_available_index;
        if self.segments.len() == self.min_available_index {
            self.segments.push(Some(segment));
            self.min_available_index = self.segments.len();
        } else {
            self.segments[self.min_available_index] = Some(segment);
            self.update_min_available_index();
        }
        ret
    }
    
    pub fn remove_segment(&mut self, index: usize) {
        self.isolate_segment(index);
        self.segments[index] = Option::None;
        if index < self.min_available_index {
            self.min_available_index = index;
        }
    }
    
    pub fn connect_endpoints(&mut self, index1: usize, endpoint1: usize, index2: usize, endpoint2: usize) {
        // Needs improvement: silently fails with invalid endpoints.
        let connector1 = RailConnector::new(index2, endpoint2);
        let connector2 = RailConnector::new(index1, endpoint1);
        // Before we continue, make sure that anything that these endpoints are already connected to are disconnected.
        if let Some(segment1) = &self.segments[index1] {
            if let Ok(old_connection) = segment1.borrow_connection(endpoint1) {
                let old_index = old_connection.segment_index;
                let old_endpoint = old_connection.endpoint_index;
                if let Some(old_segment) = &mut self.segments[old_index] {
                    let _ = old_segment.remove_connection(old_endpoint);
                }
            }
        }
        if let Some(segment2) = &self.segments[index2] {
            if let Ok(old_connection) = segment2.borrow_connection(endpoint2) {
                let old_index = old_connection.segment_index;
                let old_endpoint = old_connection.endpoint_index;
                if let Some(old_segment) = &mut self.segments[old_index] {
                    let _ = old_segment.remove_connection(old_endpoint);
                }
            }
        }
        // Connect them now
        if let Some(segment1) = &mut self.segments[index1] {
            let _ = segment1.add_connection(endpoint1, connector1);
        }
        if let Some(segment2) = &mut self.segments[index2] {
            let _ = segment2.add_connection(endpoint2, connector2);
        }
    }
    
    pub fn disconnect_endpoint(&mut self, index: usize, endpoint: usize) {
        if let Some(segment) = &self.segments[index] {
            if let Ok(old_connection) = segment.borrow_connection(endpoint) {
                let old_index = old_connection.segment_index;
                let old_endpoint = old_connection.endpoint_index;
                if let Some(old_segment) = &mut self.segments[old_index] {
                    let _ = old_segment.remove_connection(old_endpoint);
                }
            }
        }
        if let Some(segment) = &mut self.segments[index] {
            let _ = segment.remove_connection(endpoint);
        }
    }
    
    pub fn append_segment(&mut self,
                          new_segment: Box<dyn RailSegment>,
                          new_segment_endpoint: usize,
                          onto_segment: usize,
                          ont_segment_endpoint: usize) {
        let index = self.add_segment(new_segment);
        self.connect_endpoints(index, new_segment_endpoint, onto_segment, ont_segment_endpoint);
    }
    
    fn isolate_segment(&mut self, index: usize) {
        // Get the old connections
        let mut connections = Vec::<RailConnector>::new();
        if let Some(segment) = &self.segments[index] {
            for connection in segment.connection_iter() {
                connections.push(*connection);
            }
        }
        
        // disconnect neighbors
        for connection in connections {
            if let Some(old_segment) = &mut self.segments[connection.segment_index] {
                let _ = old_segment.remove_connection(connection.endpoint_index);
            }
        }
        
        // reflect the same change in the original segment.
        if let Some(segment) = &mut self.segments[index] {
            segment.remove_all_connections();
        }
    }
    
    fn update_min_available_index(&mut self) {
        while let Some(_segment) = &self.segments[self.min_available_index] {
            self.min_available_index += 1;
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::samplers::tangent_sampler::*;
    use crate::samplers::circular_sampler::*;
    use crate::samplers::sampler::*;
    use crate::simple_segment::*;
    use std::f32::consts::PI;
    
    /*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}.")
    }*/
    
    fn create_test_graph() -> RailGraph {
        let alignment1 = TangentAlignmentSampler::new(Vector2::new(0.0, 0.0), Vector2::new(1.0, 0.0));
        let elevation1 = TangentElevationSampler::new(0.0, 0.0, 1.0);
        let test_segment1 = Box::new(SimpleSegment::new(alignment1, elevation1));
        let alignment2 = CircularAlignmentSampler::new(0.0, PI * 0.5, Vector2::new(1.0, 1.0), 1.0, true);
        let elevation2 = TangentElevationSampler::new(0.0, 0.0, alignment2.length());
        let test_segment2 = Box::new(SimpleSegment::new(alignment2, elevation2));
        let alignment3 = TangentAlignmentSampler::new(Vector2::new(2.0, 1.0), Vector2::new(2.0, 2.0));
        let elevation3 = TangentElevationSampler::new(0.0, 0.0, 1.0);
        let test_segment3 = Box::new(SimpleSegment::new(alignment3, elevation3));
        
        let mut test_graph = RailGraph::new();
        test_graph.add_segment(test_segment1);
        test_graph.append_segment(test_segment2, 0, 0, 1);
        test_graph.append_segment(test_segment3, 0, 1, 1);
        return test_graph;
    }
    
    #[test]
    fn test_get_segment() {
        let test_graph = create_test_graph();
        assert!(match test_graph.get_segment(0) { Option::Some(_)=>true, _=>false },
                "Segment 0 could not be retrieved.");
        assert!(match test_graph.get_segment(1) { Option::Some(_)=>true, _=>false },
                "Segment 1 could not be retrieved.");
        assert!(match test_graph.get_segment(2) { Option::Some(_)=>true, _=>false },
                "Segment 2 could not be retrieved.");
        assert!(match test_graph.get_segment(3) { Option::None=>true, _=>false },
                "Segment that didn't exist was retrieved.");
    }
    
    #[test]
    fn test_add_segment() {
        let alignment = TangentAlignmentSampler::new(Vector2::new(2.0, 2.0), Vector2::new(2.0, 3.0));
        let elevation = TangentElevationSampler::new(0.0, 0.0, 1.0);
        let test_segment = Box::new(SimpleSegment::new(alignment, elevation));
        
        let mut test_graph = create_test_graph();
        let index = test_graph.add_segment(test_segment);
        assert_eq!(index, 3, "Segment was added with incorrect index.");
        assert_eq!(test_graph.segments.len(), 4, "Graph has incorrect number of segments.");
    }
    
    #[test]
    fn test_remove_segment() {
        let mut test_graph = create_test_graph();
        test_graph.remove_segment(1);
        assert_eq!(test_graph.segments.len(), 3, "Graph has incorrect number of segments.");
        assert_eq!(test_graph.min_available_index, 1, "Incorrect min_available_index.");
        let first_segment = test_graph.get_segment(0).unwrap();
        assert!(!first_segment.check_connection(1).unwrap(), "Connection 1 wasn't broken.");
        let second_segment = test_graph.get_segment(2).unwrap();
        assert!(!second_segment.check_connection(0).unwrap(), "Connection 2 wasn't broken.");
        assert!(match test_graph.get_segment(1) { Option::None=>true, _=>false },
                "Segment that didn't exist was retrieved.");
    }
    
    #[test]
    fn test_connect_endpoints() {
        let mut test_graph = create_test_graph();
        test_graph.connect_endpoints(0, 0, 2, 1);
        let first_segment = test_graph.get_segment(0).unwrap();
        let first_connection = first_segment.borrow_connection(0).unwrap();
        assert_eq!(first_connection.segment_index, 2, "Connection(0, 0) had the incorrect index.");
        assert_eq!(first_connection.endpoint_index, 1, "Connection(0, 0) had the incorrect endpoint.");
        let second_segment = test_graph.get_segment(2).unwrap();
        let second_connection = second_segment.borrow_connection(1).unwrap();
        assert_eq!(second_connection.segment_index, 0, "Connection(2, 1) had the incorrect index.");
        assert_eq!(second_connection.endpoint_index, 0, "Connection(2, 1) had the incorrect endpoint.");
        test_graph.connect_endpoints(0, 1, 2, 0);
        let first_segment = test_graph.get_segment(0).unwrap();
        let first_connection = first_segment.borrow_connection(1).unwrap();
        assert_eq!(first_connection.segment_index, 2, "Connection(0, 1) had the incorrect index.");
        assert_eq!(first_connection.endpoint_index, 0, "Connection(0, 1) had the incorrect endpoint.");
        let second_segment = test_graph.get_segment(2).unwrap();
        let second_connection = second_segment.borrow_connection(0).unwrap();
        assert_eq!(second_connection.segment_index, 0, "Connection(2, 0) had the incorrect index.");
        assert_eq!(second_connection.endpoint_index, 1, "Connection(2, 0) had the incorrect endpoint.");
        let middle_segment = test_graph.get_segment(1).unwrap();
        let first_connection = middle_segment.borrow_connection(0);
        assert!(match first_connection { Result::Err(_)=>true, _=>false },
                "Connection(1, 0) still exists.");
        let second_connection = middle_segment.borrow_connection(1);
        assert!(match second_connection { Result::Err(_)=>true, _=>false },
                "Connection(1, 1) still exists.")
    }
    
    #[test]
    fn test_disconnect_endpoint() {
        let mut test_graph = create_test_graph();
        test_graph.disconnect_endpoint(0, 1);
        let first_segment = test_graph.get_segment(0).unwrap();
        let first_connection = first_segment.borrow_connection(1);
        assert!(match first_connection { Result::Err(_)=>true, _=>false },
                "Connection(0, 1) still exists.");
        let middle_segment = test_graph.get_segment(1).unwrap();
        let middle_connection = middle_segment.borrow_connection(0);
        assert!(match middle_connection { Result::Err(_)=>true, _=>false },
                "Connection(1, 0) still exists.");
    }
    
    #[test]
    fn test_append_segment() {
        let test_graph = create_test_graph();
        let first_segment = test_graph.get_segment(0).unwrap();
        let second_segment = test_graph.get_segment(1).unwrap();
        let third_segment = test_graph.get_segment(2).unwrap();
        
        let connection01 = first_segment.borrow_connection(1).unwrap();
        assert_eq!(connection01.segment_index, 1, "Connection(0, 1) had the incorrect index.");
        assert_eq!(connection01.endpoint_index, 0, "Connection(0, 1) had the incorrect endpoint.");
        let connection10 = second_segment.borrow_connection(0).unwrap();
        assert_eq!(connection10.segment_index, 0, "Connection(1, 0) had the incorrect index.");
        assert_eq!(connection10.endpoint_index, 1, "Connection(1, 0) had the incorrect endpoint.");
        let connection11 = second_segment.borrow_connection(1).unwrap();
        assert_eq!(connection11.segment_index, 2, "Connection(1, 1) had the incorrect index.");
        assert_eq!(connection11.endpoint_index, 0, "Connection(1, 1) had the incorrect endpoint.");
        let connection20 = third_segment.borrow_connection(0).unwrap();
        assert_eq!(connection20.segment_index, 1, "Connection(2, 0) had the incorrect index.");
        assert_eq!(connection20.endpoint_index, 1, "Connection(2, 0) had the incorrect endpoint.");
    }
}