use lazy_static::lazy_static;
use nalgebra::{Const, DMatrix, DVector, DVectorView, Dyn, SymmetricEigen};
use std::fmt::{Display, Error, Formatter};

// --- elements ---

pub fn point(x: f64, y: f64, z: f64) -> DVector<f64> {
    DVector::from_column_slice(&[x, y, z, 0.5, 0.5*(x*x + y*y + z*z)])
}

// the sphere with the given center and radius, with inward-pointing normals
pub fn sphere(center_x: f64, center_y: f64, center_z: f64, radius: f64) -> DVector<f64> {
    let center_norm_sq = center_x * center_x + center_y * center_y + center_z * center_z;
    DVector::from_column_slice(&[
        center_x / radius,
        center_y / radius,
        center_z / radius,
        0.5 / radius,
        0.5 * (center_norm_sq / radius - radius),
    ])
}

// the sphere of curvature `curv` whose closest point to the origin has position
// `off * dir` and normal `dir`, where `dir` is a unit vector. setting the
// curvature to zero gives a plane
pub fn sphere_with_offset(dir_x: f64, dir_y: f64, dir_z: f64, off: f64, curv: f64) -> DVector<f64> {
    let norm_sp = 1.0 + off * curv;
    DVector::from_column_slice(&[
        norm_sp * dir_x,
        norm_sp * dir_y,
        norm_sp * dir_z,
        0.5 * curv,
        off * (1.0 + 0.5 * off * curv),
    ])
}

// project a sphere's representation vector to the normalization variety by
// contracting toward the last coordinate axis
pub fn project_sphere_to_normalized(rep: &mut DVector<f64>) {
    let q_sp = rep.fixed_rows::<3>(0).norm_squared();
    let half_q_lt = -2.0 * rep[3] * rep[4];
    let half_q_lt_sq = half_q_lt * half_q_lt;
    let scaling = half_q_lt + (q_sp + half_q_lt_sq).sqrt();
    rep.fixed_rows_mut::<4>(0).scale_mut(1.0 / scaling);
}

// normalize a point's representation vector by scaling
pub fn project_point_to_normalized(rep: &mut DVector<f64>) {
    rep.scale_mut(0.5 / rep[3]);
}

// --- partial matrices ---

pub struct MatrixEntry {
    index: (usize, usize),
    value: f64,
}

pub struct PartialMatrix(Vec<MatrixEntry>);

impl PartialMatrix {
    pub fn new() -> PartialMatrix {
        PartialMatrix(Vec::<MatrixEntry>::new())
    }
    
    pub fn push(&mut self, row: usize, col: usize, value: f64) {
        let PartialMatrix(entries) = self;
        entries.push(MatrixEntry { index: (row, col), value });
    }
    
    pub fn push_sym(&mut self, row: usize, col: usize, value: f64) {
        self.push(row, col, value);
        if row != col {
            self.push(col, row, value);
        }
    }
    
    fn freeze(&self, a: &DMatrix<f64>) -> DMatrix<f64> {
        let mut result = a.clone();
        for &MatrixEntry { index, value } in self {
            result[index] = value;
        }
        result
    }
    
    fn proj(&self, a: &DMatrix<f64>) -> DMatrix<f64> {
        let mut result = DMatrix::<f64>::zeros(a.nrows(), a.ncols());
        for &MatrixEntry { index, .. } in self {
            result[index] = a[index];
        }
        result
    }
    
    fn sub_proj(&self, rhs: &DMatrix<f64>) -> DMatrix<f64> {
        let mut result = DMatrix::<f64>::zeros(rhs.nrows(), rhs.ncols());
        for &MatrixEntry { index, value } in self {
            result[index] = value - rhs[index];
        }
        result
    }
}

impl Display for PartialMatrix {
    fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), Error> {
        for &MatrixEntry { index: (row, col), value } in self {
            writeln!(f, "  {row} {col} {value}")?;
        }
        Ok(())
    }
}

impl IntoIterator for PartialMatrix {
    type Item = MatrixEntry;
    type IntoIter = std::vec::IntoIter<Self::Item>;
    
    fn into_iter(self) -> Self::IntoIter {
        let PartialMatrix(entries) = self;
        entries.into_iter()
    }
}

impl<'a> IntoIterator for &'a PartialMatrix {
    type Item = &'a MatrixEntry;
    type IntoIter = std::slice::Iter<'a, MatrixEntry>;
    
    fn into_iter(self) -> Self::IntoIter {
        let PartialMatrix(entries) = self;
        entries.into_iter()
    }
}

// --- configuration subspaces ---

#[derive(Clone)]
pub struct ConfigSubspace {
    assembly_dim: usize,
    basis_std: Vec<DMatrix<f64>>,
    basis_proj: Vec<DMatrix<f64>>,
}

impl ConfigSubspace {
    pub fn zero(assembly_dim: usize) -> ConfigSubspace {
        ConfigSubspace {
            assembly_dim,
            basis_proj: Vec::new(),
            basis_std: Vec::new(),
        }
    }
    
