// Copyright (c) 2019 Autonomous Robots and Cognitive Systems Laboratory
// Author: Daniel Garcia-Vaglio <degv364@gmail.com>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

use nalgebra::{DMatrix};
use crate::segment::{Segment};
use crate::joint::{JointType};

/// Implementation of kinematic chains
///
/// segments: a vector with the segments of the chain\
/// num_joints: number of joints (segments with movable joints)\
/// num_ind_joints: Number of joints with independent movement\
/// num_unlocked_joints: Num of enabled/unlocked joints\
/// locked_joints: vector with bools indicating if the joint is locked\
/// coupling_matrix: matrix correlating the joints with their couples\
#[derive(Clone)]
pub struct Chain {
    segments: Vec<Segment>,
    num_joints: usize,
    num_ind_joints: usize,
    num_unlocked_joints: usize,
    locked_joints: Vec<bool>,
    coupling_matrix: DMatrix<f64>,
}

impl Chain {
    /// Default empty chain
    pub fn default() -> Chain {
        Chain {
            segments: Vec::new(),
            num_joints: 0,
            num_ind_joints: 0,
            num_unlocked_joints: 0,
            locked_joints: vec![],
            coupling_matrix: DMatrix::identity(0,0),
        }
    }

    /// Returns the total number of segments
    pub fn get_num_segments(&self) -> usize {
        self.segments.len()
    }

    /// Returns the number of segments with moveable joints
    pub fn get_num_joints(&self) -> usize {
        self.num_joints
    }

    /// Returns the number of Independent moveable joints
    pub fn get_num_ind_joints(&self) -> usize {
        self.num_ind_joints
    }

    /// Returns the coupling matrix
    pub fn get_coupling_matrix(&self) -> DMatrix<f64> {
        self.coupling_matrix.clone()
    }

    /// Add a new semgment at the end of the kinematic chain\
    /// (Where the end is where the end-effector would be)
    pub fn add_segment(&mut self, segment: Segment) {
        self.segments.push(segment);
        if segment.get_joint_type() != JointType::NoJoint {
            self.num_joints += 1;
            self.locked_joints.push(false);
            self.num_unlocked_joints += 1;
            self.num_ind_joints = self.num_unlocked_joints;
            self.coupling_matrix = DMatrix::identity(
                self.num_unlocked_joints, self.num_ind_joints);
        }
    }

    /// Append an entire chain at the end of this chain
    pub fn add_chain(&mut self, mut chain: Chain) {
        self.segments.append(&mut chain.segments);
        self.num_joints += chain.num_joints;
    }

    /// Get the segment at the index-th position
    pub fn get_segment(&self, index: usize) -> Segment {
        self.segments[index]
    }

    /// Get the vector with lock information
    pub fn get_locked_joints(&self) -> Vec<bool> {
        self.locked_joints.clone()
    }

    /// Set coupling information, locked joints and coupling relations
    pub fn set_coupling(&mut self, new_locked_joints: Vec<bool>,
                        new_coupling_matrix: DMatrix<f64>){
        let new_num_unlocked_joints = new_locked_joints
            .iter().filter(|&n| !(*n)).count();
        if new_coupling_matrix.nrows() != new_num_unlocked_joints{
            panic!("Wrong size of coupling arguments. Matrix has {} rows,
                   but the vector says there should be {}",
                   new_coupling_matrix.nrows(),
                   new_num_unlocked_joints);
        }
        self.locked_joints = new_locked_joints;
        self.num_unlocked_joints = new_num_unlocked_joints;
        self.num_ind_joints = new_coupling_matrix.ncols();
        self.coupling_matrix = new_coupling_matrix;
    }
}


#[cfg(test)]
pub mod tests{
    use crate::geometry::{Frame, EulerBuild};
    use crate::joint::{Joint, JointType};
    use crate::segment::{Segment};
    use crate::chains::{Chain};
    
    pub fn create_testing_chain() -> Chain {
        let mut chain = Chain::default();
        // Base segment
        chain.add_segment(Segment::new(
            Joint::default().set_type(JointType::NoJoint),
            Frame::from_translation_euler(
                0.1, -0.2, 0.3, -0.1, -0.2, 0.3),
            0.0
        ));
        // Segment 0
        chain.add_segment(Segment::new(
            Joint::default().set_type(JointType::RotZ),
            Frame::from_translation_euler(
                0.2, 0.5, 0.1, 0.58, -1.2, 2.3),
            0.0
        ));
        // Segment 1
        chain.add_segment(Segment::new(
            Joint::default().set_type(JointType::RotZ),
            Frame::from_translation_euler(
                0.02, 0.3, -0.81, 3.0, -1.7, 1.3),
            0.0
        ));
        // Segment 2
        chain.add_segment(Segment::new(
            Joint::default().set_type(JointType::RotZ),
            Frame::from_translation_euler(
                0.4, -0.27, 0.19, 1.57, -1.57, 0.0),
            0.0
        ));
        // Segment 3
        chain.add_segment(Segment::new(
            Joint::default().set_type(JointType::RotZ),
            Frame::from_translation_euler(
                0.24, -0.18, 0.16, 0.01, 0.0, -0.01),
            0.0
        ));
        // Segment 4
        chain.add_segment(Segment::new(
            Joint::default().set_type(JointType::RotZ),
            Frame::from_translation_euler(
                1.0, -0.5, -0.01, 0.05, -1.02, 0.34),
            0.0
        ));
        // Segment 5
        chain.add_segment(Segment::new(
            Joint::default().set_type(JointType::RotZ),
            Frame::from_translation_euler(
                0.02, 0.58, -0.87, 0.95, -1.74, 0.68),
            0.0
        ));
        // Segment 6
        chain.add_segment(Segment::new(
            Joint::default().set_type(JointType::RotZ),
            Frame::from_translation_euler(
                0.2, 0.1, -0.1, -0.76, -0.94, 0.25),
            0.0
        ));
        chain
    }
}