qtruss 0.13.0

//! Describes a 2D truss.

use std::{
    thread::Builder,
    time::Instant,
};

use raqote::*;

use maria_linalg::{
    Matrix,
    Vector,
};

use super::{
    Constraints,
    Element,
    Material,
    PlottableElement,
};

/// Width of canvas (in px) for plotting.
const CANVAS_WIDTH: i32 = 1024;

/// Height of canvas (in px) for plotting.
const CANVAS_HEIGHT: i32 = 768;

/// Maximum element thickness (in px).
const MAX_THICKNESS: f32 = 10.0;

/// Default stack size for optimization.
const DEFAULT_STACK_SIZE: usize = 4 * 1024 * 1024;

/// Finite difference for autodifferentiation.
const DX: f64 = 1E-6;

/// Maximum permitted gradient contribution.
const MAX_GRAD_CONTR: f64 = 1E+6;

#[derive(Clone, Copy, PartialEq, Debug)]
/// Enumerates the types of constraints available on
/// truss nodes.
pub enum Constraint {
    /// A free node
    Free (Vector<2>),
    /// A pinned node
    Pin,
    /// A node free to slide in the X direction, but not in the Y direction.
    HorizontalSlide (Vector<2>),
    /// A node free to slide in the Y direction, but not in the X direction.
    VerticalSlide (Vector<2>),
}

#[derive(Clone, Copy, PartialEq, Debug)]
/// A 2D truss node.
pub struct Node {
    pub location: Vector<2>,
    pub constraint: Constraint,
}

impl Node {
    /// Constructs an empty "zero" node.
    pub fn zero() -> Self {
        Self {
            location: Vector::zero(),
            constraint: Constraint::Free (Vector::zero()),
        }
    }

    /// Constructs a new node.
    pub fn new(
        location: Vector<2>,
        constraint: Constraint,
    ) -> Self {
        Self {
            location,
            constraint,
        }
    }
}

#[derive(Clone, Copy, Debug)]
/// A 2D truss with `N` nodes, `K` elements, and `F` degrees of freedom.
pub struct Truss<const N: usize, const K: usize, const F: usize> {
    nodes: [Node; N],
    pub elements: [Element; K],
    i: usize,
    pub forces: Option<Vector<F>>,
    pub displacements: Option<Vector<F>>,
}

impl<const N: usize, const K: usize, const F: usize> Truss<N, K, F> {
    pub fn new(nodes: [Node; N]) -> Self {
        Self {
            nodes,
            elements: [Element::zero(); K],
            i: 0,
            forces: None,
            displacements: None,
        }
    }

    /// Add an element to this truss and returns the index of this element.
    /// 
    /// Element indices begin at `0` and end at `K - 1`.
    pub fn add(&mut self, one: usize, two: usize) -> Option<usize> {
        // Quit if the node numbers are invalid
        if one >= N {
            println!("Invalid node number: {}", one);
            return None;
        }
        if two >= N {
            println!("Invalid node number: {}", two);
            return None;
        }

        let node_one = self.nodes[one];
        let node_two = self.nodes[two];
        self.elements[self.i] = Element::new(
            one,
            node_one.location,
            two,
            node_two.location,
        );

        let output = self.i;
        self.i += 1;

        Some (output)
    }