    // approximate the kernel of a symmetric endomorphism of the configuration
    // space for `assembly_dim` elements. we consider an eigenvector to be part
    // of the kernel if its eigenvalue is smaller than the constant `THRESHOLD`
    fn symmetric_kernel(
        a: DMatrix<f64>,
        proj_to_std: DMatrix<f64>,
        assembly_dim: usize,
    ) -> ConfigSubspace {
        // find a basis for the kernel. the basis is expressed in the projection
        // coordinates, and it's orthonormal with respect to the projection
        // inner product
        const THRESHOLD: f64 = 0.1;
        let eig = SymmetricEigen::new(proj_to_std.tr_mul(&a) * &proj_to_std);
        let eig_vecs = eig.eigenvectors.column_iter();
        let eig_pairs = eig.eigenvalues.iter().zip(eig_vecs);
        let basis_proj = DMatrix::from_columns(
            eig_pairs.filter_map(
                |(λ, v)| (λ.abs() < THRESHOLD).then_some(v)
            ).collect::<Vec<_>>().as_slice()
        );
        
        // express the basis in the standard coordinates
        let basis_std = proj_to_std * &basis_proj;
        
        const ELEMENT_DIM: usize = 5;
        const UNIFORM_DIM: usize = 4;
        ConfigSubspace {
            assembly_dim,
            basis_std: basis_std.column_iter().map(
                |v| Into::<DMatrix<f64>>::into(
                    v.reshape_generic(Dyn(ELEMENT_DIM), Dyn(assembly_dim))
                )
            ).collect(),
            basis_proj: basis_proj.column_iter().map(
                |v| Into::<DMatrix<f64>>::into(
                    v.reshape_generic(Dyn(UNIFORM_DIM), Dyn(assembly_dim))
                )
            ).collect(),
        }
    }
    
    pub fn dim(&self) -> usize {
        self.basis_std.len()
    }
    
    pub fn assembly_dim(&self) -> usize {
        self.assembly_dim
    }
    
    // find the projection onto this subspace of the motion where the element
    // with the given column index has velocity `v`. the velocity is given in
    // projection coordinates, and the projection is done with respect to the
    // projection inner product
    pub fn proj(&self, v: &DVectorView<f64>, column_index: usize) -> DMatrix<f64> {
        if self.dim() == 0 {
            const ELEMENT_DIM: usize = 5;
            DMatrix::zeros(ELEMENT_DIM, self.assembly_dim)
        } else {
            self.basis_proj.iter().zip(self.basis_std.iter()).map(
                |(b_proj, b_std)| b_proj.column(column_index).dot(&v) * b_std
            ).sum()
        }
    }
}

// --- descent history ---

pub struct DescentHistory {
    pub config: Vec<DMatrix<f64>>,
    pub scaled_loss: Vec<f64>,
    pub neg_grad: Vec<DMatrix<f64>>,
    pub hess_eigvals: Vec<DVector<f64>>,
    pub base_step: Vec<DMatrix<f64>>,
    pub backoff_steps: Vec<i32>,
}

impl DescentHistory {
    pub fn new() -> DescentHistory {
        DescentHistory {
            config: Vec::<DMatrix<f64>>::new(),
            scaled_loss: Vec::<f64>::new(),
            neg_grad: Vec::<DMatrix<f64>>::new(),
            hess_eigvals: Vec::<DVector<f64>>::new(),
            base_step: Vec::<DMatrix<f64>>::new(),
            backoff_steps: Vec::<i32>::new(),
        }
    }
}

// --- constraint problems ---

pub struct ConstraintProblem {
    pub gram: PartialMatrix,
    pub frozen: PartialMatrix,
    pub guess: DMatrix<f64>,
}

impl ConstraintProblem {
    pub fn new(element_count: usize) -> ConstraintProblem {
        const ELEMENT_DIM: usize = 5;
        ConstraintProblem {
            gram: PartialMatrix::new(),
            frozen: PartialMatrix::new(),
            guess: DMatrix::<f64>::zeros(ELEMENT_DIM, element_count),
        }
    }
    
    #[cfg(feature = "dev")]
    pub fn from_guess(guess_columns: &[DVector<f64>]) -> ConstraintProblem {
        ConstraintProblem {
            gram: PartialMatrix::new(),
            frozen: PartialMatrix::new(),
            guess: DMatrix::from_columns(guess_columns),
        }
    }
}

// --- gram matrix realization ---

// the Lorentz form
lazy_static! {
    pub static ref Q: DMatrix<f64> = DMatrix::from_row_slice(5, 5, &[
        1.0, 0.0, 0.0,  0.0,  0.0,
        0.0, 1.0, 0.0,  0.0,  0.0,
        0.0, 0.0, 1.0,  0.0,  0.0,
        0.0, 0.0, 0.0,  0.0, -2.0,
        0.0, 0.0, 0.0, -2.0,  0.0,
    ]);
}

