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

#[derive(Debug)]
pub enum Error {
    IndexError(usize),
    SegmentError(rail_segment::Error),
}

impl fmt::Display for Error {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            Self::IndexError(index) => write!(f, "Nonexistent segment index: {index}."),
            Self::SegmentError(err) => write!(f, "Segment error: {err}"),
        }
    }
}

impl error::Error for Error {}

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,
        }
    }
    
    // definition
    
    pub fn get_segment(&self, index: usize) -> Result<&Box<dyn RailSegment>, Error> {
        match &self.segments.get(index) {
            Some(Some(ret)) => Ok(ret),
            _               => Err(Error::IndexError(index)),
        }
    }
    
    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) -> Result<bool, Error> {
        if index >= self.segments.len() {
            return Err(Error::IndexError(index));
        }
        
        self.isolate_segment(index);
        self.segments[index] = Option::None;
        if index < self.min_available_index {
            self.min_available_index = index;
        }
        Ok(true)
    }
    
    pub fn connect_endpoints(&mut self, index1: usize, endpoint1: usize, index2: usize, endpoint2: usize) -> Result<bool, Error> {
        // validate input first
        if !self.validate_index(index1) {
            return Err(Error::IndexError(index1));
        } else if !self.validate_index(index2) {
            return Err(Error::IndexError(index2));
        } else if !self.validate_endpoint(index1, endpoint1) {
            return Err(Error::SegmentError(rail_segment::Error::EndpointError(endpoint1)));
        } else if !self.validate_endpoint(index2, endpoint2) {
            return Err(Error::SegmentError(rail_segment::Error::EndpointError(endpoint2)));
        }
        
        // 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.
        let old_connection1 = self.get_connection_from_index(index1, endpoint1);
        let old_connection2 = self.get_connection_from_index(index2, endpoint2);
        if let Ok(old_connection) = old_connection1 {
            match &mut self.segments[old_connection.segment_index] {
                Some(old_segment) => {
                    if let Err(e) = old_segment.remove_connection(old_connection.endpoint_index) {
                        return Err(Error::SegmentError(e));
                    }
                },
                None => {
                    return Err(Error::IndexError(old_connection.segment_index));
                }
            }
        }
        if let Ok(old_connection) = old_connection2 {
            match &mut self.segments[old_connection.segment_index] {
                Some(old_segment) => {
                    if let Err(e) = old_segment.remove_connection(old_connection.endpoint_index) {
                        return Err(Error::SegmentError(e));
                    }
                },
                None => {
                    return Err(Error::IndexError(old_connection.segment_index));
                }
            }
        }
        // Connect them now
        let segment1 = &mut self.segments[index1].as_mut().unwrap();
        let _ = segment1.add_connection(endpoint1, connector1);
        let segment2 = &mut self.segments[index2].as_mut().unwrap();
        let _ = segment2.add_connection(endpoint2, connector2);
        
        //if let Some(segment1) = &mut self.segments[index1] {
        //    if let Err(e) = segment1.add_connection(endpoint1, connector1) {
        //        return Err(Error::SegmentError(e));
        //    } 
        //}
        //if let Some(segment2) = &mut self.segments[index2] {
        //    if let Err(e) = segment2.add_connection(endpoint2, connector2) {
        //        return Err(Error::SegmentError(e));
        //    }
        //}
        Ok(true)
    }
    
    pub fn disconnect_endpoint(&mut self, index: usize, endpoint: usize) -> Result<bool, Error> {
        if index >= self.segments.len() {
            return Err(Error::IndexError(index));
        }
        
        match self.get_connection_from_index(index, endpoint) {
            Ok(old_connection) => {
                match &mut self.segments[old_connection.segment_index] {
                    Some(old_segment) => {
                        if let Err(e) = old_segment.remove_connection(old_connection.endpoint_index) {
                            return Err(Error::SegmentError(e));
                        }
                    },
                    None              => {
                        return Err(Error::IndexError(old_connection.segment_index));
                    },
                }
            },
            Err(e) => {
                return Err(e);
            },
        }
        
        if let Some(segment) = &mut self.segments[index] {
            match segment.remove_connection(endpoint) {
                Ok(_)  => Ok(true),
                Err(e) => Err(Error::SegmentError(e)),
            }
        } else {
            Err(Error::IndexError(index)) // should never happen since this is already tested in the big match block.
        }
    }
    
    pub fn append_segment(&mut self,
                          new_segment: Box<dyn RailSegment>,
                          new_segment_endpoint: usize,
                          onto_segment: usize,
                          ont_segment_endpoint: usize) -> Result<bool, Error> {
        let index = self.add_segment(new_segment);
        self.connect_endpoints(index, new_segment_endpoint, onto_segment, ont_segment_endpoint)
    }
    
    // querying and traversal
    
    //pub fn transform_of_coord(&self, coord: &GraphCoord) -> Transform3D {
    //    if let Some(segment) = self.
    //}
    
    // private
    
    fn validate_index(&self, index: usize) -> bool {
        match &self.segments.get(index) {
            Some(Some(_)) => true,
            _             => false,
        }
    }
    
    fn validate_endpoint(&self, index: usize, endpoint: usize) -> bool {
        match self.get_segment(index) {
            Ok(segment) => {
                match segment.check_connection(endpoint) {
                    Err(rail_segment::Error::EndpointError(endpoint)) => false,
                    _                                                 => true,
                }
            },
            Err(_)      => false,
        }
    }
    
    fn get_connection_from_segment(segment: &Box<dyn RailSegment>, endpoint: usize) -> Result<RailConnector, Error> {
        match segment.get_connection(endpoint) {
            Ok(connection) => Ok(connection),
            Err(e)         => Err(Error::SegmentError(e)),
        }
    }
    
    fn get_connection_from_index(&self, index: usize, endpoint: usize) -> Result<RailConnector, Error> {
        match self.get_segment(index) {
            Ok(segment) => Self::get_connection_from_segment(segment, endpoint),
            Err(e)      => Err(e),
        }
    }
    
    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);
        let _ = test_graph.append_segment(test_segment2, 0, 0, 1);
        let _ = test_graph.append_segment(test_segment3, 0, 1, 1);
        return test_graph;
    }
    
    #[test]
    fn test_get_segment() {
        let mut test_graph = create_test_graph();
        
        // test successes
        assert!(match test_graph.get_segment(0) { Ok(_)=>true, Err(_)=>false, },
                "Segment 0 could not be retrieved.");
        assert!(match test_graph.get_segment(1) { Ok(_)=>true, _=>false, },
                "Segment 1 could not be retrieved.");
        assert!(match test_graph.get_segment(2) { Ok(_)=>true, _=>false, },
                "Segment 2 could not be retrieved.");
                
        // test failures
        assert!(match test_graph.get_segment(3) { Err(Error::IndexError(3)) =>true, _=>false, },
                "Segment was retrieved: index out of bounds.");
        _ = test_graph.remove_segment(1);
        assert!(match test_graph.get_segment(1) { Err(Error::IndexError(1)) =>true, _=>false, },
                "Segment was retrieved: deleted.");
    }
    
    #[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 failures
        assert!(match test_graph.remove_segment(3) { Err(Error::IndexError(3))=>true, _=>false, },
                "Non-existent segment was removed.");
        
        // test successes
        assert!(match test_graph.remove_segment(1) { Ok(true)=>true, _=>false, },
                "Segment was not removed.");
        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) { Err(Error::IndexError(1))=>true, _=>false, },
                "Segment that didn't exist was retrieved.");
    }
    
    #[test]
    fn test_connect_endpoints() {
        let mut test_graph = create_test_graph();
        
        // test failures
        assert!(match test_graph.connect_endpoints(3, 0, 2, 1) { Err(Error::IndexError(3))=>true, _=> false, },
                "Connection(3, 0) to (2, 1) was made.");
        assert!(match test_graph.get_segment(2).unwrap().borrow_connection(1) { Err(rail_segment::Error::ConnectionError(1))=>true, _=>false, },
                "Invalid connection at (2, 1) #1.");
        assert!(match test_graph.connect_endpoints(0, 2, 2, 1) { Err(Error::SegmentError(rail_segment::Error::EndpointError(2)))=>true, _=> false, },
                "Connection(0, 2) to (2, 1) was made.");
        assert!(match test_graph.get_segment(2).unwrap().borrow_connection(1) { Err(rail_segment::Error::ConnectionError(1))=>true, _=>false, },
                "Invalid connection at (2, 1) #2.");
        assert!(match test_graph.connect_endpoints(0, 0, 3, 1) { Err(Error::IndexError(3))=>true, _=> false, },
                "Connection(0, 0) to (3, 1) was made.");
        assert!(match test_graph.get_segment(0).unwrap().borrow_connection(0) { Err(rail_segment::Error::ConnectionError(0))=>true, _=>false, },
                "Invalid connection at (0, 0) #1.");
        assert!(match test_graph.connect_endpoints(0, 0, 2, 2) { Err(Error::SegmentError(rail_segment::Error::EndpointError(2)))=>true, _=> false, },
                "Connection(0, 0) to (2, 2) was made.");
        assert!(match test_graph.get_segment(0).unwrap().borrow_connection(0) { Err(rail_segment::Error::ConnectionError(0))=>true, _=>false, },
                "Invalid connection at (0, 0) #2.");
        assert!(match test_graph.connect_endpoints(0, 1, 3, 1) { Err(Error::IndexError(3))=>true, _=> false, },
                "Connection(0, 1) to (3, 1) was made.");
        assert!(match test_graph.get_segment(0).unwrap().borrow_connection(1) { Ok(_)=>true, _=>false, },
                "Removed connection at (0, 1) #1.");
        assert!(match test_graph.connect_endpoints(0, 1, 2, 2) { Err(Error::SegmentError(rail_segment::Error::EndpointError(2)))=>true, _=> false, },
                "Connection(0, 1) to (2, 2) was made.");
        assert!(match test_graph.get_segment(0).unwrap().borrow_connection(1) { Ok(_)=>true, _=>false, },
                "Removed connection at (0, 1) #2.");
        
        // test successes
        assert!(match test_graph.connect_endpoints(0, 0, 2, 1) { Ok(true)=>true, _=> false, },
                "Connection(0, 0) to (2, 1) was not made.");
        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.");
        assert!(match test_graph.connect_endpoints(0, 1, 2, 0) { Ok(true)=>true, _=> false, },
                "Connection(0, 1) to (2, 0) was not made.");
        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 failures
        assert!(match test_graph.disconnect_endpoint(3, 1) { Err(Error::IndexError(3))=>true, _=>false },
                "Invalid segment accessed.");
        assert!(match test_graph.disconnect_endpoint(1, 2) { Err(Error::SegmentError(rail_segment::Error::EndpointError(2)))=>true, _=>false },
                "Invalid endpoint disconnected.");
        assert!(match test_graph.disconnect_endpoint(0, 0) { Err(Error::SegmentError(rail_segment::Error::ConnectionError(0)))=>true, _=>false },
                "Unconnected endpoint disconnected.");
        
        // test successes
        assert!(match test_graph.disconnect_endpoint(0, 1) { Ok(true)=>true, _=>false },
                "Endpoint failed to disconnect.");
        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.");
    }
}