    /// Constructs a new sparsely connected truss, given dimensions, node counts, and constraints.
    pub fn sparse(width: f64, height: f64, x_nodes: usize, y_nodes: usize, constraints: [Constraint; N]) -> Option<Self> {
        if x_nodes * y_nodes != N {
            println!(
                "Inconsistent node count: cannot construct {}x{} mesh with {} nodes",
                x_nodes,
                y_nodes,
                N
            );
            return None;
        }

        let mut nodes = [Node::zero(); N];

        let x_spacing = width / (x_nodes - 1) as f64;
        let y_spacing = height / (y_nodes - 1) as f64;

        for j in 0..y_nodes {
            for i in 0..x_nodes {
                let x = i as f64 * x_spacing;
                let y = j as f64 * y_spacing;

                nodes[j*x_nodes + i] = Node::new(
                    [x, y].into(),
                    constraints[j*x_nodes + i],
                );
            }
        }

        let mut truss = Self::new(nodes);

        // Construct sparse mesh
        // 
        // Each node should create up to four new elements
        // Top left, top, top right, right
        for n in 0..N {
            let i = n%x_nodes;
            let j = (n - i)/x_nodes;

            // Top left
            if i > 0 && j < y_nodes - 1 {
                truss.add(n, n + x_nodes - 1);
            }

            // Top
            if j < y_nodes - 1 {
                truss.add(n, n + x_nodes);
            }

            // Top right
            if j < y_nodes - 1 && i < x_nodes - 1 {
                truss.add(n, n + x_nodes + 1);
            }

            // Right
            if i < x_nodes - 1 {
                truss.add(n, n + 1);
            }
        }

        Some (truss)
    }

    /// Constructs a new densely connected truss, given dimensions, node counts, and constraints.
    pub fn dense(width: f64, height: f64, x_nodes: usize, y_nodes: usize, constraints: [Constraint; N]) -> Option<Self> {
        if x_nodes * y_nodes != N {
            println!(
                "Inconsistent node count: cannot construct {}x{} mesh with {} nodes",
                x_nodes,
                y_nodes,
                N
            );
            return None;
        }

        let mut nodes = [Node::zero(); N];

        let x_spacing = width / (x_nodes - 1) as f64;
        let y_spacing = height / (y_nodes - 1) as f64;

        for j in 0..y_nodes {
            for i in 0..x_nodes {
                let x = i as f64 * x_spacing;
                let y = j as f64 * y_spacing;

                nodes[j*x_nodes + i] = Node::new(
                    [x, y].into(),
                    constraints[j*x_nodes + i],
                );
            }
        }

        let mut truss = Self::new(nodes);

        // Construct dense mesh
        for i in 0..N {
            for j in (i + 1)..N {
                truss.add(i, j);
            }
        }

        Some (truss)
    }

    /// Assemble the global stiffness matrix for this truss.
    /// 
    /// The global stiffness matrix has dimensions `(F, F)`.
    pub fn global_stiffness_matrix(&self, areas: [f64; K], materials: [Material; K]) -> Option<Matrix<F>> {
        // If we don't have enough elements, quit
        if self.i < K {
            return None;
        }

        // Create a new `F` by `F` matrix
        let mut matrix = Matrix::zero();

        // Assemble a list of free degrees (up to two per node)
        // `degrees[i]` represents the global indices (if they exist) at node `i`
        let mut degrees: [(Option<usize>, Option<usize>); N] = [(None, None); N];
        let mut f = 0;
        for (n, node) in self.nodes.iter().enumerate() {
            // Create an X degree of freedom, if necessary
            if matches!(node.constraint, Constraint::Free (_))
                || matches!(node.constraint, Constraint::HorizontalSlide (_))
            {
                degrees[n].0 = Some (f);
                f += 1;
            }

            // Create a Y degree of freedom, if necessary
            if matches!(node.constraint, Constraint::Free (_))
                || matches!(node.constraint, Constraint::VerticalSlide (_))
            {
                degrees[n].1 = Some (f);
                f += 1;
            }
        }

        for k in 0..K {
            let element = self.elements[k];
            let area = areas[k];
            let e = materials[k].e;

            let attached: [Option<usize>; 4] = [
                degrees[element.one].0,
                degrees[element.one].1,
                degrees[element.two].0,
                degrees[element.two].1,
            ];

            for (i, a0) in attached.iter().enumerate() {
                for (j, a1) in attached.iter().enumerate() {
                    if let Some (arow) = a0 {
                        if let Some (acol) = a1 {
                            matrix[(*arow, *acol)] += area * e * element.stiffness[(i, j)];
                        }
                    }
                }
            }
        }

        Some (matrix)
    }
    