struct SearchState {
    config: DMatrix<f64>,
    err_proj: DMatrix<f64>,
    loss: f64,
}

impl SearchState {
    fn from_config(gram: &PartialMatrix, config: DMatrix<f64>) -> SearchState {
        let err_proj = gram.sub_proj(&(config.tr_mul(&*Q) * &config));
        let loss = err_proj.norm_squared();
        SearchState { config, err_proj, loss }
    }
}

fn basis_matrix(index: (usize, usize), nrows: usize, ncols: usize) -> DMatrix<f64> {
    let mut result = DMatrix::<f64>::zeros(nrows, ncols);
    result[index] = 1.0;
    result
}

// given a normalized vector `v` representing an element, build a basis for the
// element's linear configuration space consisting of:
// - the unit translation motions of the element
// - the unit shrinking motion of the element, if it's a sphere
// - one or two vectors whose coefficients vanish on the tangent space of the
//   normalization variety
pub fn local_unif_to_std(v: DVectorView<f64>) -> DMatrix<f64> {
    const ELEMENT_DIM: usize = 5;
    const UNIFORM_DIM: usize = 4;
    let curv = 2.0*v[3];
    if v.dot(&(&*Q * v)) < 0.5 {
        // `v` represents a point. the normalization condition says that the
        // curvature component of `v` is 1/2
        DMatrix::from_column_slice(ELEMENT_DIM, UNIFORM_DIM, &[
            curv,  0.0,  0.0,  0.0, v[0],
             0.0, curv,  0.0,  0.0, v[1],
             0.0,  0.0, curv,  0.0, v[2],
             0.0,  0.0,  0.0,  0.0,  1.0,
        ])
    } else {
        // `v` represents a sphere. the normalization condition says that the
        // Lorentz product of `v` with itself is 1
        DMatrix::from_column_slice(ELEMENT_DIM, UNIFORM_DIM, &[
                 curv,       0.0,       0.0,       0.0,            v[0],
                  0.0,      curv,       0.0,       0.0,            v[1],
                  0.0,       0.0,      curv,       0.0,            v[2],
            curv*v[0], curv*v[1], curv*v[2], curv*v[3], curv*v[4] + 1.0,
        ])
    }
}

// use backtracking line search to find a better configuration
fn seek_better_config(
    gram: &PartialMatrix,
    state: &SearchState,
    base_step: &DMatrix<f64>,
    base_target_improvement: f64,
    min_efficiency: f64,
    backoff: f64,
    max_backoff_steps: i32,
) -> Option<(SearchState, i32)> {
    let mut rate = 1.0;
    for backoff_steps in 0..max_backoff_steps {
        let trial_config = &state.config + rate * base_step;
        let trial_state = SearchState::from_config(gram, trial_config);
        let improvement = state.loss - trial_state.loss;
        if improvement >= min_efficiency * rate * base_target_improvement {
            return Some((trial_state, backoff_steps));
        }
        rate *= backoff;
    }
    None
}

// a first-order neighborhood of a configuration
pub struct ConfigNeighborhood {
    pub config: DMatrix<f64>,
    pub nbhd: ConfigSubspace,
}

pub struct Realization {
    pub result: Result<ConfigNeighborhood, String>,
    pub history: DescentHistory,
}