    /// Solves this truss.
    pub fn solve(&self, areas: [f64; K], materials: [Material; K]) -> Option<(Vector<F>, Vector<F>)> {
        // Get forces applied
        let mut f = Vector::zero();
        let mut i = 0;

        for node in self.nodes {
            match node.constraint {
                Constraint::Pin => (),
                Constraint::Free (applied) => {
                    f[i] = applied[0];
                    f[i + 1] = applied[1];
                    i += 2;
                }
                Constraint::HorizontalSlide (applied) => {
                    f[i] = applied[0];
                    i += 1;
                }
                Constraint::VerticalSlide (applied) => {
                    f[i] = applied[0];
                    i += 1;
                }
            }
        }

        // Get global stiffness matrix
        let k = match self.global_stiffness_matrix(areas, materials) {
            Some (mx) => mx,
            None => {
                println!("Global stiffness matrix is singular.");
                return None;
            },
        };

        // Compute displacements
        let displacements = k.inverse().mult(f);

        Some ((f, displacements))
    }

    /// Gets the displacements of a provided node, given its index.
    pub fn displacement(&self, areas: [f64; K], materials: [Material; K], node: usize) -> Option<Vector<2>> {
        // Get global displacements
        let displacements = match self.solve(areas, materials) {
            Some ((_, d)) => d,
            None => {
                println!("Could not compute displacement of node `{}`.", node);
                return None;
            },
        };

        self.node_displacement(displacements, node)
    }

    /// Gets the displacements of a provided node, given its index.
    /// 
    /// *Note*, this requires the global displacement vector.
    fn node_displacement(&self, displacements: Vector<F>, node: usize) -> Option<Vector<2>> {
        // Start a counter
        let mut i = 0;

        // Iterate through the nodes, up to the current node index
        for k in 0..node {
            // Get the current node
            let n = self.nodes[k];

            i += match n.constraint {
                Constraint::Pin => 0,
                Constraint::Free (_) => 2,
                Constraint::HorizontalSlide (_) => 1,
                Constraint::VerticalSlide (_) => 1,
            };
        }

        let mut output: Vector<2> = Vector::zero();

        match self.nodes[node].constraint {
            // A pin doesn't move
            Constraint::Pin => return Some (output),
            
            // A free node can move in either dimension
            Constraint::Free (_) => {
                output[0] = displacements[i];
                output[1] = displacements[i + 1];
            },

            // A slider can move in only one dimension
            Constraint::HorizontalSlide (_) => output[0] = displacements[i],
            Constraint::VerticalSlide (_) => output[1] = displacements[i],
        }

        Some (output)
    }

    /// Computes the internal force in a provided element, given its index.
    pub fn internal_force(&self, areas: [f64; K], materials: [Material; K], elt: usize) -> Option<f64> {
        // Get this element
        let element = self.elements[elt];

        // Get this element's displacements
        let mut u: Vector<4> = Vector::zero();

        let left_constraint = self.nodes[element.one].constraint;
        let right_constraint = self.nodes[element.two].constraint;

        let left_displacement = match self.displacement(areas, materials, element.one) {
            Some (d) => d,
            None => {
                println!("Could not compute internal force of element `{}`.", elt);
                return None;
            },
        };
        match left_constraint {
            Constraint::Pin => {
                u[0] = 0.0;
                u[1] = 0.0;
            },
            Constraint::Free (_) => {
                u[0] = left_displacement[0];
                u[1] = left_displacement[1];
            },
            Constraint::HorizontalSlide (_) => {
                u[0] = left_displacement[0];
                u[1] = 0.0;
            },
            Constraint::VerticalSlide (_) => {
                u[0] = 0.0;
                u[1] = left_displacement[1];
            },
        }

        let right_displacement = match self.displacement(areas, materials, element.two) {
            Some (d) => d,
            None => {
                println!("Could not compute internal force of element `{}`.", elt);
                return None;
            },
        };
        match right_constraint {
            Constraint::Pin => {
                u[2] = 0.0;
                u[3] = 0.0;
            },
            Constraint::Free (_) => {
                u[2] = right_displacement[0];
                u[3] = right_displacement[1];
            },
            Constraint::HorizontalSlide (_) => {
                u[2] = right_displacement[0];
                u[3] = 0.0;
            },
            Constraint::VerticalSlide (_) => {
                u[2] = 0.0;
                u[3] = right_displacement[1];
            },
        }
        
        // Get this element's stiffness matrix
        let k = element.stiffness;

        // Get the reaction forces on this element
        let force = k.mult(u);

        // Get the internal force in this element
        // 
        // Note: for reasons of convention, we always take the last two
        // values of the reaction forces vector
        let internal = Vector::new([
            force[2],
            force[3],
        ]);

        // Get the direction of this element
        let direction = element.direction;

        // Return the internal force of this element
        Some (internal.dot(direction))
    }

    /// Computes the internal stress in a provided element.
    pub fn stress(&self, areas: [f64; K], materials: [Material; K], elt: usize) -> Option<f64> {
        let force = match self.internal_force(areas, materials, elt) {
            Some (d) => d,
            None => {
                println!("Could not compute internal stress of element `{}`.", elt);
                return None;
            },
        };
        let area = areas[elt];
        Some (force / area)
    }

    /// Computes the compliance of this truss.
    pub fn compliance(&self, areas: [f64; K], materials: [Material; K]) -> Option<f64> {
        let (forces, displacements) = match self.solve(areas, materials) {
            Some ((f, d)) => (f, d),
            None => return None,
        };

        Some (forces.dot(displacements))
    }

    /// Checks if truss materials can hold stress applied.
    pub fn check(&self, areas: [f64; K], materials: [Material; K]) -> bool {
        // Check internal stress in each element
        for k in 0..K {
            let material = materials[k];

            let stress = match self.stress(areas, materials, k) {
                Some (s) => s,
                None => return false,
            };

            // Does the element fail?
            if let Some (s) = material.minstress {
                if stress < s {
                    return false;
                }
            } else if let Some (s) = material.maxstress {
                if stress > s {
                    return false;
                }
            }
        }

        true
    }

    /// Computes fabrication complexity.
    pub fn complexity(&self, areas: [f64; K], maxarea: f64, beta: f64) -> f64 {
        let mut complexity = 0.0;

        for k in 0..K {
            let area = areas[k];

            if area < maxarea {
                complexity += 1.0 - (-beta * area).exp() + area * (-beta).exp();
            } else {
                complexity += 1.0;
            }
        }

        complexity
    }

    /// Computes the total volume of material contained in this truss.
    pub fn volume(&self, areas: [f64; K]) -> f64 {
        let mut volume = 0.0;

        for k in 0..K {
            let element = self.elements[k];
            let area = areas[k];
            volume += area * element.length;
        }

        volume
    }

    /// Computes the total mass of material contained in this truss.
    pub fn mass(&self, areas: [f64; K], materials: [Material; K]) -> f64 {
        let mut mass = 0.0;

        for k in 0..K {
            mass += areas[k] * self.elements[k].length * materials[k].density;
        }

        mass
    }

    /// Computes the *magnitude* of the maximum internal stress and the maximum area in this truss.
    /// 
    /// *Note*: the maximum stress does not necessarily correspond to the maximum area.  This function
    /// enables truss plotting.
    fn maximum(&self, areas: [f64; K], materials: [Material; K], min_area: f64) -> (f64, f64) {
        let mut max_stress: f64 = 0.0;
        let mut max_area: f64 = 0.0;

        for k in 0..K {
            if let Some (s) = self.stress(areas, materials, k) {
                if s.abs() > max_stress.abs() && areas[k] > min_area {
                    max_stress = s;
                }
            }

            if areas[k] > max_area {
                max_area = areas[k];
            }
        }

        (max_stress.abs(), max_area)
    }

    /// Returns the top-left corner and dimensions of the rectangular bounding box around the truss.
    fn bounding_box(&self) -> (f64, f64, f64, f64) {
        let mut topleft = Vector::zero();
        let mut bottomright = Vector::zero();

        let score = |vec: Vector<2>| {
            -vec[0] + vec[1]
        };

        for k in 0..K {
            let one = self.nodes[self.elements[k].one].location;
            let two = self.nodes[self.elements[k].two].location;

            if score(one) > score(topleft) {
                topleft = one;
            } else if score(two) > score(topleft) {
                topleft = two;
            }

            if score(one) < score(bottomright) {
                bottomright = one;
            } else if score(two) < score(bottomright) {
                bottomright = two;
            }
        }

        (
            topleft[1],
            topleft[0],
            bottomright[0] - topleft[0],
            topleft[1] - bottomright[1],
        )
    }