// seek a matrix `config` that matches the partial matrix `problem.frozen` and
// has `config' * Q * config` matching the partial matrix `problem.gram`. start
// at `problem.guess`, set the frozen entries to their desired values, and then
// use a regularized Newton's method to seek the desired Gram matrix
pub fn realize_gram(
    problem: &ConstraintProblem,
    scaled_tol: f64,
    min_efficiency: f64,
    backoff: f64,
    reg_scale: f64,
    max_descent_steps: i32,
    max_backoff_steps: i32,
) -> Realization {
    // destructure the problem data
    let ConstraintProblem { gram, guess, frozen } = problem;
    
    // start the descent history
    let mut history = DescentHistory::new();
    
    // handle the case where the assembly is empty. our general realization
    // routine can't handle this case because it builds the Hessian using
    // `DMatrix::from_columns`, which panics when the list of columns is empty
    let assembly_dim = guess.ncols();
    if assembly_dim == 0 {
        let result = Ok(
            ConfigNeighborhood {
                config: guess.clone(),
                nbhd: ConfigSubspace::zero(0),
            }
        );
        return Realization { result, history };
    }
    
    // find the dimension of the search space
    let element_dim = guess.nrows();
    let total_dim = element_dim * assembly_dim;
    
    // scale the tolerance
    let scale_adjustment = (gram.0.len() as f64).sqrt();
    let tol = scale_adjustment * scaled_tol;
    
    // convert the frozen indices to stacked format
    let frozen_stacked: Vec<usize> = frozen.into_iter().map(
        |MatrixEntry { index: (row, col), .. }| col*element_dim + row
    ).collect();
    
    // use a regularized Newton's method with backtracking
    let mut state = SearchState::from_config(gram, frozen.freeze(guess));
    let mut hess = DMatrix::zeros(element_dim, assembly_dim);
    for _ in 0..max_descent_steps {
        // find the negative gradient of the loss function
        let neg_grad = 4.0 * &*Q * &state.config * &state.err_proj;
        let mut neg_grad_stacked = neg_grad.clone().reshape_generic(Dyn(total_dim), Const::<1>);
        history.neg_grad.push(neg_grad.clone());
        
        // find the negative Hessian of the loss function
        let mut hess_cols = Vec::<DVector<f64>>::with_capacity(total_dim);
        for col in 0..assembly_dim {
            for row in 0..element_dim {
                let index = (row, col);
                let basis_mat = basis_matrix(index, element_dim, assembly_dim);
                let neg_d_err =
                    basis_mat.tr_mul(&*Q) * &state.config
                    + state.config.tr_mul(&*Q) * &basis_mat;
                let neg_d_err_proj = gram.proj(&neg_d_err);
                let deriv_grad = 4.0 * &*Q * (
                    -&basis_mat * &state.err_proj
                    + &state.config * &neg_d_err_proj
                );
                hess_cols.push(deriv_grad.reshape_generic(Dyn(total_dim), Const::<1>));
            }
        }
        hess = DMatrix::from_columns(hess_cols.as_slice());
        
        // regularize the Hessian
        let hess_eigvals = hess.symmetric_eigenvalues();
        let min_eigval = hess_eigvals.min();
        if min_eigval <= 0.0 {
            hess -= reg_scale * min_eigval * DMatrix::identity(total_dim, total_dim);
        }
        history.hess_eigvals.push(hess_eigvals);
        
        // project the negative gradient and negative Hessian onto the
        // orthogonal complement of the frozen subspace
        let zero_col = DVector::zeros(total_dim);
        let zero_row = zero_col.transpose();
        for &k in &frozen_stacked {
            neg_grad_stacked[k] = 0.0;
            hess.set_row(k, &zero_row);
            hess.set_column(k, &zero_col);
            hess[(k, k)] = 1.0;
        }
        
        // stop if the loss is tolerably low
        history.config.push(state.config.clone());
        history.scaled_loss.push(state.loss / scale_adjustment);
        if state.loss < tol { break; }
        
        // compute the Newton step
        /* TO DO */
        /*
          we should change our regularization to ensure that the Hessian is
          is positive-definite, rather than just positive-semidefinite. ideally,
          that would guarantee the success of the Cholesky decomposition---
          although we'd still need the error-handling routine in case of
          numerical hiccups
        */
        let hess_cholesky = match hess.clone().cholesky() {
            Some(cholesky) => cholesky,
            None => return Realization {
                result: Err("Cholesky decomposition failed".to_string()),
                history,
            },
        };
        let base_step_stacked = hess_cholesky.solve(&neg_grad_stacked);
        let base_step = base_step_stacked.reshape_generic(Dyn(element_dim), Dyn(assembly_dim));
        history.base_step.push(base_step.clone());
        
        // use backtracking line search to find a better configuration
        if let Some((better_state, backoff_steps)) = seek_better_config(
            gram, &state, &base_step, neg_grad.dot(&base_step),
            min_efficiency, backoff, max_backoff_steps,
        ) {
            state = better_state;
            history.backoff_steps.push(backoff_steps);
        } else {
            return Realization {
                result: Err("Line search failed".to_string()),
                history,
            };
        }
    }
    let result = if state.loss < tol {
        // express the uniform basis in the standard basis
        const UNIFORM_DIM: usize = 4;
        let total_dim_unif = UNIFORM_DIM * assembly_dim;
        let mut unif_to_std = DMatrix::<f64>::zeros(total_dim, total_dim_unif);
        for n in 0..assembly_dim {
            let block_start = (element_dim * n, UNIFORM_DIM * n);
            unif_to_std
                .view_mut(block_start, (element_dim, UNIFORM_DIM))
                .copy_from(&local_unif_to_std(state.config.column(n)));
        }
        
        // find the kernel of the Hessian. give it the uniform inner product
        let tangent = ConfigSubspace::symmetric_kernel(hess, unif_to_std, assembly_dim);
        
        Ok(ConfigNeighborhood { config: state.config, nbhd: tangent })
    } else {
        Err("Failed to reach target accuracy".to_string())
    };
    Realization { result, history }
}

// --- tests ---

#[cfg(feature = "dev")]
pub mod examples {
    use std::f64::consts::PI;
    
    use super::*;
    