    /// Returns a list of plottable elements.
    fn make_plottable_elements(&self, areas: [f64; K], materials: [Material; K], min_plot_area: f64) -> [PlottableElement; K] {
        let mut plottable = [PlottableElement::zero(); K];

        let (top, left, width, height) = self.bounding_box();
        let (maxstress, maxarea) = self.maximum(areas, materials, min_plot_area);

        for k in 0..K {
            let stress = self.stress(areas, materials, k).unwrap_or(0.0);
            let area = if areas[k] > min_plot_area {
                areas[k]
            } else {
                0.0
            };

            plottable[k] = self.elements[k].to_plottable(
                top,
                left,
                width,
                height,
                stress,
                maxstress,
                area,
                maxarea,
            );
        }

        plottable
    }

    /// Returns a list of visualizable elements.
    fn make_visualizable_elements(&self) -> [PlottableElement; K] {
        let mut plottable = [PlottableElement::zero(); K];

        let (top, left, width, height) = self.bounding_box();

        for k in 0..K {
            plottable[k] = self.elements[k].to_plottable(
                top,
                left,
                width,
                height,
                1.0,
                1.0,
                1.0,
                1.0,
            );
        }

        plottable
    }

    /// Visualizes this truss's topology.
    /// 
    /// Note: this only displays a truss's topology.
    /// This function does not attempt to solve the truss, nor does it color elements by stress.
    pub fn visualize(&self, filename: &str) -> Option<()> {
        let elements = self.make_visualizable_elements();
    
        // Construct canvas
        let mut dt = DrawTarget::new(CANVAS_WIDTH, CANVAS_HEIGHT);

        // Construct white background
        dt.fill_rect(
            0.0,
            0.0,
            CANVAS_WIDTH as f32,
            CANVAS_HEIGHT as f32,
            &Source::Solid(SolidSource {
                r: 0xff,
                g: 0xff,
                b: 0xff,
                a: 0xff,
            }),
            &DrawOptions::new(),
        );

        for element in elements {
            let mut pb = PathBuilder::new();
            pb.move_to(CANVAS_WIDTH as f32 * element.x1, CANVAS_HEIGHT as f32 * element.y1);
            pb.line_to(CANVAS_WIDTH as f32 * element.x2, CANVAS_HEIGHT as f32 * element.y2);
            let path = pb.finish();
    
            dt.stroke(
                &path,
                &Source::Solid(SolidSource {
                    r: 0x00,
                    g: 0x00,
                    b: 0x00,
                    a: 0xff,
                }),
                &StrokeStyle {
                    cap: LineCap::Square,
                    join: LineJoin::Round,
                    width: element.thickness * MAX_THICKNESS,
                    miter_limit: 0.,
                    dash_array: Vec::new(),
                    dash_offset: 0.,
                },
                &DrawOptions::new(),
            );
        }
    
        dt.write_png(filename).ok()
    }

    /// Plots this truss.
    pub fn plot(&self, areas: [f64; K], materials: [Material; K], min_plot_area: f64, filename: &str) -> Option<()> {
        let elements = self.make_plottable_elements(areas, materials, min_plot_area);
    
        // Construct canvas
        let mut dt = DrawTarget::new(CANVAS_WIDTH, CANVAS_HEIGHT);

        // Construct white background
        dt.fill_rect(
            0.0,
            0.0,
            CANVAS_WIDTH as f32,
            CANVAS_HEIGHT as f32,
            &Source::Solid(SolidSource {
                r: 0xff,
                g: 0xff,
                b: 0xff,
                a: 0xff,
            }),
            &DrawOptions::new(),
        );

        for element in elements {
            let mut pb = PathBuilder::new();
            pb.move_to(CANVAS_WIDTH as f32 * element.x1, CANVAS_HEIGHT as f32 * element.y1);
            pb.line_to(CANVAS_WIDTH as f32 * element.x2, CANVAS_HEIGHT as f32 * element.y2);
            let path = pb.finish();

            let (r, g, b, a) = element.rgba();
    
            dt.stroke(
                &path,
                &Source::Solid(SolidSource {
                    r,
                    g,
                    b,
                    a,
                }),
                &StrokeStyle {
                    cap: LineCap::Square,
                    join: LineJoin::Round,
                    width: element.thickness * MAX_THICKNESS,
                    miter_limit: 0.,
                    dash_array: Vec::new(),
                    dash_offset: 0.,
                },
                &DrawOptions::new(),
            );
        }
    
        dt.write_png(filename).ok()
    }

    /// Numerically computes the Hessian.
    #[allow(dead_code)]
    #[deprecated]
    fn hessian(
        &self,
        areas: Vector<K>,
        materials: [Material; K],
        min_area: f64,
        max_mass: f64,
    ) -> Matrix<K> {
        let mut hessian = Matrix::zero();

        for i in 0..K {
            let vector = areas;
            let mut high = vector;
            let mut low = vector;

            high[i] += DX / 2.0;
            low[i] -= DX / 2.0;

            let f_high = self.gradient(high, materials, min_area, max_mass);
            let f_low = self.gradient(low, materials, min_area, max_mass);

            let row = (f_high - f_low).scale(1.0 / DX);

            for j in 0..K {
                hessian[(i, j)] = row[j];
            }
        }

        hessian
    }

    /// Analytically computes the gradient, accounting for barrier functions.
    fn gradient(
        &self,
        areas: Vector<K>,
        materials: [Material; K],
        min_area: f64,
        max_mass: f64,
    ) -> Vector<K> {
        let mut gradient = Vector::zero();

        let mu = 1E-4;

        let (_forces, displacements) = self.solve(areas.into(), materials).unwrap();

        for elt in 0..K {
            // Find objective gradient
            let e = materials[elt].e;
            let element = self.elements[elt];
            let k = element.stiffness.scale(e);
            let u1 = self.node_displacement(displacements, element.one).unwrap();
            let u2 = self.node_displacement(displacements, element.two).unwrap();
            let u: Vector<4> = [
                u1[0],
                u1[1],
                u2[0],
                u2[1],
            ].into();

            gradient[elt] += u.scale(-1.0).dot(k.mult(u));

            // Find area barrier gradient
            let area = areas[elt];
            gradient[elt] += - (mu / (area - min_area)).min(MAX_GRAD_CONTR);

            // Find mass barrier gradient
            let mass = self.mass(areas.into(), materials);
            let density = materials[elt].density;
            let length = self.elements[elt].length;
            gradient[elt] += (mu * density * length / (max_mass - mass)).min(MAX_GRAD_CONTR);
        }

        gradient
    }

    /// Updates element areas according to a line-search algorithm, using
    ///     the theoretical best step size as the initial guess.
    fn update(
        &self,
        areas: Vector<K>,
        materials: [Material; K],
        step: Vector<K>,
        stepsize: f64,
        min_area: f64,
        max_mass: f64,
    ) -> Vector<K> {
        let mut s = stepsize;
        let mut output = areas + step.scale(s);
        let mut mass = self.mass(output.into(), materials);

        let objective = self.compliance(
            areas.into(),
            materials,
        ).unwrap_or(f64::MAX);
        let mut new_objective = self.compliance(
            output.into(),
            materials,
        ).unwrap_or(f64::MAX);

        while !output.check(
            [Some (min_area); K],
            [None; K],
        ) || mass > max_mass
          || new_objective > objective {
            s *= 0.5;
            output = areas + step.scale(s);
            mass = self.mass(output.into(), materials);
            new_objective = self.compliance(
                output.into(),
                materials,
            ).unwrap_or(f64::MAX);
        }

        output
    }