    // this problem is from a sangaku by Irisawa Shintarō Hiroatsu. the article
    // below includes a nice translation of the problem statement, which was
    // recorded in Uchida Itsumi's book _Kokon sankan_ (_Mathematics, Past and
    // Present_)
    //
    //   "Japan's 'Wasan' Mathematical Tradition", by Abe Haruki
    //   https://www.nippon.com/en/japan-topics/c12801/
    //
    pub fn realize_irisawa_hexlet(scaled_tol: f64) -> Realization {
        let mut problem = ConstraintProblem::from_guess(
            [
                sphere(0.0, 0.0, 0.0, 15.0),
                sphere(0.0, 0.0, -9.0, 5.0),
                sphere(0.0, 0.0, 11.0, 3.0),
            ].into_iter().chain(
                (1..=6).map(
                    |k| {
                        let ang = (k as f64) * PI/3.0;
                        sphere(9.0 * ang.cos(), 9.0 * ang.sin(), 0.0, 2.5)
                    }
                )
            ).collect::<Vec<_>>().as_slice()
        );
        
        for s in 0..9 {
            // each sphere is represented by a spacelike vector
            problem.gram.push_sym(s, s, 1.0);
            
            // the circumscribing sphere is tangent to all of the other
            // spheres, with matching orientation
            if s > 0 {
                problem.gram.push_sym(0, s, 1.0);
            }
            
            if s > 2 {
                // each chain sphere is tangent to the "sun" and "moon"
                // spheres, with opposing orientation
                for n in 1..3 {
                    problem.gram.push_sym(s, n, -1.0);
                }
                
                // each chain sphere is tangent to the next chain sphere,
                // with opposing orientation
                let s_next = 3 + (s-2) % 6;
                problem.gram.push_sym(s, s_next, -1.0);
            }
        }
        
        // the frozen entries fix the radii of the circumscribing sphere, the
        // "sun" and "moon" spheres, and one of the chain spheres
        for k in 0..4 {
            problem.frozen.push(3, k, problem.guess[(3, k)]);
        }
        
        realize_gram(&problem, scaled_tol, 0.5, 0.9, 1.1, 200, 110)
    }
    
    // set up a kaleidocycle, made of points with fixed distances between them,
    // and find its tangent space
    pub fn realize_kaleidocycle(scaled_tol: f64) -> Realization {
        const N_HINGES: usize = 6;
        let mut problem = ConstraintProblem::from_guess(
            (0..N_HINGES).step_by(2).flat_map(
                |n| {
                    let ang_hor = (n as f64) * PI/3.0;
                    let ang_vert = ((n + 1) as f64) * PI/3.0;
                    let x_vert = ang_vert.cos();
                    let y_vert = ang_vert.sin();
                    [
                        point(0.0, 0.0, 0.0),
                        point(ang_hor.cos(), ang_hor.sin(), 0.0),
                        point(x_vert, y_vert, -0.5),
                        point(x_vert, y_vert, 0.5),
                    ]
                }
            ).collect::<Vec<_>>().as_slice()
        );
        
        const N_POINTS: usize = 2 * N_HINGES;
        for block in (0..N_POINTS).step_by(2) {
            let block_next = (block + 2) % N_POINTS;
            for j in 0..2 {
                // diagonal and hinge edges
                for k in j..2 {
                    problem.gram.push_sym(block + j, block + k, if j == k { 0.0 } else { -0.5 });
                }
                
                // non-hinge edges
                for k in 0..2 {
                    problem.gram.push_sym(block + j, block_next + k, -0.625);
                }
            }
        }
        
        for k in 0..N_POINTS {
            problem.frozen.push(3, k, problem.guess[(3, k)])
        }
        
        realize_gram(&problem, scaled_tol, 0.5, 0.9, 1.1, 200, 110)
    }
}

#[cfg(test)]
mod tests {
    use nalgebra::Vector3;
    use std::{f64::consts::{FRAC_1_SQRT_2, PI}, iter};
    
    use super::{*, examples::*};
    
    #[test]
    fn freeze_test() {
        let frozen = PartialMatrix(vec![
            MatrixEntry { index: (0, 0), value: 14.0 },
            MatrixEntry { index: (0, 2), value: 28.0 },
            MatrixEntry { index: (1, 1), value: 42.0 },
            MatrixEntry { index: (1, 2), value: 49.0 },
        ]);
        let config = DMatrix::<f64>::from_row_slice(2, 3, &[
            1.0, 2.0, 3.0,
            4.0, 5.0, 6.0,
        ]);
        let expected_result = DMatrix::<f64>::from_row_slice(2, 3, &[
            14.0, 2.0, 28.0,
            4.0, 42.0, 49.0,
        ]);
        assert_eq!(frozen.freeze(&config), expected_result);
    }
    
    #[test]
    fn sub_proj_test() {
        let target = PartialMatrix(vec![
            MatrixEntry { index: (0, 0), value: 19.0 },
            MatrixEntry { index: (0, 2), value: 39.0 },
            MatrixEntry { index: (1, 1), value: 59.0 },
            MatrixEntry { index: (1, 2), value: 69.0 },
        ]);
        let attempt = DMatrix::<f64>::from_row_slice(2, 3, &[
            1.0, 2.0, 3.0,
            4.0, 5.0, 6.0,
        ]);
        let expected_result = DMatrix::<f64>::from_row_slice(2, 3, &[
            18.0, 0.0, 36.0,
            0.0, 54.0, 63.0,
        ]);
        assert_eq!(target.sub_proj(&attempt), expected_result);
    }
    
    #[test]
    fn zero_loss_test() {
        let mut gram = PartialMatrix::new();
        for j in 0..3 {
            for k in 0..3 {
                gram.push(j, k, if j == k { 1.0 } else { -1.0 });
            }
        }
        let config = {
            let a = 0.75_f64.sqrt();
            DMatrix::from_columns(&[
                sphere(1.0, 0.0, 0.0, a),
                sphere(-0.5, a, 0.0, a),
                sphere(-0.5, -a, 0.0, a),
            ])
        };
        let state = SearchState::from_config(&gram, config);
        assert!(state.loss.abs() < f64::EPSILON);
    }
    
    /* TO DO */
    // at the frozen indices, the optimization steps should have exact zeros,
    // and the realized configuration should have the desired values
    #[test]
    fn frozen_entry_test() {
        let mut problem = ConstraintProblem::from_guess(&[
            point(0.0, 0.0, 2.0),
            sphere(0.0, 0.0, 0.0, 0.95),
        ]);
        for j in 0..2 {
            for k in j..2 {
                problem.gram.push_sym(j, k, if (j, k) == (1, 1) { 1.0 } else { 0.0 });
            }
        }
        problem.frozen.push(3, 0, problem.guess[(3, 0)]);
        problem.frozen.push(3, 1, 0.5);
        let Realization { result, history } = realize_gram(
            &problem, 1.0e-12, 0.5, 0.9, 1.1, 200, 110
        );
        let config = result.unwrap().config;
        for base_step in history.base_step.into_iter() {
            for &MatrixEntry { index, .. } in &problem.frozen {
                assert_eq!(base_step[index], 0.0);
            }
        }
        for MatrixEntry { index, value } in problem.frozen {
            assert_eq!(config[index], value);
        }
    }
    
    #[test]
    fn irisawa_hexlet_test() {
        // solve Irisawa's problem
        const SCALED_TOL: f64 = 1.0e-12;
        let config = realize_irisawa_hexlet(SCALED_TOL).result.unwrap().config;
        
        // check against Irisawa's solution
        let entry_tol = SCALED_TOL.sqrt();
        let solution_diams = [30.0, 10.0, 6.0, 5.0, 15.0, 10.0, 3.75, 2.5, 2.0 + 8.0/11.0];
        for (k, diam) in solution_diams.into_iter().enumerate() {
            assert!((config[(3, k)] - 1.0 / diam).abs() < entry_tol);
        }
    }
    
    #[test]
    fn tangent_test_three_spheres() {
        const SCALED_TOL: f64 = 1.0e-12;
        const ELEMENT_DIM: usize = 5;
        let mut problem = ConstraintProblem::from_guess(&[
            sphere(0.0, 0.0, 0.0, -2.0),
            sphere(0.0, 0.0, 1.0, 1.0),
            sphere(0.0, 0.0, -1.0, 1.0),
        ]);
        for j in 0..3 {
            for k in j..3 {
                problem.gram.push_sym(j, k, if j == k { 1.0 } else { -1.0 });
            }
        }
        for n in 0..ELEMENT_DIM {
            problem.frozen.push(n, 0, problem.guess[(n, 0)]);
        }
        let Realization { result, history } = realize_gram(
            &problem, SCALED_TOL, 0.5, 0.9, 1.1, 200, 110
        );
        let ConfigNeighborhood { config, nbhd: tangent } = result.unwrap();
        assert_eq!(config, problem.guess);
        assert_eq!(history.scaled_loss.len(), 1);
        
        // list some motions that should form a basis for the tangent space of
        // the solution variety
        const UNIFORM_DIM: usize = 4;
        let element_dim = problem.guess.nrows();
        let assembly_dim = problem.guess.ncols();
        let tangent_motions_unif = vec![
            basis_matrix((0, 1), UNIFORM_DIM, assembly_dim),
            basis_matrix((1, 1), UNIFORM_DIM, assembly_dim),
            basis_matrix((0, 2), UNIFORM_DIM, assembly_dim),
            basis_matrix((1, 2), UNIFORM_DIM, assembly_dim),
            DMatrix::<f64>::from_column_slice(UNIFORM_DIM, assembly_dim, &[
                0.0, 0.0,  0.0,  0.0,
                0.0, 0.0, -0.5, -0.5,
                0.0, 0.0, -0.5,  0.5,
            ]),
        ];
        let tangent_motions_std = vec![
            basis_matrix((0, 1), element_dim, assembly_dim),
            basis_matrix((1, 1), element_dim, assembly_dim),
            basis_matrix((0, 2), element_dim, assembly_dim),
            basis_matrix((1, 2), element_dim, assembly_dim),
            DMatrix::<f64>::from_column_slice(element_dim, assembly_dim, &[
                0.0, 0.0,  0.0,  0.00,  0.0,
                0.0, 0.0, -1.0, -0.25, -1.0,
                0.0, 0.0, -1.0,  0.25,  1.0,
            ]),
        ];
        
        // confirm that the dimension of the tangent space is no greater than
        // expected
        assert_eq!(tangent.basis_std.len(), tangent_motions_std.len());
        
        // confirm that the tangent space contains all the motions we expect it
        // to. since we've already bounded the dimension of the tangent space,
        // this confirms that the tangent space is what we expect it to be
        let tol_sq = ((element_dim * assembly_dim) as f64) * SCALED_TOL * SCALED_TOL;
        for (motion_unif, motion_std) in tangent_motions_unif.into_iter().zip(tangent_motions_std) {
            let motion_proj: DMatrix<_> = motion_unif.column_iter().enumerate().map(
                |(k, v)| tangent.proj(&v, k)
            ).sum();
            assert!((motion_std - motion_proj).norm_squared() < tol_sq);
        }
    }
    