    /// Determines the minimum-compliance areas and materials for this truss.
    /// 
    /// *Note*: this function accepts a stack size (in bytes) in order to 
    ///     avoid stack overflows for large problems.  If no stack size is
    ///     provided, it defaults to 4 MB.
    pub fn optimize(
        &self,
        materials: [Material; K],
        constraints: Constraints,
        stacksize: Option<usize>,
    ) -> [f64; K] {
        let s = stacksize.unwrap_or(DEFAULT_STACK_SIZE);

        let builder = Builder::new()
            .stack_size(s);
        let truss = *self;
        let handler = builder.spawn(move || {
            truss.optimize_truss(materials, constraints)
        }).unwrap();

        handler.join().unwrap()
    }

    /// Determines the minimum-compliance areas and materials for this truss.
    fn optimize_truss(
        &self,
        materials: [Material; K],
        constraints: Constraints,
    ) -> [f64; K] {
        // Start a timer
        let start = Instant::now();

        let mut areas: Vector<K> = [2.0 * constraints.min_area; K].into();

        let mut i = 0;

        let mut step = self.gradient(
            areas,
            materials,
            constraints.min_area,
            constraints.max_mass,
        ).scale(-1.0);

        while i < constraints.max_iter && step.norm() > constraints.criterion {
            let iterstart = Instant::now();

            areas = self.update(
                areas,
                materials,
                step,
                constraints.stepsize,
                constraints.min_area,
                constraints.max_mass,
            );
            step = self.gradient(
                areas,
                materials,
                constraints.min_area,
                constraints.max_mass,
            ).scale(-1.0);

            let objective = self.compliance(
                areas.into(),
                materials,
            ).unwrap_or(f64::NAN);

            i += 1;

            let itertime = iterstart.elapsed().as_micros() as f64 / 1000.0;

            println!("Iteration: {}", i);
            println!("Iteration time: {:.3} ms", itertime);
            println!("Step size: {}", constraints.stepsize);
            println!("Objective: {:.8}", objective);
            println!("Gradient magnitude: {:.8}", step.norm());
            println!("");
        }

        let time = start.elapsed().as_micros() as f64 / 1000.0;

        if i == constraints.max_iter {
            println!("Maximum iteration limit reached in {:.3} milliseconds", time);
        } else {
            println!("Convergence achieved in {:.3} milliseconds", time);
        }

        let compliance = self.compliance(areas.into(), materials).unwrap_or(f64::NAN);

        println!("Areas\n{}", areas);
        println!("Objective: {:.8}", compliance);

        areas.into()
    }
}

#[test]
/// Visualize a 3x3 densely connected ground structure.
fn visualize_dense() {
    let constraints = [Constraint::Free (Vector::zero()); 25];

    let truss = Truss::<25, 300, 50>::dense(
        100.0,
        100.0,
        5,
        5,
        constraints,
    ).unwrap();

    truss.visualize("dense.png");
}

#[test]
/// Visualize a 3x3 sparsely connected ground structure.
fn visualize_sparse() {
    let constraints = [Constraint::Free (Vector::zero()); 9];

    let truss = Truss::<9, 20, 18>::sparse(
        30.0,
        30.0,
        3,
        3,
        constraints,
    ).unwrap();

    truss.visualize("sparse.png");
}

#[test]
fn optimize_dense() {
    let mut constraints = [Constraint::Free (Vector::zero()); 25];
    constraints[20] = Constraint::Pin;
    constraints[0] = Constraint::VerticalSlide (Vector::zero());
    constraints[14] = Constraint::Free ([0.0, -10.0].into());

    let truss = Truss::<25, 300, 47>::dense(
        100.0,
        100.0,
        5,
        5,
        constraints,
    ).unwrap();

    let materials = [Material::new(30E+3, 1.0, None, None); 300];

    let areas = truss.optimize(
        materials,
        Constraints {
            max_mass: 300.0,
            min_area: 1E-3,
            max_iter: 4_000,
            criterion: 1E-4,
            stepsize: 2E-5,
        },
        Some (32 * 1024 * 1024),
    );
    
    truss.plot(areas.into(), materials, 0.04, "optimized.png");
}