    fn translation_motion_unif(vel: &Vector3<f64>, assembly_dim: usize) -> Vec<DVector<f64>> {
        let mut elt_motion = DVector::zeros(4);
        elt_motion.fixed_rows_mut::<3>(0).copy_from(vel);
        iter::repeat(elt_motion).take(assembly_dim).collect()
    }
    
    fn rotation_motion_unif(ang_vel: &Vector3<f64>, points: Vec<DVectorView<f64>>) -> Vec<DVector<f64>> {
        points.into_iter().map(
            |pt| {
                let vel = ang_vel.cross(&pt.fixed_rows::<3>(0));
                let mut elt_motion = DVector::zeros(4);
                elt_motion.fixed_rows_mut::<3>(0).copy_from(&vel);
                elt_motion
            }
        ).collect()
    }
    
    #[test]
    fn tangent_test_kaleidocycle() {
        // set up a kaleidocycle and find its tangent space
        const SCALED_TOL: f64 = 1.0e-12;
        let Realization { result, history } = realize_kaleidocycle(SCALED_TOL);
        let ConfigNeighborhood { config, nbhd: tangent } = result.unwrap();
        assert_eq!(history.scaled_loss.len(), 1);
        
        // list some motions that should form a basis for the tangent space of
        // the solution variety
        const N_HINGES: usize = 6;
        let element_dim = config.nrows();
        let assembly_dim = config.ncols();
        let tangent_motions_unif = vec![
            // the translations along the coordinate axes
            translation_motion_unif(&Vector3::new(1.0, 0.0, 0.0), assembly_dim),
            translation_motion_unif(&Vector3::new(0.0, 1.0, 0.0), assembly_dim),
            translation_motion_unif(&Vector3::new(0.0, 0.0, 1.0), assembly_dim),
            
            // the rotations about the coordinate axes
            rotation_motion_unif(&Vector3::new(1.0, 0.0, 0.0), config.column_iter().collect()),
            rotation_motion_unif(&Vector3::new(0.0, 1.0, 0.0), config.column_iter().collect()),
            rotation_motion_unif(&Vector3::new(0.0, 0.0, 1.0), config.column_iter().collect()),
            
            // the twist motion. more precisely: a motion that keeps the center
            // of mass stationary and preserves the distances between the
            // vertices to first order. this has to be the twist as long as:
            //   - twisting is the kaleidocycle's only internal degree of
            //     freedom
            //   - every first-order motion of the kaleidocycle comes from an
            //     actual motion
            (0..N_HINGES).step_by(2).flat_map(
                |n| {
                    let ang_vert = ((n + 1) as f64) * PI/3.0;
                    let vel_vert_x = 4.0 * ang_vert.cos();
                    let vel_vert_y = 4.0 * ang_vert.sin();
                    [
                        DVector::from_column_slice(&[0.0, 0.0, 5.0, 0.0]),
                        DVector::from_column_slice(&[0.0, 0.0, 1.0, 0.0]),
                        DVector::from_column_slice(&[-vel_vert_x, -vel_vert_y, -3.0, 0.0]),
                        DVector::from_column_slice(&[vel_vert_x, vel_vert_y, -3.0, 0.0]),
                    ]
                }
            ).collect::<Vec<_>>(),
        ];
        let tangent_motions_std = tangent_motions_unif.iter().map(
            |motion| DMatrix::from_columns(
                &config.column_iter().zip(motion).map(
                    |(v, elt_motion)| local_unif_to_std(v) * elt_motion
                ).collect::<Vec<_>>()
            )
        ).collect::<Vec<_>>();
        
        // confirm that the dimension of the tangent space is no greater than
        // expected
        assert_eq!(tangent.basis_std.len(), tangent_motions_unif.len());
        
        // confirm that the tangent space contains all the motions we expect it
        // to. since we've already bounded the dimension of the tangent space,
        // this confirms that the tangent space is what we expect it to be
        let tol_sq = ((element_dim * assembly_dim) as f64) * SCALED_TOL * SCALED_TOL;
        for (motion_unif, motion_std) in tangent_motions_unif.into_iter().zip(tangent_motions_std) {
            let motion_proj: DMatrix<_> = motion_unif.into_iter().enumerate().map(
                |(k, v)| tangent.proj(&v.as_view(), k)
            ).sum();
            assert!((motion_std - motion_proj).norm_squared() < tol_sq);
        }
    }
    
    fn translation(dis: Vector3<f64>) -> DMatrix<f64> {
        const ELEMENT_DIM: usize = 5;
        DMatrix::from_column_slice(ELEMENT_DIM, ELEMENT_DIM, &[
                   1.0,        0.0,        0.0, 0.0,             dis[0],
                   0.0,        1.0,        0.0, 0.0,             dis[1],
                   0.0,        0.0,        1.0, 0.0,             dis[2],
            2.0*dis[0], 2.0*dis[1], 2.0*dis[2], 1.0, dis.norm_squared(),
                   0.0,        0.0,        0.0, 0.0,                1.0,
        ])
    }
    
    // confirm that projection onto a configuration subspace is equivariant with
    // respect to Euclidean motions
    #[test]
    fn proj_equivar_test() {
        // find a pair of spheres that meet at 120°
        const SCALED_TOL: f64 = 1.0e-12;
        let mut problem_orig = ConstraintProblem::from_guess(&[
            sphere(0.0, 0.0, 0.5, 1.0),
            sphere(0.0, 0.0, -0.5, 1.0),
        ]);
        problem_orig.gram.push_sym(0, 0, 1.0);
        problem_orig.gram.push_sym(1, 1, 1.0);
        problem_orig.gram.push_sym(0, 1, 0.5);
        let Realization { result: result_orig, history: history_orig } = realize_gram(
            &problem_orig, SCALED_TOL, 0.5, 0.9, 1.1, 200, 110
        );
        let ConfigNeighborhood { config: config_orig, nbhd: tangent_orig } = result_orig.unwrap();
        assert_eq!(config_orig, problem_orig.guess);
        assert_eq!(history_orig.scaled_loss.len(), 1);
        
        // find another pair of spheres that meet at 120°. we'll think of this
        // solution as a transformed version of the original one
        let guess_tfm = {
            let a = 0.5 * FRAC_1_SQRT_2;
            DMatrix::from_columns(&[
                sphere(a, 0.0, 7.0 + a, 1.0),
                sphere(-a, 0.0, 7.0 - a, 1.0),
            ])
        };
        let problem_tfm = ConstraintProblem {
            gram: problem_orig.gram,
            frozen: problem_orig.frozen,
            guess: guess_tfm,
        };
        let Realization { result: result_tfm, history: history_tfm } = realize_gram(
            &problem_tfm, SCALED_TOL, 0.5, 0.9, 1.1, 200, 110
        );
        let ConfigNeighborhood { config: config_tfm, nbhd: tangent_tfm } = result_tfm.unwrap();
        assert_eq!(config_tfm, problem_tfm.guess);
        assert_eq!(history_tfm.scaled_loss.len(), 1);
        
        // project a nudge to the tangent space of the solution variety at the
        // original solution
        let motion_orig = DVector::from_column_slice(&[0.0, 0.0, 1.0, 0.0]);
        let motion_orig_proj = tangent_orig.proj(&motion_orig.as_view(), 0);
        
        // project the equivalent nudge to the tangent space of the solution
        // variety at the transformed solution
        let motion_tfm = DVector::from_column_slice(&[FRAC_1_SQRT_2, 0.0, FRAC_1_SQRT_2, 0.0]);
        let motion_tfm_proj = tangent_tfm.proj(&motion_tfm.as_view(), 0);
        
        // take the transformation that sends the original solution to the
        // transformed solution and apply it to the motion that the original
        // solution makes in response to the nudge
        const ELEMENT_DIM: usize = 5;
        let rot = DMatrix::from_column_slice(ELEMENT_DIM, ELEMENT_DIM, &[
            FRAC_1_SQRT_2, 0.0, -FRAC_1_SQRT_2, 0.0, 0.0,
                      0.0, 1.0,            0.0, 0.0, 0.0,
            FRAC_1_SQRT_2, 0.0,  FRAC_1_SQRT_2, 0.0, 0.0,
                      0.0, 0.0,            0.0, 1.0, 0.0,
                      0.0, 0.0,            0.0, 0.0, 1.0,
        ]);
        let transl = translation(Vector3::new(0.0, 0.0, 7.0));
        let motion_proj_tfm = transl * rot * motion_orig_proj;
        
        // confirm that the projection of the nudge is equivariant. we loosen
        // the comparison tolerance because the transformation seems to
        // introduce some numerical error
        const SCALED_TOL_TFM: f64 = 1.0e-9;
        let tol_sq = ((problem_orig.guess.nrows() * problem_orig.guess.ncols()) as f64) * SCALED_TOL_TFM * SCALED_TOL_TFM;
        assert!((motion_proj_tfm - motion_tfm_proj).norm_squared() < tol_sq);
    }
}