Merge branch 'millepede'

Fixes: https://gitlab.desy.de/thomas.white/crystfel/-/issues/3
Fixes: https://gitlab.desy.de/thomas.white/crystfel/-/issues/29

16 files changed, 2048 insertions, 753 deletions

diff --git a/libcrystfel/meson.build b/libcrystfel/meson.build
index 8897aaae..497abef6 100644
--- a/libcrystfel/meson.build
+++ b/libcrystfel/meson.build
@@ -138,6 +138,7 @@ libcrystfel_sources = ['src/image.c',
                        'src/detgeom.c',
                        'src/fom.c',
                        'src/profile.c',
+                       'src/crystfel-mille.c',
                        'src/image-cbf.c',
                        'src/image-hdf5.c',
                        'src/image-msgpack.c',
@@ -197,7 +198,8 @@ install_headers(['src/reflist.h',
                  'src/datatemplate.h',
                  'src/colscale.h',
                  'src/detgeom.h',
-                 'src/fom.h'],
+                 'src/fom.h',
+                 'src/crystfel-mille.h'],
                 subdir: 'crystfel')
 
 # API documentation (Doxygen)
diff --git a/libcrystfel/src/crystfel-mille.c b/libcrystfel/src/crystfel-mille.c
new file mode 100644
index 00000000..6a80e323
--- /dev/null
+++ b/libcrystfel/src/crystfel-mille.c
@@ -0,0 +1,311 @@
+/*
+ * crystfel-mille.c
+ *
+ * Interface to Millepede geometry refinement
+ *
+ * Copyright © 2023 Deutsches Elektronen-Synchrotron DESY,
+ *                  a research centre of the Helmholtz Association.
+ *
+ * Authors:
+ *   2023 Thomas White <taw@physics.org>
+ *
+ * This file is part of CrystFEL.
+ *
+ * CrystFEL 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.
+ *
+ * CrystFEL 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 CrystFEL.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <libcrystfel-config.h>
+
+#include <stdlib.h>
+#include <assert.h>
+
+#include "image.h"
+#include "geometry.h"
+#include "cell-utils.h"
+#include "predict-refine.h"
+#include "profile.h"
+
+int mille_label(int group_serial, enum gparam param)
+{
+	switch ( param ) {
+		case GPARAM_DET_TX : return group_serial+1;  /* x-shift */
+		case GPARAM_DET_TY : return group_serial+2;  /* y-shift */
+		case GPARAM_DET_TZ : return group_serial+3;  /* z-shift */
+		case GPARAM_DET_RX : return group_serial+4;  /* Rotation around x */
+		case GPARAM_DET_RY : return group_serial+5;  /* Rotation around y */
+		case GPARAM_DET_RZ : return group_serial+6;  /* Rotation around z */
+		default : abort();
+	}
+}
+
+
+/* Opposite of mille_label(), for decoding labels later */
+enum gparam mille_unlabel(int n)
+{
+	switch ( n ) {
+		case 1 : return GPARAM_DET_TX;
+		case 2 : return GPARAM_DET_TY;
+		case 3 : return GPARAM_DET_TZ;
+		case 4 : return GPARAM_DET_RX;
+		case 5 : return GPARAM_DET_RY;
+		case 6 : return GPARAM_DET_RZ;
+		default : abort();
+	}
+}
+
+
+struct mille
+{
+	float *float_arr;
+	int *int_arr;
+	int max_entries;
+	int n;
+	FILE *fh;
+};
+
+typedef struct mille Mille;
+
+
+static void mille_add_measurement(Mille *m,
+                                  int NLC, float *derLc,
+                                  int NGL, float *derGl, int *labels,
+                                  float rMeas, float sigma)
+{
+	int space_required;
+	int i;
+
+	if ( m == NULL ) return;
+
+	/* Allocate extra space if necessary */
+	space_required = m->n + NLC + NGL + 2;
+	if ( space_required > m->max_entries ) {
+
+		float *new_float_arr;
+		int *new_int_arr;
+		int new_max_entries;
+
+		if ( m->max_entries == 0 ) {
+			new_max_entries = 256;
+		} else {
+			new_max_entries = m->max_entries;
+		}
+
+		while ( new_max_entries < space_required ) {
+			new_max_entries *= 2;
+		}
+
+		new_float_arr = realloc(m->float_arr, new_max_entries*sizeof(float));
+		new_int_arr = realloc(m->int_arr, new_max_entries*sizeof(int));
+		if ( (new_float_arr == NULL) || (new_int_arr == NULL) ) return;
+
+		m->float_arr = new_float_arr;
+		m->int_arr = new_int_arr;
+		m->max_entries = new_max_entries;
+	}
+
+	/* The measurement */
+	m->float_arr[m->n] = rMeas;
+	m->int_arr[m->n] = 0;
+	m->n++;
+
+	/* Local gradients */
+	for ( i=0; i<NLC; i++ ) {
+		if ( derLc[i] != 0.0 ) {
+			m->float_arr[m->n] = derLc[i];
+			m->int_arr[m->n] = i+1;
+			m->n++;
+		}
+	}
+
+	/* The measurement error */
+	m->float_arr[m->n] = sigma;
+	m->int_arr[m->n] = 0;
+	m->n++;
+
+	/* Global gradients */
+	for ( i=0; i<NGL; i++ ) {
+		if ( (derGl[i] != 0.0) && (labels[i] > 0) ) {
+			m->float_arr[m->n] = derGl[i];
+			m->int_arr[m->n] = labels[i];
+			m->n++;
+		}
+	}
+}
+
+
+void write_mille(Mille *mille, int n, UnitCell *cell,
+                 struct reflpeak *rps, struct image *image,
+                 gsl_matrix **Minvs)
+{
+	int i;
+
+	/* No groups -> no refinement */
+	if ( image->detgeom->top_group == NULL ) return;
+
+	/* Local parameters */
+	const enum gparam rvl[] =
+	{
+		GPARAM_ASX,
+		GPARAM_ASY,
+		GPARAM_ASZ,
+		GPARAM_BSX,
+		GPARAM_BSY,
+		GPARAM_BSZ,
+		GPARAM_CSX,
+		GPARAM_CSY,
+		GPARAM_CSZ,
+	};
+	const int nl = 9;
+
+	/* Global parameters */
+	const enum gparam rvg[] =
+	{
+		GPARAM_DET_TX,
+		GPARAM_DET_TY,
+		GPARAM_DET_TZ,
+		GPARAM_DET_RX,
+		GPARAM_DET_RY,
+		GPARAM_DET_RZ,
+	};
+	const int ng = 6;
+	const int max_hierarchy_levels = 8;
+
+	for ( i=0; i<n; i++ ) {
+
+		float local_gradients_fs[nl];
+		float local_gradients_ss[nl];
+		float local_gradients_r[nl];
+		float global_gradients_fs[ng*max_hierarchy_levels];
+		float global_gradients_ss[ng*max_hierarchy_levels];
+		int labels[ng*max_hierarchy_levels];
+		int j, levels;
+		const struct detgeom_panel_group *group;
+
+		/* Local gradients */
+		for ( j=0; j<nl; j++ ) {
+			fs_ss_gradient(rvl[j], rps[i].refl, cell,
+			               &image->detgeom->panels[rps[i].peak->pn],
+			               Minvs[rps[i].peak->pn], 0, 0, 0,
+			               &local_gradients_fs[j],
+			               &local_gradients_ss[j]);
+			local_gradients_r[j] = EXC_WEIGHT * r_gradient(rvl[j], rps[i].refl,
+			                                               cell, image->lambda);
+		}
+
+		/* Global gradients for each hierarchy level, starting at the
+		 * individual panel and working up to the top level */
+		j = 0;
+		levels = 0;
+		group = image->detgeom->panels[rps[i].peak->pn].group;
+		while ( group != NULL ) {
+
+			double cx, cy, cz;
+			int g;
+
+			detgeom_group_center(group, &cx, &cy, &cz);
+
+			for ( g=0; g<ng; g++ ) {
+				fs_ss_gradient(rvg[g], rps[i].refl, cell,
+				               &image->detgeom->panels[rps[i].peak->pn],
+				               Minvs[rps[i].peak->pn], 0, 0, 0,
+				               &global_gradients_fs[j],
+				               &global_gradients_ss[j]);
+				labels[j] = mille_label(group->serial, rvg[g]);
+				j++;
+			}
+
+			levels++;
+			group = group->parent;
+
+			if ( levels >= max_hierarchy_levels ) {
+				ERROR("Too many nested hierarchy levels for refinement.\n");
+				break;
+			}
+		}
+
+		/* Add fs measurement */
+		mille_add_measurement(mille,
+		                      nl, local_gradients_fs,
+		                      j, global_gradients_fs, labels,
+		                      fs_dev(&rps[i], image->detgeom), 0.22);
+
+		/* Add ss measurement */
+		mille_add_measurement(mille,
+		                      nl, local_gradients_ss,
+		                      j, global_gradients_ss, labels,
+		                      ss_dev(&rps[i], image->detgeom), 0.22);
+
+		/* Add excitation error "measurement" (local-only) */
+		mille_add_measurement(mille, nl, local_gradients_r,
+		                      0, NULL, NULL, r_dev(&rps[i])*EXC_WEIGHT, 1.0);
+	}
+}
+
+
+Mille *crystfel_mille_new(const char *outFileName)
+{
+	Mille *m;
+
+	m = malloc(sizeof(Mille));
+	if ( m == NULL ) return NULL;
+
+	m->max_entries = 0;
+	m->n = 0;
+	m->float_arr = NULL;
+	m->int_arr = NULL;
+
+	m->fh = fopen(outFileName, "wb");
+	if ( m->fh == NULL ) {
+		ERROR("Failed to open Mille file '%s'\n", outFileName);
+		free(m);
+		return NULL;
+	}
+
+
+	return m;
+}
+
+
+void crystfel_mille_free(Mille *m)
+{
+	if ( m == NULL ) return;
+	fclose(m->fh);
+	free(m->float_arr);
+	free(m->int_arr);
+	free(m);
+}
+
+
+void crystfel_mille_delete_last_record(Mille *m)
+{
+	m->n = 0;
+}
+
+
+void crystfel_mille_write_record(Mille *m)
+{
+	float nf = 0.0;
+	int ni = 0;
+	int nw = (m->n * 2)+2;
+
+	fwrite(&nw, sizeof(int), 1, m->fh);
+
+	fwrite(&nf, sizeof(float), 1, m->fh);
+	fwrite(m->float_arr, sizeof(float), m->n, m->fh);
+
+	fwrite(&ni, sizeof(int), 1, m->fh);
+	fwrite(m->int_arr, sizeof(int), m->n, m->fh);
+	m->n = 0;
+}
diff --git a/libcrystfel/src/crystfel-mille.h b/libcrystfel/src/crystfel-mille.h
new file mode 100644
index 00000000..a4b83815
--- /dev/null
+++ b/libcrystfel/src/crystfel-mille.h
@@ -0,0 +1,60 @@
+/*
+ * crystfel-mille.h
+ *
+ * Interface to Millepede geometry refinement
+ *
+ * Copyright © 2023 Deutsches Elektronen-Synchrotron DESY,
+ *                  a research centre of the Helmholtz Association.
+ *
+ * Authors:
+ *   2023 Thomas White <taw@physics.org>
+ *
+ * This file is part of CrystFEL.
+ *
+ * CrystFEL 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.
+ *
+ * CrystFEL 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 CrystFEL.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef CRYSTFEL_MILLE_H
+#define CRYSTFEL_MILLE_H
+
+#include <gsl/gsl_matrix.h>
+
+typedef struct mille Mille;
+
+#include "cell.h"
+#include "image.h"
+#include "predict-refine.h"
+
+/**
+ * \file crystfel-mille.h
+ * Detector geometry refinement using Millepede
+ */
+
+extern Mille *crystfel_mille_new(const char *outFileName);
+
+extern void crystfel_mille_free(Mille *m);
+
+extern int mille_label(int group_serial, enum gparam param);
+extern enum gparam mille_unlabel(int n);
+
+extern void write_mille(Mille *mille, int n, UnitCell *cell,
+                        struct reflpeak *rps, struct image *image,
+                        gsl_matrix **Minvs);
+
+extern void crystfel_mille_delete_last_record(Mille *m);
+
+extern void crystfel_mille_write_record(Mille *m);
+
+#endif	/* CRYSTFEL_MILLE_H */
diff --git a/libcrystfel/src/datatemplate.c b/libcrystfel/src/datatemplate.c
index e97ae3df..36c0a422 100644
--- a/libcrystfel/src/datatemplate.c
+++ b/libcrystfel/src/datatemplate.c
@@ -45,16 +45,94 @@
  * \file datatemplate.h
  */
 
-struct rg_definition {
-	char *name;
-	char *pns;
-};
+static struct panel_group_template *find_group(const DataTemplate *dt, const char *name)
+{
+	int i;
+
+	for ( i=0; i<dt->n_groups; i++ ) {
+		if ( strcmp(dt->groups[i]->name, name) == 0 ) {
+			return dt->groups[i];
+		}
+	}
+
+	return NULL;
+}
+
+
+static struct panel_group_template *add_group(const char *name, DataTemplate *dt)
+{
+	struct panel_group_template *gt;
+
+	if ( find_group(dt, name) != NULL ) {
+		ERROR("Duplicate panel group '%s'\n", name);
+		return NULL;
+	}
+
+	if ( dt->n_groups >= MAX_PANEL_GROUPS ) {
+		ERROR("Too many panel groups\n");
+		return NULL;
+	}
+
+	gt = malloc(sizeof(struct panel_group_template));
+	if ( gt == NULL ) return NULL;
+
+	gt->name = strdup(name);
+	gt->n_children = 0;
+
+	if ( gt->name == NULL ) {
+		free(gt);
+		return NULL;
+	}
+
+	dt->groups[dt->n_groups++] = gt;
+
+	return gt;
+}
+
+
+static int parse_group(const char *name, DataTemplate *dt, const char *val)
+{
+	struct panel_group_template *gt;
+	int n_members;
+	char **members;
+	int i;
+	int fail = 0;
+
+	gt = add_group(name, dt);
+	if ( gt == NULL ) {
+		ERROR("Failed to add group\n");
+		return 1;
+	}
+
+	n_members = assplode(val, ",",  &members, ASSPLODE_NONE);
+	if ( n_members == 0 ) {
+		ERROR("Panel group '%s' has no members\n", name);
+		fail = 1;
+	}
+
+	if ( n_members > MAX_PANEL_GROUP_CHILDREN ) {
+		ERROR("Panel group '%s' has too many members\n", name);
+		fail = 1;
+	} else {
+
+		for ( i=0; i<n_members; i++ ) {
+			gt->children[i] = find_group(dt, members[i]);
+			if ( gt->children[i] == NULL ) {
+				ERROR("Unknown panel group '%s'\n", members[i]);
+				fail = 1;
+			}
+		}
+
+		gt->n_children = n_members;
+
+	}
 
+	for ( i=0; i<n_members; i++ ) free(members[i]);
+	free(members);
+
+	return fail;
+}
 
-struct rgc_definition {
-	char *name;
-	char *rgs;
-};
 
 
 static struct panel_template *new_panel(DataTemplate *det,
@@ -75,7 +153,6 @@ static struct panel_template *new_panel(DataTemplate *det,
 	new->name = strdup(name);
 
 	/* Copy strings */
-	new->cnz_from = safe_strdup(defaults->cnz_from);
 	new->data = safe_strdup(defaults->data);
 	new->satmap = safe_strdup(defaults->satmap);
 	new->satmap_file = safe_strdup(defaults->satmap_file);
@@ -84,6 +161,9 @@ static struct panel_template *new_panel(DataTemplate *det,
 		new->masks[i].filename = safe_strdup(defaults->masks[i].filename);
 	}
 
+	/* Create a new group just for this panel */
+	add_group(name, det);
+
 	return new;
 }
 
@@ -143,150 +223,6 @@ static struct dt_badregion *find_bad_region_by_name(DataTemplate *det,
 }
 
 
-static struct rigid_group *find_or_add_rg(DataTemplate *det,
-                                          const char *name)
-{
-	int i;
-	struct rigid_group **new;
-	struct rigid_group *rg;
-
-	for ( i=0; i<det->n_rigid_groups; i++ ) {
-
-		if ( strcmp(det->rigid_groups[i]->name, name) == 0 ) {
-			return det->rigid_groups[i];
-		}
-
-	}
-
-	new = realloc(det->rigid_groups,
-	              (1+det->n_rigid_groups)*sizeof(struct rigid_group *));
-	if ( new == NULL ) return NULL;
-
-	det->rigid_groups = new;
-
-	rg = malloc(sizeof(struct rigid_group));
-	if ( rg == NULL ) return NULL;
-
-	det->rigid_groups[det->n_rigid_groups++] = rg;
-
-	rg->name = strdup(name);
-	rg->panel_numbers = NULL;
-	rg->n_panels = 0;
-
-	return rg;
-}
-
-
-static struct rg_collection *find_or_add_rg_coll(DataTemplate *det,
-                                                 const char *name)
-{
-	int i;
-	struct rg_collection **new;
-	struct rg_collection *rgc;
-
-	for ( i=0; i<det->n_rg_collections; i++ ) {
-		if ( strcmp(det->rigid_group_collections[i]->name, name) == 0 )
-		{
-			return det->rigid_group_collections[i];
-		}
-	}
-
-	new = realloc(det->rigid_group_collections,
-	              (1+det->n_rg_collections)*sizeof(struct rg_collection *));
-	if ( new == NULL ) return NULL;
-
-	det->rigid_group_collections = new;
-
-	rgc = malloc(sizeof(struct rg_collection));
-	if ( rgc == NULL ) return NULL;
-
-	det->rigid_group_collections[det->n_rg_collections++] = rgc;
-
-	rgc->name = strdup(name);
-	rgc->rigid_groups = NULL;
-	rgc->n_rigid_groups = 0;
-
-	return rgc;
-}
-
-
-static void add_to_rigid_group(struct rigid_group *rg, int panel_number)
-{
-	int *pn;
-
-	pn = realloc(rg->panel_numbers, (1+rg->n_panels)*sizeof(int));
-	if ( pn == NULL ) {
-		ERROR("Couldn't add panel to rigid group.\n");
-		return;
-	}
-
-	rg->panel_numbers = pn;
-	rg->panel_numbers[rg->n_panels++] = panel_number;
-}
-
-
-static void add_to_rigid_group_coll(struct rg_collection *rgc,
-                                    struct rigid_group *rg)
-{
-	struct rigid_group **r;
-
-	r = realloc(rgc->rigid_groups, (1+rgc->n_rigid_groups)*
-	            sizeof(struct rigid_group *));
-	if ( r == NULL ) {
-		ERROR("Couldn't add rigid group to collection.\n");
-		return;
-	}
-
-	rgc->rigid_groups = r;
-	rgc->rigid_groups[rgc->n_rigid_groups++] = rg;
-}
-
-
-/* Free all rigid groups in detector */
-static void free_all_rigid_groups(DataTemplate *det)
-{
-	int i;
-
-	if ( det->rigid_groups == NULL ) return;
-	for ( i=0; i<det->n_rigid_groups; i++ ) {
-		free(det->rigid_groups[i]->name);
-		free(det->rigid_groups[i]->panel_numbers);
-		free(det->rigid_groups[i]);
-	}
-	free(det->rigid_groups);
-}
-
-
-/* Free all rigid groups in detector */
-static void free_all_rigid_group_collections(DataTemplate *det)
-{
-	int i;
-
-	if ( det->rigid_group_collections == NULL ) return;
-	for ( i=0; i<det->n_rg_collections; i++ ) {
-		free(det->rigid_group_collections[i]->name);
-		free(det->rigid_group_collections[i]->rigid_groups);
-		free(det->rigid_group_collections[i]);
-	}
-	free(det->rigid_group_collections);
-}
-
-
-static struct rigid_group *find_rigid_group_by_name(DataTemplate *det,
-                                                    char *name)
-{
-	int i;
-
-	for ( i=0; i<det->n_rigid_groups; i++ ) {
-		if ( strcmp(det->rigid_groups[i]->name, name) == 0 ) {
-			return det->rigid_groups[i];
-		}
-	}
-
-	return NULL;
-}
-
-
 static int atob(const char *a)
 {
 	if ( strcasecmp(a, "true") == 0 ) return 1;
@@ -490,7 +426,8 @@ static int add_flag_value(struct panel_template *p,
 
 static int parse_mask(struct panel_template *panel,
                       const char *key_orig,
-                      const char *val)
+                      const char *val,
+                      int def)
 {
 	int n;
 	char *key;
@@ -551,13 +488,15 @@ static int parse_mask(struct panel_template *panel,
 		return 1;
 	}
 
+	panel->masks[n].mask_default = def;
 	free(key);
 	return 0;
 }
 
 
 static int parse_field_for_panel(struct panel_template *panel, const char *key,
-                                 const char *val, DataTemplate *det)
+                                 const char *val, DataTemplate *det,
+                                 int def)
 {
 	int reject = 0;
 
@@ -576,16 +515,19 @@ static int parse_field_for_panel(struct panel_template *panel, const char *key,
 	} else if ( strcmp(key, "adu_per_eV") == 0 ) {
 		panel->adu_scale = atof(val);
 		panel->adu_scale_unit = ADU_PER_EV;
+		panel->adu_scale_default = def;
 	} else if ( strcmp(key, "adu_per_photon") == 0 ) {
 		panel->adu_scale = atof(val);
 		panel->adu_scale_unit = ADU_PER_PHOTON;
+		panel->adu_scale_default = def;
 	} else if ( strcmp(key, "clen") == 0 ) {
-		/* Gets expanded when image is loaded */
-		panel->cnz_from = strdup(val);
+		ERROR("'clen' is a top-level property in this version of CrystFEL.\n");
+		reject = 1;
 
 	} else if ( strcmp(key, "data") == 0 ) {
 		free(panel->data);
 		panel->data = strdup(val);
+		panel->data_default = def;
 
 	} else if ( strcmp(key, "mask_edge_pixels") == 0 ) {
 		if ( convert_int(val, &panel->mask_edge_pixels) ) {
@@ -593,30 +535,36 @@ static int parse_field_for_panel(struct panel_template *panel, const char *key,
 			      panel->name, val);
 			reject = 1;
 		}
+		panel->mask_edge_pixels_default = def;
 
 	} else if ( strcmp(key, "mask_bad") == 0 ) {
-		parse_field_for_panel(panel, "mask0_badbits", val, det);
+		parse_field_for_panel(panel, "mask0_badbits", val, det, def);
 	} else if ( strcmp(key, "mask_good") == 0 ) {
-		parse_field_for_panel(panel, "mask0_goodbits", val, det);
+		parse_field_for_panel(panel, "mask0_goodbits", val, det, def);
 	} else if ( strcmp(key, "mask") == 0 ) {
-		parse_field_for_panel(panel, "mask0_data", val, det);
+		parse_field_for_panel(panel, "mask0_data", val, det, def);
 	} else if ( strcmp(key, "mask_file") == 0 ) {
-		parse_field_for_panel(panel, "mask0_file", val, det);
+		parse_field_for_panel(panel, "mask0_file", val, det, def);
 
 	} else if ( strncmp(key, "mask", 4) == 0 ) {
-		reject = parse_mask(panel, key, val);
+		reject = parse_mask(panel, key, val, def);
 
 	} else if ( strcmp(key, "saturation_map") == 0 ) {
 		panel->satmap = strdup(val);
+		panel->satmap_default = def;
 	} else if ( strcmp(key, "saturation_map_file") == 0 ) {
 		panel->satmap_file = strdup(val);
+		panel->satmap_file_default = def;
 
 	} else if ( strcmp(key, "coffset") == 0) {
 		panel->cnz_offset = atof(val);
+		panel->cnz_offset_default = def;
 	} else if ( strcmp(key, "res") == 0 ) {
 		panel->pixel_pitch = 1.0/atof(val);
+		panel->pixel_pitch_default = def;
 	} else if ( strcmp(key, "max_adu") == 0 ) {
 		panel->max_adu = atof(val);
+		panel->max_adu_default = def;
 		ERROR("WARNING: It's usually better not to set max_adu "
 		      "in the geometry file.  Use --max-adu during "
 		      "merging instead.\n");
@@ -625,14 +573,17 @@ static int parse_field_for_panel(struct panel_template *panel, const char *key,
 		if ( add_flag_value(panel, atof(val), FLAG_EQUAL) ) {
 			reject = -1;
 		}
+		panel->flag_values_default = def;
 	} else if ( strcmp(key, "flag_lessthan") == 0 ) {
 		if ( add_flag_value(panel, atof(val), FLAG_LESSTHAN) ) {
 			reject = -1;
 		}
+		panel->flag_values_default = def;
 	} else if ( strcmp(key, "flag_morethan") == 0 ) {
 		if ( add_flag_value(panel, atof(val), FLAG_MORETHAN) ) {
 			reject = -1;
 		}
+		panel->flag_values_default = def;
 
 	} else if ( strcmp(key, "badrow_direction") == 0 ) {
 		ERROR("WARNING 'badrow_direction' is ignored in this version.\n");
@@ -851,10 +802,6 @@ static int parse_peak_layout(const char *val,
 static int parse_toplevel(DataTemplate *dt,
                           const char *key,
                           const char *val,
-                          struct rg_definition ***rg_defl,
-                          struct rgc_definition ***rgc_defl,
-                          int *n_rg_defs,
-                          int *n_rgc_defs,
                           struct panel_template *defaults,
                           int *defaults_updated)
 {
@@ -864,6 +811,9 @@ static int parse_toplevel(DataTemplate *dt,
 	} else if ( strcmp(key, "detector_shift_y") == 0 ) {
 		dt->shift_y_from = strdup(val);
 
+	} else if ( strcmp(key, "clen") == 0 ) {
+		dt->cnz_from = strdup(val);
+
 	} else if ( strcmp(key, "photon_energy") == 0 ) {
 		return parse_photon_energy(val,
 		                           &dt->wavelength_from,
@@ -895,37 +845,22 @@ static int parse_toplevel(DataTemplate *dt,
 			ERROR("Invalid value for bandwidth\n");
 		}
 
-	} else if (strncmp(key, "rigid_group", 11) == 0
-	        && strncmp(key, "rigid_group_collection", 22) != 0 ) {
-
-		struct rg_definition **new;
-
-		new = realloc(*rg_defl,
-		             ((*n_rg_defs)+1)*sizeof(struct rg_definition*));
-		*rg_defl = new;
+	} else if ( strncmp(key, "rigid_group", 11) == 0 ) {
 
-		(*rg_defl)[*n_rg_defs] = malloc(sizeof(struct rg_definition));
-		(*rg_defl)[*n_rg_defs]->name = strdup(key+12);
-		(*rg_defl)[*n_rg_defs]->pns = strdup(val);
-		*n_rg_defs = *n_rg_defs+1;
+		/* Rigid group lines are ignored in this version */
 
-	} else if ( strncmp(key, "rigid_group_collection", 22) == 0 ) {
+	} else if ( strncmp(key, "group_", 6) == 0 ) {
 
-		struct rgc_definition **new;
-
-		new = realloc(*rgc_defl, ((*n_rgc_defs)+1)*
-		      sizeof(struct rgc_definition*));
-		*rgc_defl = new;
-
-		(*rgc_defl)[*n_rgc_defs] =
-		                   malloc(sizeof(struct rgc_definition));
-		(*rgc_defl)[*n_rgc_defs]->name = strdup(key+23);
-		(*rgc_defl)[*n_rgc_defs]->rgs = strdup(val);
-		*n_rgc_defs = *n_rgc_defs+1;
+		if ( parse_group(key+6, dt, val) ) {
+			return 1;
+		}
 
 	} else {
 
-		if ( parse_field_for_panel(defaults, key, val, dt) == 0 ) {
+		/* If there are any panels, the value in 'defaults' gets marked
+		 * as "not default".  This will cause it to be written out for
+		 * each subsequent panel. */
+		if ( parse_field_for_panel(defaults, key, val, dt, (dt->n_panels==0)) == 0 ) {
 			*defaults_updated = 1;
 		} else {
 			return 1;
@@ -1038,18 +973,38 @@ static int try_guess_panel(struct dt_badregion *bad, DataTemplate *dt)
 }
 
 
+static void show_group(const struct panel_group_template *gt, int level)
+{
+	int i;
+
+	for ( i=0; i<level; i++ ) STATUS("  ");
+
+	if ( gt == NULL ) {
+		STATUS("!!!\n");
+		return;
+	}
+
+	STATUS("%s\n", gt->name);
+
+	for ( i=0; i<gt->n_children; i++ ) {
+		show_group(gt->children[i], level+1);
+	}
+}
+
+
+void data_template_show_hierarchy(const DataTemplate *dtempl)
+{
+	STATUS("Hierarchy:\n");
+	show_group(find_group(dtempl, "all"), 0);
+}
+
+
 DataTemplate *data_template_new_from_string(const char *string_in)
 {
 	DataTemplate *dt;
-	char **bits;
 	int done = 0;
 	int i;
-	int rgi, rgci;
 	int reject = 0;
-	struct rg_definition **rg_defl = NULL;
-	struct rgc_definition **rgc_defl = NULL;
-	int n_rg_definitions = 0;
-	int n_rgc_definitions = 0;
 	char *string;
 	char *string_orig;
 	size_t len;
@@ -1063,15 +1018,13 @@ DataTemplate *data_template_new_from_string(const char *string_in)
 	dt->panels = NULL;
 	dt->n_bad = 0;
 	dt->bad = NULL;
-	dt->n_rigid_groups = 0;
-	dt->rigid_groups = NULL;
-	dt->n_rg_collections = 0;
-	dt->rigid_group_collections = NULL;
 	dt->bandwidth = 0.00000001;
 	dt->peak_list = NULL;
 	dt->shift_x_from = NULL;
 	dt->shift_y_from = NULL;
+	dt->cnz_from = NULL;
 	dt->n_headers_to_copy = 0;
+	dt->n_groups = 0;
 
 	/* The default defaults... */
 	defaults.orig_min_fs = -1;
@@ -1080,11 +1033,13 @@ DataTemplate *data_template_new_from_string(const char *string_in)
 	defaults.orig_max_ss = -1;
 	defaults.cnx = NAN;
 	defaults.cny = NAN;
-	defaults.cnz_from = NULL;
 	defaults.cnz_offset = 0.0;
+	defaults.cnz_offset_default = 1;
 	defaults.pixel_pitch = -1.0;
+	defaults.pixel_pitch_default = 1;
 	defaults.bad = 0;
 	defaults.mask_edge_pixels = 0;
+	defaults.mask_edge_pixels_default = 1;
 	defaults.fsx = NAN;
 	defaults.fsy = NAN;
 	defaults.fsz = NAN;
@@ -1093,22 +1048,30 @@ DataTemplate *data_template_new_from_string(const char *string_in)
 	defaults.ssz = NAN;
 	defaults.adu_scale = NAN;
 	defaults.adu_scale_unit = ADU_PER_PHOTON;
+	defaults.adu_scale_default = 1;
 	for ( i=0; i<MAX_FLAG_VALUES; i++ ) defaults.flag_values[i] = 0;
 	for ( i=0; i<MAX_FLAG_VALUES; i++ ) defaults.flag_types[i] = FLAG_NOTHING;
+	defaults.flag_values_default = 1;
 	for ( i=0; i<MAX_MASKS; i++ ) {
 		defaults.masks[i].data_location = NULL;
 		defaults.masks[i].filename = NULL;
 		defaults.masks[i].good_bits = 0;
 		defaults.masks[i].bad_bits = 0;
+		defaults.masks[i].mask_default = 1;
 	}
 	defaults.max_adu = +INFINITY;
+	defaults.max_adu_default = 1;
 	defaults.satmap = NULL;
+	defaults.satmap_default = 1;
 	defaults.satmap_file = NULL;
+	defaults.satmap_file_default = 1;
 	defaults.data = strdup("/data/data");
+	defaults.data_default = 1;
 	defaults.name = NULL;
 	defaults.dims[0] = DIM_SS;
 	defaults.dims[1] = DIM_FS;
 	for ( i=2; i<MAX_DIMS; i++ ) defaults.dims[i] = DIM_UNDEFINED;
+	for ( i=0; i<MAX_DIMS; i++ ) defaults.dims_default[i] = 1;
 
 	string = strdup(string_in);
 	if ( string == NULL ) return NULL;
@@ -1184,10 +1147,6 @@ DataTemplate *data_template_new_from_string(const char *string_in)
 
 			/* Top-level option */
 		        if ( parse_toplevel(dt, line, val,
-		                            &rg_defl,
-		                            &rgc_defl,
-		                            &n_rg_definitions,
-		                            &n_rgc_definitions,
 		                            &defaults,
 		                            &have_unused_defaults) )
 			{
@@ -1218,11 +1177,9 @@ DataTemplate *data_template_new_from_string(const char *string_in)
 		}
 
 		if ( panel != NULL ) {
-			if ( parse_field_for_panel(panel, key, val,
-			                           dt) ) reject = 1;
+			if ( parse_field_for_panel(panel, key, val, dt, 0) ) reject = 1;
 		} else {
-			if ( parse_field_bad(badregion, key,
-			                     val) ) reject = 1;
+			if ( parse_field_bad(badregion, key, val) ) reject = 1;
 		}
 
 		free(line);
@@ -1254,6 +1211,11 @@ DataTemplate *data_template_new_from_string(const char *string_in)
 		reject = 1;
 	}
 
+	if ( dt->cnz_from == NULL ) {
+		ERROR("Geometry file must specify the camera length\n");
+		reject = 1;
+	}
+
 	for ( i=0; i<dt->n_panels; i++ ) {
 
 		int j;
@@ -1316,11 +1278,6 @@ DataTemplate *data_template_new_from_string(const char *string_in)
 			      " panel %s\n", dt->panels[i].name);
 			reject = 1;
 		}
-		if ( p->cnz_from == NULL ) {
-			ERROR("Please specify the camera length for panel %s\n",
-			      dt->panels[i].name);
-			reject = 1;
-		}
 		if ( p->pixel_pitch < 0 ) {
 			ERROR("Please specify the pixel size for"
 			      " panel %s\n", dt->panels[i].name);
@@ -1420,73 +1377,17 @@ DataTemplate *data_template_new_from_string(const char *string_in)
 		}
 	}
 
-	free(defaults.cnz_from);
 	free(defaults.data);
 	for ( i=0; i<MAX_MASKS; i++ ) {
 		free(defaults.masks[i].data_location);
 		free(defaults.masks[i].filename);
 	}
 
-	for ( rgi=0; rgi<n_rg_definitions; rgi++) {
-
-		int pi, n1;
-		struct rigid_group *rigidgroup = NULL;
-
-		rigidgroup = find_or_add_rg(dt, rg_defl[rgi]->name);
-
-		n1 = assplode(rg_defl[rgi]->pns, ",", &bits, ASSPLODE_NONE);
-
-		for ( pi=0; pi<n1; pi++ ) {
-
-			int panel_number;
-			if ( data_template_panel_name_to_number(dt,
-			                                        bits[pi],
-			                                        &panel_number) )
-			{
-				ERROR("Cannot add panel to rigid group\n");
-				ERROR("Panel not found: %s\n", bits[pi]);
-				return NULL;
-			}
-			add_to_rigid_group(rigidgroup, panel_number);
-			free(bits[pi]);
-
-		}
-		free(bits);
-		free(rg_defl[rgi]->name);
-		free(rg_defl[rgi]->pns);
-		free(rg_defl[rgi]);
-	}
-	free(rg_defl);
-
-	for ( rgci=0; rgci<n_rgc_definitions; rgci++ ) {
-
-		int n2;
-		struct rg_collection *rgcollection = NULL;
-
-		rgcollection = find_or_add_rg_coll(dt, rgc_defl[rgci]->name);
-
-		n2 = assplode(rgc_defl[rgci]->rgs, ",", &bits, ASSPLODE_NONE);
-
-		for ( rgi=0; rgi<n2; rgi++ ) {
-
-			struct rigid_group *r;
-
-			r = find_rigid_group_by_name(dt, bits[rgi]);
-			if ( r == NULL ) {
-				ERROR("Cannot add rigid group to collection\n");
-				ERROR("Rigid group not found: %s\n", bits[rgi]);
-				return NULL;
-			}
-			add_to_rigid_group_coll(rgcollection, r);
-			free(bits[rgi]);
-		}
-		free(bits);
-		free(rgc_defl[rgci]->name);
-		free(rgc_defl[rgci]->rgs);
-		free(rgc_defl[rgci]);
-
+	/* If this is a single-panel detector, there should only be one group
+	 * called "all" which points to the panel */
+	if ( (dt->n_panels == 1) && (dt->n_groups == 1) ) {
+		parse_group("all", dt, dt->groups[0]->name);
 	}
-	free(rgc_defl);
 
 	free(string_orig);
 
@@ -1519,9 +1420,6 @@ void data_template_free(DataTemplate *dt)
 
 	if ( dt == NULL ) return;
 
-	free_all_rigid_groups(dt);
-	free_all_rigid_group_collections(dt);
-
 	for ( i=0; i<dt->n_panels; i++ ) {
 
 		int j;
@@ -1530,7 +1428,6 @@ void data_template_free(DataTemplate *dt)
 		free(dt->panels[i].data);
 		free(dt->panels[i].satmap);
 		free(dt->panels[i].satmap_file);
-		free(dt->panels[i].cnz_from);
 
 		for ( j=0; j<MAX_MASKS; j++ ) {
 			free(dt->panels[i].masks[j].filename);
@@ -1542,8 +1439,14 @@ void data_template_free(DataTemplate *dt)
 		free(dt->headers_to_copy[i]);
 	}
 
+	for ( i=0; i<dt->n_groups; i++ ) {
+		free(dt->groups[i]->name);
+		free(dt->groups[i]);
+	}
+
 	free(dt->wavelength_from);
 	free(dt->peak_list);
+	free(dt->cnz_from);
 
 	free(dt->panels);
 	free(dt->bad);
@@ -1872,87 +1775,23 @@ static int im_get_length(struct image *image, const char *from,
 }
 
 
-static int safe_strcmp(const char *a, const char *b)
-{
-	if ( (a==NULL) && (b==NULL) ) return 0;
-	if ( (a!=NULL) && (b!=NULL) ) return strcmp(a, b);
-	return 1;
-}
-
-
-static int all_panels_reference_same_clen(const DataTemplate *dtempl)
-{
-	int i;
-	char *first_val = NULL;
-	char *first_units = NULL;
-	int fail = 0;
-
-	for ( i=0; i<dtempl->n_panels; i++ ) {
-		struct panel_template *p = &dtempl->panels[i];
-		char *val;
-		char *units;
-		if ( separate_value_and_units(p->cnz_from, &val, &units) ) {
-			/* Parse error */
-			return 0;
-		}
-		if ( i == 0 ) {
-			first_val = val;
-			first_units = units;
-		} else {
-			if ( safe_strcmp(val, first_val) != 0 ) fail = 1;
-			if ( safe_strcmp(units, first_units) != 0 ) fail = 1;
-			free(val);
-			free(units);
-		}
-	}
-
-	free(first_val);
-	free(first_units);
-	return fail;
-}
-
-
-static int all_coffsets_small(const DataTemplate *dtempl)
+static int all_panels_same_coffset(const DataTemplate *dtempl)
 {
 	int i;
-
-	for ( i=0; i<dtempl->n_panels; i++ ) {
-		struct panel_template *p = &dtempl->panels[i];
-		if ( p->cnz_offset > 10.0*p->pixel_pitch ) return 0;
-	}
-
-	return 1;
-}
-
-
-static int all_panels_same_clen(const DataTemplate *dtempl)
-{
-	int i;
-	double *zvals;
-	double total = 0.0;
+	double total;
 	double mean;
 
-	zvals = malloc(sizeof(double)*dtempl->n_panels);
-	if ( zvals == NULL ) return 0;
-
+	total = 0.0;
 	for ( i=0; i<dtempl->n_panels; i++ ) {
-		struct panel_template *p = &dtempl->panels[i];
-		if ( im_get_length(NULL, p->cnz_from, 1e-3, &zvals[i]) ) {
-			/* Can't get length because it used a header reference */
-			free(zvals);
-			return 0;
-		}
-		total += zvals[i];
+		total += dtempl->panels[i].cnz_offset;
 	}
-
 	mean = total/dtempl->n_panels;
+
 	for ( i=0; i<dtempl->n_panels; i++ ) {
 		struct panel_template *p = &dtempl->panels[i];
-		if ( fabs(zvals[i] - mean) > 10.0*p->pixel_pitch ) return 0;
+		if ( fabs(dtempl->panels[i].cnz_offset - mean) > 10.0*p->pixel_pitch ) return 0;
 	}
 
-	free(zvals);
-
 	return 1;
 }
 
@@ -1974,8 +1813,92 @@ static int all_panels_perpendicular_to_beam(const DataTemplate *dtempl)
 static int detector_flat(const DataTemplate *dtempl)
 {
 	return all_panels_perpendicular_to_beam(dtempl)
-	    && ( (all_panels_reference_same_clen(dtempl) && all_coffsets_small(dtempl))
-	          || all_panels_same_clen(dtempl) );
+	    && all_panels_same_coffset(dtempl);
+}
+
+
+static void add_dg_point(const struct detgeom_panel *p,
+                         int fs, int ss,
+                         double *tx, double *ty, double *tz)
+{
+	*tx += (p->cnx + fs*p->fsx + ss*p->ssx) * p->pixel_pitch;
+	*ty += (p->cny + fs*p->fsy + ss*p->ssy) * p->pixel_pitch;
+	*tz += (p->cnz + fs*p->fsz + ss*p->ssz) * p->pixel_pitch;
+}
+
+
+static struct detgeom_panel_group *walk_group(const DataTemplate *dtempl,
+                                              struct panel_group_template *gt,
+                                              struct detgeom *detgeom,
+                                              int serial, int c_mul)
+{
+	struct detgeom_panel_group *gr;
+
+	if ( gt == NULL ) return NULL;
+
+	gr = malloc(sizeof(struct detgeom_panel_group));
+	if ( gr == NULL ) return NULL;
+
+	gr->name = strdup(gt->name);
+	gr->n_children = gt->n_children;
+
+	if ( gr->n_children == 0 ) {
+
+		/* Leaf node */
+		gr->children = NULL;
+		gr->panel = detgeom_find_panel(detgeom, gr->name);
+		if ( gr->panel == NULL ) {
+			ERROR("Couldn't find panel %s for leaf group\n", gr->name);
+			return NULL;
+		}
+		gr->panel->group = gr;
+
+		/* Calculate and make a note of the panel center */
+		double tx = 0.0;
+		double ty = 0.0;
+		double tz = 0.0;
+
+		add_dg_point(gr->panel, 0, 0, &tx, &ty, &tz);
+		add_dg_point(gr->panel, gr->panel->w, 0, &tx, &ty, &tz);
+		add_dg_point(gr->panel, 0, gr->panel->h, &tx, &ty, &tz);
+		add_dg_point(gr->panel, gr->panel->w, gr->panel->h, &tx, &ty, &tz);
+
+		gr->cx = tx / 4.0;
+		gr->cy = ty / 4.0;
+		gr->cz = tz / 4.0;
+
+	} else {
+
+		int i;
+		double tx = 0.0;
+		double ty = 0.0;
+		double tz = 0.0;
+
+		gr->panel = NULL;
+		gr->children = malloc(gt->n_children*sizeof(struct detgeom_panel_group *));
+		if ( gr->children == NULL ) {
+			free(gr);
+			return NULL;
+		}
+
+		for ( i=0; i<gt->n_children; i++ ) {
+			gr->children[i] = walk_group(dtempl, gt->children[i], detgeom,
+			                             serial + c_mul*(i+1), c_mul*100);
+			if ( gr->children[i] == NULL ) return NULL;
+			gr->children[i]->parent = gr;
+			tx += gr->children[i]->cx;
+			ty += gr->children[i]->cy;
+			tz += gr->children[i]->cz;
+		}
+
+		gr->cx = tx / gt->n_children;
+		gr->cy = ty / gt->n_children;
+		gr->cz = tz / gt->n_children;
+
+	}
+
+	gr->serial = serial;
+	return gr;
 }
 
 
@@ -1985,6 +1908,7 @@ struct detgeom *create_detgeom(struct image *image,
 {
 	struct detgeom *detgeom;
 	int i;
+	double clen;
 
 	if ( dtempl == NULL ) {
 		ERROR("NULL data template!\n");
@@ -1994,6 +1918,8 @@ struct detgeom *create_detgeom(struct image *image,
 	detgeom = malloc(sizeof(struct detgeom));
 	if ( detgeom == NULL ) return NULL;
 
+	detgeom->top_group = NULL;
+
 	detgeom->panels = malloc(dtempl->n_panels*sizeof(struct detgeom_panel));
 	if ( detgeom->panels == NULL ) {
 		free(detgeom);
@@ -2013,6 +1939,17 @@ struct detgeom *create_detgeom(struct image *image,
 		}
 	}
 
+	if ( im_get_length(image, dtempl->cnz_from, 1e-3, &clen) )
+	{
+		if ( two_d_only ) {
+			clen = NAN;
+		} else {
+			ERROR("Failed to read length from '%s'\n", dtempl->cnz_from);
+			return NULL;
+		}
+	}
+
+
 	for ( i=0; i<dtempl->n_panels; i++ ) {
 
 		struct detgeom_panel *p = &detgeom->panels[i];
@@ -2027,20 +1964,8 @@ struct detgeom *create_detgeom(struct image *image,
 		p->cnx = tmpl->cnx;
 		p->cny = tmpl->cny;
 
-		if ( im_get_length(image, tmpl->cnz_from, 1e-3, &p->cnz) )
-		{
-			if ( two_d_only ) {
-				p->cnz = NAN;
-			} else {
-				ERROR("Failed to read length from '%s'\n", tmpl->cnz_from);
-				return NULL;
-			}
-		}
-
-		/* Apply offset (in m) and then convert cnz from
-		 * m to pixels */
-		p->cnz += tmpl->cnz_offset;
-		p->cnz /= p->pixel_pitch;
+		/* Apply offset (in m) and then convert cnz from m to pixels */
+		p->cnz = (clen + tmpl->cnz_offset) / p->pixel_pitch;
 
 		/* Apply overall shift (already in m) */
 		if ( dtempl->shift_x_from != NULL ) {
@@ -2103,6 +2028,14 @@ struct detgeom *create_detgeom(struct image *image,
 
 	}
 
+	detgeom->top_group = walk_group(dtempl, find_group(dtempl, "all"), detgeom, 0, 100);
+	if ( detgeom->top_group != NULL ) {
+		detgeom->top_group->parent = NULL;
+	} else {
+		ERROR("Warning: Top-level panel group ('all') not found.  "
+		      "Geometry refinement will not be possible.\n");
+	}
+
 	return detgeom;
 }
 
@@ -2145,3 +2078,598 @@ double data_template_get_clen_if_possible(const DataTemplate *dt)
 	detgeom_free(dg);
 	return clen;
 }
+
+
+static int translate_group_contents(DataTemplate *dtempl,
+                                    const struct panel_group_template *group,
+                                    double x, double y, double z,
+                                    int is_metres)
+{
+	int i;
+
+	if ( group->n_children == 0 ) {
+
+		struct panel_template *p = find_panel_by_name(dtempl, group->name);
+		if ( p == NULL ) return 1;
+
+		if ( is_metres ) {
+			p->cnx += x/p->pixel_pitch;
+			p->cny += y/p->pixel_pitch;
+			p->cnz_offset += z;
+		} else {
+			p->cnx += x;
+			p->cny += y;
+			p->cnz_offset += z*p->pixel_pitch;
+		}
+
+	} else {
+		for ( i=0; i<group->n_children; i++ ) {
+			translate_group_contents(dtempl, group->children[i],
+			                         x, y, z, is_metres);
+		}
+	}
+
+	return 0;
+}
+
+
+/**
+ * Alters dtempl by shifting the named panel group by x,y,z in the CrystFEL
+ * coordinate system.  x,y,z are in pixels, and all panels in the group must
+ * have the same pixel size (but, this will not be checked).
+ *
+ * \returns zero for success, non-zero on error
+ */
+int data_template_translate_group_px(DataTemplate *dtempl, const char *group_name,
+                                     double x, double y, double z)
+{
+	const struct panel_group_template *group = find_group(dtempl, group_name);
+	if ( group == NULL ) return 1;
+	return translate_group_contents(dtempl, group, x, y, z, 0);
+}
+
+
+/**
+ * Alters dtempl by shifting the named panel group by x,y,z in the CrystFEL
+ * coordinate system.  x,y,z are in metres.
+ *
+ * \returns zero for success, non-zero on error
+ */
+int data_template_translate_group_m(DataTemplate *dtempl, const char *group_name,
+                                    double x, double y, double z)
+{
+	const struct panel_group_template *group = find_group(dtempl, group_name);
+	if ( group == NULL ) return 1;
+	return translate_group_contents(dtempl, group, x, y, z, 1);
+}
+
+
+static void add_point(const struct panel_template *p,
+                      int fs, int ss,
+                      double *tx, double *ty, double *tz)
+{
+	*tx += (p->cnx + fs*p->fsx + ss*p->ssx) * p->pixel_pitch;
+	*ty += (p->cny + fs*p->fsy + ss*p->ssy) * p->pixel_pitch;
+	*tz += p->cnz_offset + (fs*p->fsz + ss*p->ssz) * p->pixel_pitch;
+}
+
+
+static int group_center(DataTemplate *dtempl,
+                        const struct panel_group_template *group,
+                        double *cx, double *cy, double *cz)
+{
+	if ( group->n_children == 0 ) {
+
+		const struct panel_template *p = find_panel_by_name(dtempl, group->name);
+		if ( p == NULL ) return 1;
+
+		double tx = 0.0;
+		double ty = 0.0;
+		double tz = 0.0;
+
+		add_point(p, 0, 0, &tx, &ty, &tz);
+		add_point(p, PANEL_WIDTH(p), 0, &tx, &ty, &tz);
+		add_point(p, 0, PANEL_HEIGHT(p), &tx, &ty, &tz);
+		add_point(p, PANEL_WIDTH(p), PANEL_HEIGHT(p), &tx, &ty, &tz);
+
+		*cx = tx / 4.0;
+		*cy = ty / 4.0;
+		*cz = tz / 4.0;
+
+		return 0;
+
+	} else {
+
+		int i;
+		double tx = 0.0;
+		double ty = 0.0;
+		double tz = 0.0;
+
+		for ( i=0; i<group->n_children; i++ ) {
+			double gcx, gcy, gcz;
+			group_center(dtempl, group->children[i], &gcx, &gcy, &gcz);
+			tx += gcx;
+			ty += gcy;
+			tz += gcz;
+		}
+
+		*cx = tx / group->n_children;
+		*cy = ty / group->n_children;
+		*cz = tz / group->n_children;
+
+		return 0;
+
+	}
+}
+
+
+static int rotate_all_panels(DataTemplate *dtempl,
+                             struct panel_group_template *group,
+                             char axis, double ang,
+                             double cx, double cy, double cz)
+{
+	if ( group->n_children == 0 ) {
+
+		double cnz_px;
+		struct panel_template *p = find_panel_by_name(dtempl, group->name);
+		if ( p == NULL ) return 1;
+
+		cx /= p->pixel_pitch;
+		cy /= p->pixel_pitch;
+		cz /= p->pixel_pitch;
+		cnz_px = p->cnz_offset / p->pixel_pitch;
+
+		switch ( axis )
+		{
+			case 'x':
+			rotate2d(&p->cny, &cnz_px, cy, cz, ang);
+			rotate2d(&p->fsy, &p->fsz, 0, 0, ang);
+			rotate2d(&p->ssy, &p->ssz, 0, 0, ang);
+			p->cnz_offset = cnz_px * p->pixel_pitch;
+			break;
+
+			case 'y':
+			rotate2d(&cnz_px, &p->cnx, cz, cx, ang);
+			rotate2d(&p->fsz, &p->fsx, 0, 0, ang);
+			rotate2d(&p->ssz, &p->ssx, 0, 0, ang);
+			p->cnz_offset = cnz_px * p->pixel_pitch;
+			break;
+
+			case 'z':
+			rotate2d(&p->cnx, &p->cny, cx, cy, ang);
+			rotate2d(&p->fsx, &p->fsy, 0, 0, ang);
+			rotate2d(&p->ssx, &p->ssy, 0, 0, ang);
+			break;
+
+			default:
+			ERROR("Invalid rotation axis '%c'\n", axis);
+			return 1;
+		}
+
+		return 0;
+
+	} else {
+
+		int i;
+
+		for ( i=0; i<group->n_children; i++ ) {
+			rotate_all_panels(dtempl, group->children[i],
+			                  axis, ang, cx, cy, cz);
+		}
+
+		return 0;
+
+	}
+}
+
+/**
+ * Alters dtempl by rotating the named panel group by ang (degrees) about its
+ * center.
+ *
+ * \returns zero for success, non-zero on error
+ */
+int data_template_rotate_group(DataTemplate *dtempl, const char *group_name,
+                               double ang, char axis)
+{
+	struct panel_group_template *group;
+	double cx, cy, cz;
+
+	group = find_group(dtempl, group_name);
+	if ( group == NULL ) return 1;
+
+	if ( group_center(dtempl, group, &cx, &cy, &cz) ) return 1;
+
+	return rotate_all_panels(dtempl, group, axis, ang, cx, cy, cz);
+}
+
+
+static const char *str_dim(int dim)
+{
+	switch ( dim ) {
+		case DIM_FS: return "fs";
+		case DIM_SS: return "ss";
+		case DIM_PLACEHOLDER: return "%";
+		default: return NULL;
+	}
+}
+
+
+int data_template_write_to_file(const DataTemplate *dtempl, const char *filename)
+{
+	FILE *fh;
+	int i;
+
+	fh = fopen(filename, "w");
+	if ( fh == NULL ) return 1;
+
+	/* Basic top-level parameters */
+	switch ( dtempl->wavelength_unit ) {
+
+		case WAVELENGTH_M:
+		fprintf(fh, "wavelength = %s m\n", dtempl->wavelength_from);
+		break;
+
+		case WAVELENGTH_A:
+		fprintf(fh, "wavelength = %s A\n", dtempl->wavelength_from);
+		break;
+
+		case WAVELENGTH_ELECTRON_KV:
+		fprintf(fh, "electron_voltage = %s kV\n", dtempl->wavelength_from);
+		break;
+
+		case WAVELENGTH_ELECTRON_V:
+		fprintf(fh, "electron_voltage = %s V\n", dtempl->wavelength_from);
+		break;
+
+		case WAVELENGTH_PHOTON_KEV:
+		fprintf(fh, "photon_energy = %s keV\n", dtempl->wavelength_from);
+		break;
+
+		case WAVELENGTH_PHOTON_EV:
+		fprintf(fh, "photon_energy = %s eV\n", dtempl->wavelength_from);
+		break;
+
+		default:
+		ERROR("Unknown wavelength unit (%i)\n", dtempl->wavelength_unit);
+		return 1;
+
+	}
+
+	fprintf(fh, "clen = %s\n", dtempl->cnz_from);
+
+	if ( dtempl->peak_list != NULL ) {
+		fprintf(fh, "peak_list = %s\n", dtempl->peak_list);
+	}
+	switch ( dtempl->peak_list_type ) {
+		case PEAK_LIST_AUTO:
+		break;
+
+		case PEAK_LIST_CXI:
+		fprintf(fh, "peak_list_type = cxi\n");
+		break;
+
+		case PEAK_LIST_LIST3:
+		fprintf(fh, "peak_list_type = list3\n");
+		break;
+
+		default:
+		ERROR("Unknown peak list type (%i)\n", dtempl->peak_list_type);
+		return 1;
+	}
+
+	fprintf(fh, "bandwidth = %e\n", dtempl->bandwidth);
+
+	if ( dtempl->shift_x_from != NULL ) {
+		fprintf(fh, "detector_shift_x = %s\n", dtempl->shift_x_from);
+	}
+	if ( dtempl->shift_y_from != NULL ) {
+		fprintf(fh, "detector_shift_y = %s\n", dtempl->shift_y_from);
+	}
+
+	/* Other top-levels */
+	int cnz_offset_done = 0;
+	int mask_done[MAX_MASKS] = {0};
+	int satmap_done = 0;
+	int satmap_file_done = 0;
+	int mask_edge_pixels_done = 0;
+	int pixel_pitch_done = 0;
+	int adu_scale_done = 0;
+	int max_adu_done = 0;
+	int flag_values_done = 0;
+	int data_done = 0;
+	int dims_done[MAX_DIMS] = {0};
+	for ( i=0; i<dtempl->n_panels; i++ ) {
+
+		const struct panel_template *p = &dtempl->panels[i];
+		int j;
+
+		if ( p->cnz_offset_default && !cnz_offset_done ) {
+			fprintf(fh, "coffset = %f\n", p->cnz_offset);
+			cnz_offset_done = 1;
+		}
+
+		for ( j=0; j<MAX_MASKS; j++ ) {
+			if ( p->masks[j].data_location == NULL ) continue;
+			if ( !p->masks[j].mask_default ) continue;
+			if ( mask_done[j] ) continue;
+			fprintf(fh, "mask%i_data = %s\n",
+			        j, p->masks[j].data_location);
+			if ( p->masks[j].filename != NULL ) {
+				fprintf(fh, "mask%i_filename = %s\n",
+				        j, p->masks[j].filename);
+			}
+			fprintf(fh, "mask%i_goodbits = 0x%x\n",
+			        j, p->masks[j].good_bits);
+			fprintf(fh, "mask%i_badbits = 0x%x\n",
+			        j, p->masks[j].bad_bits);
+			mask_done[j] = 1;
+		}
+
+		if ( p->satmap_default && !satmap_done && (p->satmap != NULL) ) {
+			fprintf(fh, "saturation_map = %s\n", p->satmap);
+			satmap_done = 1;
+		}
+
+		if ( p->satmap_file_default && !satmap_file_done && (p->satmap_file != NULL) ) {
+			fprintf(fh, "saturation_map_file = %s\n", p->satmap);
+			satmap_file_done = 1;
+		}
+
+		if ( p->mask_edge_pixels_default && !mask_edge_pixels_done && (p->mask_edge_pixels != 0) ) {
+			fprintf(fh, "mask_edge_pixels = %i\n", p->mask_edge_pixels);
+			mask_edge_pixels_done = 1;
+		}
+
+		if ( p->pixel_pitch_default && !pixel_pitch_done ) {
+			fprintf(fh, "res = %f\n", 1.0/p->pixel_pitch);
+			pixel_pitch_done = 1;
+		}
+
+		if ( p->max_adu_default && !max_adu_done && !isinf(p->max_adu) ) {
+			fprintf(fh, "max_adu = %f\n", p->max_adu);
+			max_adu_done = 1;
+		}
+
+		if ( p->data_default && !data_done ) {
+			fprintf(fh, "data = %s\n", p->data);
+			data_done = 1;
+		}
+
+		if ( p->flag_values_default && !flag_values_done ) {
+			for ( j=0; j<MAX_FLAG_VALUES; j++ ) {
+				switch ( p->flag_types[j] ) {
+					case FLAG_NOTHING :
+					break;
+
+					case FLAG_EQUAL:
+					fprintf(fh, "flag_equal = %i\n",
+					        p->flag_values[j]);
+					break;
+
+					case FLAG_MORETHAN:
+					fprintf(fh, "flag_morethan = %i\n",
+					        p->flag_values[j]);
+					break;
+
+					case FLAG_LESSTHAN:
+					fprintf(fh, "flag_lessthan = %i\n",
+					        p->flag_values[j]);
+					break;
+				}
+			}
+			flag_values_done = 1;
+		}
+
+		if ( p->adu_scale_default && !adu_scale_done ) {
+			switch ( p->adu_scale_unit ) {
+
+				case ADU_PER_EV:
+				fprintf(fh, "adu_per_eV = %f\n", p->adu_scale);
+				break;
+
+				case ADU_PER_PHOTON:
+				fprintf(fh, "adu_per_photon = %f\n", p->adu_scale);
+				break;
+			}
+			adu_scale_done = 1;
+		}
+
+		for ( j=0; j<MAX_DIMS; j++ ) {
+			if ( p->dims_default[j] && !dims_done[j] && p->dims[j] != DIM_UNDEFINED ) {
+				if ( p->dims[j] < 0 ) {
+					fprintf(fh, "dim%i = %s\n", j, str_dim(p->dims[j]));
+				} else {
+					fprintf(fh, "dim%i = %i\n", j, p->dims[j]);
+				}
+				dims_done[j] = 1;
+			}
+		}
+	}
+
+	fprintf(fh, "\n");
+
+	/* Bad regions */
+	for ( i=0; i<dtempl->n_bad; i++ ) {
+		const struct dt_badregion *bad = &dtempl->bad[i];
+		if ( bad->is_fsss ) {
+			fprintf(fh, "bad_%s/panel = %s\n", bad->name, bad->panel_name);
+			fprintf(fh, "bad_%s/min_fs = %i\n", bad->name, bad->min_fs);
+			fprintf(fh, "bad_%s/max_fs = %i\n", bad->name, bad->max_fs);
+			fprintf(fh, "bad_%s/min_ss = %i\n", bad->name, bad->min_ss);
+			fprintf(fh, "bad_%s/max_ss = %i\n", bad->name, bad->max_ss);
+		} else {
+			fprintf(fh, "bad_%s/min_x = %f\n", bad->name, bad->min_x);
+			fprintf(fh, "bad_%s/max_x = %f\n", bad->name, bad->max_x);
+			fprintf(fh, "bad_%s/min_y = %f\n", bad->name, bad->min_y);
+			fprintf(fh, "bad_%s/max_y = %f\n", bad->name, bad->max_y);
+		}
+		fprintf(fh, "\n");
+	}
+
+	/* Panels */
+	for ( i=0; i<dtempl->n_panels; i++ ) {
+
+		int j;
+		const struct panel_template *p = &dtempl->panels[i];
+
+		fprintf(fh, "%s/min_fs = %i\n", p->name, p->orig_min_fs);
+		fprintf(fh, "%s/max_fs = %i\n", p->name, p->orig_max_fs);
+		fprintf(fh, "%s/min_ss = %i\n", p->name, p->orig_min_ss);
+		fprintf(fh, "%s/max_ss = %i\n", p->name, p->orig_max_ss);
+		fprintf(fh, "%s/corner_x = %f\n", p->name, p->cnx);
+		fprintf(fh, "%s/corner_y = %f\n", p->name, p->cny);
+		fprintf(fh, "%s/fs = %fx %+fy %+fz\n", p->name,
+		        p->fsx, p->fsy, p->fsz);
+		fprintf(fh, "%s/ss = %fx %+fy %+fz\n", p->name,
+		        p->ssx, p->ssy, p->ssz);
+
+		if ( !p->cnz_offset_default ) {
+			fprintf(fh, "%s/coffset = %f\n", p->name, p->cnz_offset);
+		}
+
+		for ( j=0; j<MAX_MASKS; j++ ) {
+			if ( p->masks[j].data_location == NULL ) continue;
+			if ( p->masks[j].mask_default ) continue;
+			fprintf(fh, "%s/mask%i_data = %s\n",
+			        p->name, j, p->masks[j].data_location);
+			if ( p->masks[j].filename != NULL ) {
+				fprintf(fh, "%smask%i_filename = %s\n",
+				        p->name, j, p->masks[j].filename);
+			}
+			fprintf(fh, "%s/mask%i_goodbits = 0x%x\n",
+			        p->name, j, p->masks[j].good_bits);
+			fprintf(fh, "%s/mask%i_badbits = 0x%x\n",
+			        p->name, j, p->masks[j].bad_bits);
+		}
+
+		if ( !p->satmap_default && (p->satmap != NULL) ) {
+			fprintf(fh, "%s/saturation_map = %s\n", p->name, p->satmap);
+		}
+
+		if ( !p->satmap_file_default && (p->satmap_file != NULL) ) {
+			fprintf(fh, "%s/saturation_map_file = %s\n", p->name, p->satmap_file);
+		}
+
+		if ( !p->mask_edge_pixels_default && (p->mask_edge_pixels != 0) ) {
+			fprintf(fh, "%s/mask_edge_pixels = %i\n", p->name, p->mask_edge_pixels);
+		}
+
+		if ( !p->pixel_pitch_default ) {
+			fprintf(fh, "%s/res = %f\n", p->name, 1.0/p->pixel_pitch);
+		}
+
+		if ( !p->adu_scale_default ) {
+			switch ( p->adu_scale_unit ) {
+
+				case ADU_PER_EV:
+				fprintf(fh, "%s/adu_per_eV = %f\n", p->name, p->adu_scale);
+				break;
+
+				case ADU_PER_PHOTON:
+				fprintf(fh, "%s/adu_per_photon = %f\n", p->name, p->adu_scale);
+				break;
+			}
+		}
+
+		if ( !p->max_adu_default ) {
+			fprintf(fh, "%s/max_adu = %f\n", p->name, p->max_adu);
+		}
+
+		if ( !p->flag_values_default ) {
+			for ( j=0; j<MAX_FLAG_VALUES; j++ ) {
+				switch ( p->flag_types[j] ) {
+					case FLAG_NOTHING :
+					break;
+
+					case FLAG_EQUAL:
+					fprintf(fh, "%s/flag_equal = %i\n",
+					        p->name, p->flag_values[j]);
+					break;
+
+					case FLAG_MORETHAN:
+					fprintf(fh, "%s/flag_morethan = %i\n",
+					        p->name, p->flag_values[j]);
+					break;
+
+					case FLAG_LESSTHAN:
+					fprintf(fh, "%s/flag_lessthan = %i\n",
+					        p->name, p->flag_values[j]);
+					break;
+				}
+			}
+		}
+
+		if ( !p->data_default ) {
+			fprintf(fh, "%s/data = %s\n", p->name, p->data);
+		}
+
+		for ( j=0; j<MAX_DIMS; j++ ) {
+			if ( !p->dims_default[j] && (p->dims[j] != DIM_UNDEFINED) ) {
+				if ( p->dims[j] < 0 ) {
+					fprintf(fh, "%s/dim%i = %s\n", p->name, j, str_dim(p->dims[j]));
+				} else {
+					fprintf(fh, "%s/dim%i = %i\n", p->name, j, p->dims[j]);
+				}
+				dims_done[j] = 1;
+			}
+		}
+
+		if ( p->bad ) {
+			fprintf(fh, "%s/no_index = 1\n", p->name);
+		}
+
+		fprintf(fh, "\n");
+	}
+
+	/* Groups */
+	for ( i=0; i<dtempl->n_groups; i++ ) {
+		int j;
+		if ( dtempl->groups[i]->n_children == 0 ) continue;
+		fprintf(fh, "group_%s = ", dtempl->groups[i]->name);
+		for ( j=0; j<dtempl->groups[i]->n_children; j++ ) {
+			if ( j > 0 ) fprintf(fh, ",");
+			fprintf(fh, "%s", dtempl->groups[i]->children[j]->name);
+		}
+		fprintf(fh, "\n");
+	}
+
+	fclose(fh);
+	return 0;
+}
+
+
+static void add_group_info(struct dg_group_info *ginfo, int *ppos,
+                           struct panel_group_template *group,
+                           int serial, int level, int c_mul)
+{
+	int j;
+	int i = *ppos;
+	(*ppos)++;
+
+	ginfo[i].name = group->name;
+	ginfo[i].serial = serial;
+	ginfo[i].hierarchy_level = level;
+
+	for ( j=0; j<group->n_children; j++ ) {
+		add_group_info(ginfo, ppos, group->children[j],
+		               serial+c_mul*(j+1), level+1, c_mul*100);
+	}
+}
+
+
+struct dg_group_info *data_template_group_info(const DataTemplate *dtempl, int *n)
+{
+	struct dg_group_info *ginfo;
+	int i;
+	struct panel_group_template *group;
+
+	ginfo = malloc(sizeof(struct dg_group_info)*dtempl->n_groups);
+	if ( ginfo == NULL ) return NULL;
+
+	group = find_group(dtempl, "all");
+	i = 0;
+	add_group_info(ginfo, &i, group, 0, 0, 100);
+
+	*n = dtempl->n_groups;
+	return ginfo;
+}
diff --git a/libcrystfel/src/datatemplate.h b/libcrystfel/src/datatemplate.h
index fea39ccd..07f2387d 100644
--- a/libcrystfel/src/datatemplate.h
+++ b/libcrystfel/src/datatemplate.h
@@ -43,25 +43,17 @@
 typedef struct _datatemplate DataTemplate;
 
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-struct rigid_group
+struct dg_group_info
 {
-	char *name;
-	int *panel_numbers;
-	int n_panels;
+	const char *name;
+	int serial;
+	int hierarchy_level;
 };
 
 
-struct rg_collection
-{
-	char *name;
-	struct rigid_group **rigid_groups;
-	int n_rigid_groups;
-};
-
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 extern DataTemplate *data_template_new_from_file(const char *filename);
 extern DataTemplate *data_template_new_from_string(const char *string_in);
@@ -91,15 +83,31 @@ extern void data_template_add_copy_header(DataTemplate *dt,
 
 extern int data_template_get_slab_extents(const DataTemplate *dt, int *pw, int *ph);
 
-extern struct rg_collection *data_template_get_rigid_groups(const DataTemplate *dtempl,
-                                                            const char *collection_name);
-
 extern double data_template_get_wavelength_if_possible(const DataTemplate *dt);
 
 extern double data_template_get_clen_if_possible(const DataTemplate *dt);
 
 extern struct detgeom *data_template_get_2d_detgeom_if_possible(const DataTemplate *dt);
 
+extern void data_template_show_hierarchy(const DataTemplate *dtempl);
+
+extern int data_template_translate_group_px(DataTemplate *dtempl,
+                                            const char *group_name,
+                                            double x, double y, double z);
+
+extern int data_template_translate_group_m(DataTemplate *dtempl,
+                                           const char *group_name,
+                                           double x, double y, double z);
+
+extern int data_template_rotate_group(DataTemplate *dtempl,
+                                      const char *group_name,
+                                      double ang, char axis);
+
+extern int data_template_write_to_file(const DataTemplate *dtempl,
+                                       const char *filename);
+
+extern struct dg_group_info *data_template_group_info(const DataTemplate *dtempl, int *n);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/libcrystfel/src/datatemplate_priv.h b/libcrystfel/src/datatemplate_priv.h
index 657d5094..ab40ac2f 100644
--- a/libcrystfel/src/datatemplate_priv.h
+++ b/libcrystfel/src/datatemplate_priv.h
@@ -40,6 +40,12 @@
 /* Maximum number of placeholders expected in path structure */
 #define MAX_PATH_PARTS (16)
 
+/* Maximum number of panel groups */
+#define MAX_PANEL_GROUPS (256)
+
+/* Maximum number of panel groups that can derive from one panel */
+#define MAX_PANEL_GROUP_CHILDREN (64)
+
 enum adu_per_unit
 {
 	ADU_PER_PHOTON,
@@ -102,6 +108,9 @@ struct mask_template
 	/** Bit mask for good pixels
 	 * (pixel cannot be good unless all of these are set) */
 	unsigned int good_bits;
+
+	/** If non-zero, this mask came from the top level */
+	int mask_default;
 };
 
 
@@ -119,46 +128,53 @@ struct panel_template
 	double cny;
 	/**@}*/
 
-	/** Location to get cnz from, e.g. from HDF5 file */
-	char *cnz_from;
-
 	/** The offset to be applied from clen */
 	double cnz_offset;
+	int cnz_offset_default;
 
 	/** Mask definitions */
 	struct mask_template masks[MAX_MASKS];
 
 	/** Location of per-pixel saturation map */
 	char *satmap;
+	int satmap_default;
 
 	/** Filename for saturation map */
 	char *satmap_file;
+	int satmap_file_default;
 
 	/** Mark entire panel as bad if set */
 	int bad;
 
 	/** Mark this number of edge rows as bad */
 	int mask_edge_pixels;
+	int mask_edge_pixels_default;
 
 	/** Pixel size in metres */
 	double pixel_pitch;
+	int pixel_pitch_default;
 
 	/** Number of detector intensity units per photon, or eV */
 	double adu_scale;
 	enum adu_per_unit adu_scale_unit;
+	int adu_scale_default;
 
 	/** Treat pixel as unreliable if higher than this */
 	double max_adu;
+	int max_adu_default;
 
 	/** Pixels with exactly this value will be marked as bad */
 	enum flag_value_type flag_types[MAX_FLAG_VALUES];
 	signed int flag_values[MAX_FLAG_VALUES];
+	int flag_values_default;
 
 	/** Location of data in file (possibly with placeholders) */
 	char *data;
+	int data_default;
 
 	/** Dimensions (see definitions for DIM_FS etc above) */
 	signed int dims[MAX_DIMS];
+	int dims_default[MAX_DIMS];
 
 	/** \name Transformation matrix from pixel coordinates to lab frame */
 	/*@{*/
@@ -205,6 +221,14 @@ struct dt_badregion
 };
 
 
+struct panel_group_template
+{
+	char *name;
+	int n_children;
+	struct panel_group_template *children[MAX_PANEL_GROUP_CHILDREN];
+};
+
+
 struct _datatemplate
 {
 	struct panel_template     *panels;
@@ -218,11 +242,8 @@ struct _datatemplate
 
 	double                     bandwidth;
 
-	struct rigid_group       **rigid_groups;
-	int                        n_rigid_groups;
-
-	struct rg_collection     **rigid_group_collections;
-	int                        n_rg_collections;
+	struct panel_group_template *groups[MAX_PANEL_GROUPS];
+	int                          n_groups;
 
 	char                      *peak_list;
 	enum peak_layout           peak_list_type;
@@ -231,6 +252,9 @@ struct _datatemplate
 	char                      *shift_x_from;
 	char                      *shift_y_from;
 
+	/** Location to get detector z from, e.g. from HDF5 file */
+	char                      *cnz_from;
+
 	char                      *headers_to_copy[MAX_COPY_HEADERS];
 	int                        n_headers_to_copy;
 };
diff --git a/libcrystfel/src/detgeom.c b/libcrystfel/src/detgeom.c
index 58e52f01..b4835317 100644
--- a/libcrystfel/src/detgeom.c
+++ b/libcrystfel/src/detgeom.c
@@ -29,6 +29,9 @@
 #include <libcrystfel-config.h>
 #include <math.h>
 #include <stdlib.h>
+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_blas.h>
+#include <gsl/gsl_linalg.h>
 
 #include "detgeom.h"
 #include "utils.h"
@@ -61,6 +64,22 @@ void detgeom_transform_coords(struct detgeom_panel *p,
 }
 
 
+static void free_group(struct detgeom_panel_group *g)
+{
+	int i;
+
+	if ( g == NULL ) return;
+
+	for ( i=0; i<g->n_children; i++ ) {
+		free_group(g->children[i]);
+	}
+
+	free(g->name);
+	free(g->children);
+	free(g);
+}
+
+
 void detgeom_free(struct detgeom *detgeom)
 {
 	int i;
@@ -71,6 +90,7 @@ void detgeom_free(struct detgeom *detgeom)
 		free(detgeom->panels[i].name);
 	}
 
+	free_group(detgeom->top_group);
 	free(detgeom->panels);
 	free(detgeom);
 }
@@ -174,3 +194,146 @@ double detgeom_mean_camera_length(struct detgeom *dg)
 
 	return mean;
 }
+
+
+struct detgeom_panel *detgeom_find_panel(struct detgeom *dg, const char *name)
+{
+	int i;
+	for ( i=0; i<dg->n_panels; i++ ) {
+		if ( strcmp(dg->panels[i].name, name) == 0 ) {
+			return &dg->panels[i];
+		}
+	}
+	return NULL;
+}
+
+
+static void detgeom_show_group(const struct detgeom_panel_group *group, int level)
+{
+	int i;
+
+	for ( i=0; i<level; i++ ) STATUS("  ");
+
+	if ( group == NULL ) {
+		STATUS("!!!\n");
+		return;
+	}
+
+	STATUS("%s (serial %i)\n", group->name, group->serial);
+
+	for ( i=0; i<group->n_children; i++ ) {
+		detgeom_show_group(group->children[i], level+1);
+	}
+}
+
+
+void detgeom_show_hierarchy(const struct detgeom *dg)
+{
+	detgeom_show_group(dg->top_group, 0);
+}
+
+
+void detgeom_translate_detector_m(struct detgeom *dg, double x, double y, double z)
+{
+	int i;
+	for ( i=0; i<dg->n_panels; i++ ) {
+		struct detgeom_panel *p = &dg->panels[i];
+		p->cnx += x / p->pixel_pitch;
+		p->cny += y / p->pixel_pitch;
+		p->cnz += z / p->pixel_pitch;
+	}
+}
+
+
+gsl_matrix **make_panel_minvs(struct detgeom *dg)
+{
+	int i;
+	gsl_matrix **Minvs;
+
+	Minvs = malloc(dg->n_panels * sizeof(gsl_matrix *));
+	if ( Minvs == NULL ) return NULL;
+
+	for ( i=0; i<dg->n_panels; i++ ) {
+
+		struct detgeom_panel *p = &dg->panels[i];
+		gsl_matrix *M = gsl_matrix_calloc(3, 3);
+
+		gsl_matrix_set(M, 0, 0, p->pixel_pitch*p->cnx);
+		gsl_matrix_set(M, 0, 1, p->pixel_pitch*p->fsx);
+		gsl_matrix_set(M, 0, 2, p->pixel_pitch*p->ssx);
+		gsl_matrix_set(M, 1, 0, p->pixel_pitch*p->cny);
+		gsl_matrix_set(M, 1, 1, p->pixel_pitch*p->fsy);
+		gsl_matrix_set(M, 1, 2, p->pixel_pitch*p->ssy);
+		gsl_matrix_set(M, 2, 0, p->pixel_pitch*p->cnz);
+		gsl_matrix_set(M, 2, 1, p->pixel_pitch*p->fsz);
+		gsl_matrix_set(M, 2, 2, p->pixel_pitch*p->ssz);
+
+		Minvs[i] = matrix_invert(M);
+		if ( Minvs[i] == NULL ) {
+			ERROR("Failed to calculate inverse panel matrix for %s\n",
+			      p->name);
+			return NULL;
+		}
+
+	}
+
+	return Minvs;
+}
+
+
+static void add_point(const struct detgeom_panel *p,
+                      int fs, int ss,
+                      double *tx, double *ty, double *tz)
+{
+	*tx += (p->cnx + fs*p->fsx + ss*p->ssx) * p->pixel_pitch;
+	*ty += (p->cny + fs*p->fsy + ss*p->ssy) * p->pixel_pitch;
+	*tz += (p->cnz + fs*p->fsz + ss*p->ssz) * p->pixel_pitch;
+}
+
+
+int detgeom_group_center(const struct detgeom_panel_group *grp,
+                         double *x, double *y, double *z)
+{
+	if ( grp->n_children == 0 ) {
+
+		const struct detgeom_panel *p = grp->panel;
+		if ( p == NULL ) return 1;
+
+		double tx = 0.0;
+		double ty = 0.0;
+		double tz = 0.0;
+
+		add_point(p, 0, 0, &tx, &ty, &tz);
+		add_point(p, p->w, 0, &tx, &ty, &tz);
+		add_point(p, 0, p->h, &tx, &ty, &tz);
+		add_point(p, p->w, p->h, &tx, &ty, &tz);
+
+		*x = tx / 4.0;
+		*y = ty / 4.0;
+		*z = tz / 4.0;
+
+		return 0;
+
+	} else {
+
+		int i;
+		double tx = 0.0;
+		double ty = 0.0;
+		double tz = 0.0;
+
+		for ( i=0; i<grp->n_children; i++ ) {
+			double gcx, gcy, gcz;
+			detgeom_group_center(grp->children[i], &gcx, &gcy, &gcz);
+			tx += gcx;
+			ty += gcy;
+			tz += gcz;
+		}
+
+		*x = tx / grp->n_children;
+		*y = ty / grp->n_children;
+		*z = tz / grp->n_children;
+
+		return 0;
+
+	}
+}
diff --git a/libcrystfel/src/detgeom.h b/libcrystfel/src/detgeom.h
index 7f291a7a..53877b2e 100644
--- a/libcrystfel/src/detgeom.h
+++ b/libcrystfel/src/detgeom.h
@@ -42,6 +42,7 @@ extern "C" {
  * Detector geometry structure and related functions.
  */
 
+#include <gsl/gsl_matrix.h>
 
 /**
  * Represents one panel of a detector
@@ -83,15 +84,43 @@ struct detgeom_panel
 	int         w;
 	int         h;
 	/*@}*/
+
+	/** \name Leaf group containing this panel (only) */
+	const struct detgeom_panel_group *group;
+};
+
+
+struct detgeom_panel_group
+{
+	char *name;
+	int n_children;
+
+	struct detgeom_panel_group *parent;
+	int serial;
+
+	/* Center of panel group, in lab coordinate system (metres)
+	 * This will be the rotation center. */
+	double cx;
+	double cy;
+	double cz;
+
+	/* If n_children > 0, here are the child groups */
+	struct detgeom_panel_group **children;
+
+	/* If n_children == 0, this is a leaf node, so: */
+	struct detgeom_panel *panel;
 };
 
 
 struct detgeom
 {
 	struct detgeom_panel *panels;
-	int                   n_panels;
+	int n_panels;
+
+	struct detgeom_panel_group *top_group;
 };
 
+
 extern void detgeom_transform_coords(struct detgeom_panel *p,
                                      double fs, double ss,
                                      double wavelength,
@@ -107,6 +136,17 @@ extern void show_panel(struct detgeom_panel *p);
 
 extern double detgeom_mean_camera_length(struct detgeom *dg);
 
+extern struct detgeom_panel *detgeom_find_panel(struct detgeom *dg, const char *name);
+
+extern void detgeom_show_hierarchy(const struct detgeom *dg);
+
+extern void detgeom_translate_detector_m(struct detgeom *dg, double x, double y, double z);
+
+extern int detgeom_group_center(const struct detgeom_panel_group *grp,
+                                double *x, double *y, double *z);
+
+extern gsl_matrix **make_panel_minvs(struct detgeom *dg);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/libcrystfel/src/geometry.c b/libcrystfel/src/geometry.c
index e454b73d..674fe4d0 100644
--- a/libcrystfel/src/geometry.c
+++ b/libcrystfel/src/geometry.c
@@ -53,17 +53,11 @@ static int locate_peak_on_panel(double x, double y, double z, double k,
                                 double det_shift_x, double det_shift_y,
                                 double *pfs, double *pss)
 {
-	double ctt, tta, phi;
 	gsl_vector *v;
 	gsl_vector *t;
 	gsl_matrix *M;
 	double fs, ss, one_over_mu;
 
-	/* Calculate 2theta (scattering angle) and azimuth (phi) */
-	tta = atan2(sqrt(x*x+y*y), k+z);
-	ctt = cos(tta);
-	phi = atan2(y, x);
-
 	/* Set up matrix equation */
 	M = gsl_matrix_alloc(3, 3);
 	v = gsl_vector_alloc(3);
@@ -73,9 +67,9 @@ static int locate_peak_on_panel(double x, double y, double z, double k,
 		return 0;
 	}
 
-	gsl_vector_set(t, 0, sin(tta)*cos(phi));
-	gsl_vector_set(t, 1, sin(tta)*sin(phi));
-	gsl_vector_set(t, 2, ctt);
+	gsl_vector_set(t, 0, x);
+	gsl_vector_set(t, 1, y);
+	gsl_vector_set(t, 2, k+z);
 
 	gsl_matrix_set(M, 0, 0, p->cnx+(det_shift_x/p->pixel_pitch));
 	gsl_matrix_set(M, 0, 1, p->fsx);
@@ -429,69 +423,6 @@ static Reflection *check_reflection(struct image *image, Crystal *cryst,
 }
 
 
-double r_gradient(UnitCell *cell, int k, Reflection *refl, struct image *image)
-{
-	double asx, asy, asz;
-	double bsx, bsy, bsz;
-	double csx, csy, csz;
-	double xl, yl, zl;
-	signed int hs, ks, ls;
-	double tl, phi, azi;
-
-	get_symmetric_indices(refl, &hs, &ks, &ls);
-
-	cell_get_reciprocal(cell, &asx, &asy, &asz,
-	                          &bsx, &bsy, &bsz,
-	                          &csx, &csy, &csz);
-	xl = hs*asx + ks*bsx + ls*csx;
-	yl = hs*asy + ks*bsy + ls*csy;
-	zl = hs*asz + ks*bsz + ls*csz;
-
-	tl = sqrt(xl*xl + yl*yl);
-	phi = angle_between_2d(tl, zl+1.0/image->lambda, 0.0, 1.0); /* 2theta */
-	azi = atan2(yl, xl); /* azimuth */
-
-	switch ( k ) {
-
-		case GPARAM_ASX :
-		return - hs * sin(phi) * cos(azi);
-
-		case GPARAM_BSX :
-		return - ks * sin(phi) * cos(azi);
-
-		case GPARAM_CSX :
-		return - ls * sin(phi) * cos(azi);
-
-		case GPARAM_ASY :
-		return - hs * sin(phi) * sin(azi);
-
-		case GPARAM_BSY :
-		return - ks * sin(phi) * sin(azi);
-
-		case GPARAM_CSY :
-		return - ls * sin(phi) * sin(azi);
-
-		case GPARAM_ASZ :
-		return - hs * cos(phi);
-
-		case GPARAM_BSZ :
-		return - ks * cos(phi);
-
-		case GPARAM_CSZ :
-		return - ls * cos(phi);
-
-		case GPARAM_DETX :
-		case GPARAM_DETY :
-		case GPARAM_CLEN :
-		return 0.0;
-
-	}
-
-	ERROR("No r gradient defined for parameter %i\n", k);
-	abort();
-}
-
-
 /**
  * \param cryst: A \ref Crystal
  * \param max_res: Maximum resolution to predict to (m^-1)
@@ -1084,125 +1015,3 @@ void polarisation_correction(RefList *list, UnitCell *cell,
 		set_esd_intensity(refl, sigma / pol);
 	}
 }
-
-
-/* Returns dx_h/dP, where P = any parameter */
-double x_gradient(int param, Reflection *refl, UnitCell *cell,
-                  struct detgeom_panel *p)
-{
-	signed int h, k, l;
-	double xl, zl, kpred;
-	double asx, asy, asz, bsx, bsy, bsz, csx, csy, csz;
-
-	get_indices(refl, &h, &k, &l);
-	kpred = get_kpred(refl);
-	cell_get_reciprocal(cell, &asx, &asy, &asz,
-	                          &bsx, &bsy, &bsz,
-	                          &csx, &csy, &csz);
-	xl = h*asx + k*bsx + l*csx;
-	zl = h*asz + k*bsz + l*csz;
-
-	switch ( param ) {
-
-		case GPARAM_ASX :
-		return h * p->cnz * p->pixel_pitch / (kpred + zl);
-
-		case GPARAM_BSX :
-		return k * p->cnz * p->pixel_pitch / (kpred + zl);
-
-		case GPARAM_CSX :
-		return l * p->cnz * p->pixel_pitch / (kpred + zl);
-
-		case GPARAM_ASY :
-		return 0.0;
-
-		case GPARAM_BSY :
-		return 0.0;
-
-		case GPARAM_CSY :
-		return 0.0;
-
-		case GPARAM_ASZ :
-		return -h * xl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
-
-		case GPARAM_BSZ :
-		return -k * xl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
-
-		case GPARAM_CSZ :
-		return -l * xl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
-
-		case GPARAM_DETX :
-		return -1;
-
-		case GPARAM_DETY :
-		return 0;
-
-		case GPARAM_CLEN :
-		return xl / (kpred+zl);
-
-	}
-
-	ERROR("Positional gradient requested for parameter %i?\n", param);
-	abort();
-}
-
-
-/* Returns dy_h/dP, where P = any parameter */
-double y_gradient(int param, Reflection *refl, UnitCell *cell,
-                  struct detgeom_panel *p)
-{
-	signed int h, k, l;
-	double yl, zl, kpred;
-	double asx, asy, asz, bsx, bsy, bsz, csx, csy, csz;
-
-	get_indices(refl, &h, &k, &l);
-	kpred = get_kpred(refl);
-	cell_get_reciprocal(cell, &asx, &asy, &asz,
-	                          &bsx, &bsy, &bsz,
-	                          &csx, &csy, &csz);
-	yl = h*asy + k*bsy + l*csy;
-	zl = h*asz + k*bsz + l*csz;
-
-	switch ( param ) {
-
-		case GPARAM_ASX :
-		return 0.0;
-
-		case GPARAM_BSX :
-		return 0.0;
-
-		case GPARAM_CSX :
-		return 0.0;
-
-		case GPARAM_ASY :
-		return h * p->cnz * p->pixel_pitch / (kpred + zl);
-
-		case GPARAM_BSY :
-		return k * p->cnz * p->pixel_pitch / (kpred + zl);
-
-		case GPARAM_CSY :
-		return l * p->cnz * p->pixel_pitch / (kpred + zl);
-
-		case GPARAM_ASZ :
-		return -h * yl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
-
-		case GPARAM_BSZ :
-		return -k * yl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
-
-		case GPARAM_CSZ :
-		return -l * yl * p->cnz * p->pixel_pitch / (kpred*kpred + 2.0*kpred*zl + zl*zl);
-
-		case GPARAM_DETX :
-		return 0;
-
-		case GPARAM_DETY :
-		return -1;
-
-		case GPARAM_CLEN :
-		return yl / (kpred+zl);
-
-	}
-
-	ERROR("Positional gradient requested for parameter %i?\n", param);
-	abort();
-}
diff --git a/libcrystfel/src/geometry.h b/libcrystfel/src/geometry.h
index 19c6a23a..d05c8832 100644
--- a/libcrystfel/src/geometry.h
+++ b/libcrystfel/src/geometry.h
@@ -8,7 +8,7 @@
  * Copyright © 2012 Richard Kirian
  *
  * Authors:
- *   2010-2020 Thomas White <taw@physics.org>
+ *   2010-2021 Thomas White <taw@physics.org>
  *   2012      Richard Kirian
  *
  * This file is part of CrystFEL.
@@ -62,32 +62,6 @@ typedef enum {
 } PartialityModel;
 
 
-/** Enumeration of parameters which may want to be refined */
-enum gparam {
-	GPARAM_ASX,
-	GPARAM_ASY,
-	GPARAM_ASZ,
-	GPARAM_BSX,
-	GPARAM_BSY,
-	GPARAM_BSZ,
-	GPARAM_CSX,
-	GPARAM_CSY,
-	GPARAM_CSZ,
-	GPARAM_R,
-	GPARAM_DIV,
-	GPARAM_DETX,
-	GPARAM_DETY,
-	GPARAM_CLEN,
-	GPARAM_OSF,   /* Linear scale factor */
-	GPARAM_BFAC,  /* D-W scale factor */
-	GPARAM_ANG1,  /* Out of plane rotation angles of crystal */
-	GPARAM_ANG2,
-	GPARAM_WAVELENGTH,
-
-	GPARAM_EOL    /* End of list */
-};
-
-
 /**
  * This structure represents the polarisation of the incident radiation
  */
@@ -107,8 +81,6 @@ extern RefList *predict_to_res(Crystal *cryst, double max_res);
 
 extern void calculate_partialities(Crystal *cryst, PartialityModel pmodel);
 
-extern double r_gradient(UnitCell *cell, int k, Reflection *refl,
-                         struct image *image);
 extern void update_predictions(Crystal *cryst);
 extern struct polarisation parse_polarisation(const char *text);
 extern void polarisation_correction(RefList *list, UnitCell *cell,
@@ -117,11 +89,6 @@ extern void polarisation_correction(RefList *list, UnitCell *cell,
 extern double sphere_fraction(double rlow, double rhigh, double pr);
 extern double gaussian_fraction(double rlow, double rhigh, double pr);
 
-extern double x_gradient(int param, Reflection *refl, UnitCell *cell,
-                         struct detgeom_panel *p);
-extern double y_gradient(int param, Reflection *refl, UnitCell *cell,
-                         struct detgeom_panel *p);
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/libcrystfel/src/index.c b/libcrystfel/src/index.c
index 02b01757..e3512b4a 100644
--- a/libcrystfel/src/index.c
+++ b/libcrystfel/src/index.c
@@ -591,7 +591,8 @@ static float real_time()
 
 /* Return non-zero for "success" */
 static int try_indexer(struct image *image, IndexingMethod indm,
-                       IndexingPrivate *ipriv, void *mpriv, char *last_task)
+                       IndexingPrivate *ipriv, void *mpriv, char *last_task,
+                       Mille *mille)
 {
 	int i, r;
 	int n_bad = 0;
@@ -719,7 +720,7 @@ static int try_indexer(struct image *image, IndexingMethod indm,
 		{
 			int r;
 			profile_start("refine");
-			r = refine_prediction(image, cr);
+			r = refine_prediction(image, cr, mille);
 			profile_end("refine");
 			if ( r ) {
 				crystal_set_user_flag(cr, 1);
@@ -920,19 +921,26 @@ static int finished_retry(IndexingMethod indm, IndexingFlags flags,
 
 void index_pattern(struct image *image, IndexingPrivate *ipriv)
 {
-	index_pattern_3(image, ipriv, NULL, NULL);
+	index_pattern_4(image, ipriv, NULL, NULL, NULL);
 }
 
 
 void index_pattern_2(struct image *image, IndexingPrivate *ipriv, int *ping)
 {
-	index_pattern_3(image, ipriv, ping, NULL);
+	index_pattern_4(image, ipriv, ping, NULL, NULL);
 }
 
 
 void index_pattern_3(struct image *image, IndexingPrivate *ipriv, int *ping,
                      char *last_task)
 {
+	index_pattern_4(image, ipriv, ping, last_task, NULL);
+}
+
+
+void index_pattern_4(struct image *image, IndexingPrivate *ipriv, int *ping,
+                     char *last_task, Mille *mille)
+{
 	int n = 0;
 	ImageFeatureList *orig;
 
@@ -982,7 +990,7 @@ void index_pattern_3(struct image *image, IndexingPrivate *ipriv, int *ping,
 
 			r = try_indexer(image, ipriv->methods[n],
 			                ipriv, ipriv->engine_private[n],
-			                last_task);
+			                last_task, mille);
 			success += r;
 			ntry++;
 			done = finished_retry(ipriv->methods[n], ipriv->flags,
diff --git a/libcrystfel/src/index.h b/libcrystfel/src/index.h
index 94018904..fa371276 100644
--- a/libcrystfel/src/index.h
+++ b/libcrystfel/src/index.h
@@ -3,13 +3,13 @@
  *
  * Perform indexing (somehow)
  *
- * Copyright © 2012-2021 Deutsches Elektronen-Synchrotron DESY,
+ * Copyright © 2012-2023 Deutsches Elektronen-Synchrotron DESY,
  *                       a research centre of the Helmholtz Association.
  * Copyright © 2012 Richard Kirian
  * Copyright © 2012 Lorenzo Galli
  *
  * Authors:
- *   2010-2021 Thomas White <taw@physics.org>
+ *   2010-2023 Thomas White <taw@physics.org>
  *   2010      Richard Kirian
  *   2012      Lorenzo Galli
  *   2015      Kenneth Beyerlein <kenneth.beyerlein@desy.de>
@@ -210,6 +210,7 @@ extern char *base_indexer_str(IndexingMethod indm);
 #include "cell.h"
 #include "image.h"
 #include "datatemplate.h"
+#include "predict-refine.h"
 
 extern struct argp felix_argp;
 extern struct argp pinkIndexer_argp;
@@ -251,6 +252,9 @@ extern void index_pattern_2(struct image *image, IndexingPrivate *ipriv,
 extern void index_pattern_3(struct image *image, IndexingPrivate *ipriv,
                             int *ping, char *last_task);
 
+extern void index_pattern_4(struct image *image, IndexingPrivate *ipriv,
+                            int *ping, char *last_task, Mille *mille);
+
 extern void cleanup_indexing(IndexingPrivate *ipriv);
 
 #ifdef __cplusplus
diff --git a/libcrystfel/src/predict-refine.c b/libcrystfel/src/predict-refine.c
index 19c689cd..43b54092 100644
--- a/libcrystfel/src/predict-refine.c
+++ b/libcrystfel/src/predict-refine.c
@@ -3,11 +3,11 @@
  *
  * Prediction refinement
  *
- * Copyright © 2012-2021 Deutsches Elektronen-Synchrotron DESY,
+ * Copyright © 2012-2023 Deutsches Elektronen-Synchrotron DESY,
  *                       a research centre of the Helmholtz Association.
  *
  * Authors:
- *   2010-2020 Thomas White <taw@physics.org>
+ *   2010-2023 Thomas White <taw@physics.org>
  *   2016      Valerio Mariani
  *
  * This file is part of CrystFEL.
@@ -33,91 +33,325 @@
 #include <assert.h>
 #include <gsl/gsl_matrix.h>
 #include <gsl/gsl_vector.h>
+#include <gsl/gsl_linalg.h>
 
 #include "image.h"
 #include "geometry.h"
 #include "cell-utils.h"
+#include "predict-refine.h"
+#include "profile.h"
+#include "crystfel-mille.h"
 
 
 /** \file predict-refine.h */
 
+double r_dev(struct reflpeak *rp)
+{
+	/* Excitation error term */
+	return get_exerr(rp->refl);
+}
 
-/* Maximum number of iterations of NLSq to do for each image per macrocycle. */
-#define MAX_CYCLES (10)
 
-/* Weighting of excitation error term (m^-1) compared to position term (m) */
-#define EXC_WEIGHT (4e-20)
+double fs_dev(struct reflpeak *rp, struct detgeom *det)
+{
+	double fsh, ssh;
+	get_detector_pos(rp->refl, &fsh, &ssh);
+	return fsh - rp->peak->fs;
+}
+
 
-/* Parameters to refine */
-static const enum gparam rv[] =
+double ss_dev(struct reflpeak *rp, struct detgeom *det)
 {
-	GPARAM_ASX,
-	GPARAM_ASY,
-	GPARAM_ASZ,
-	GPARAM_BSX,
-	GPARAM_BSY,
-	GPARAM_BSZ,
-	GPARAM_CSX,
-	GPARAM_CSY,
-	GPARAM_CSZ,
-	GPARAM_DETX,
-	GPARAM_DETY,
-};
-
-static const int num_params = 11;
-
-struct reflpeak {
-	Reflection *refl;
-	struct imagefeature *peak;
-	double Ih;   /* normalised */
-	struct detgeom_panel *panel;  /* panel the reflection appears on
-                                       * (we assume this never changes) */
-};
-
-
-static void twod_mapping(double fs, double ss, double *px, double *py,
-                         struct detgeom_panel *p, double dx, double dy)
+	double fsh, ssh;
+	get_detector_pos(rp->refl, &fsh, &ssh);
+	return ssh - rp->peak->ss;
+}
+
+
+double r_gradient(int param, Reflection *refl, UnitCell *cell, double wavelength)
 {
-	double xs, ys;
+	double asx, asy, asz;
+	double bsx, bsy, bsz;
+	double csx, csy, csz;
+	double xl, yl, zl;
+	signed int hs, ks, ls;
+	double tl, phi, azi;
+
+	get_symmetric_indices(refl, &hs, &ks, &ls);
+
+	cell_get_reciprocal(cell, &asx, &asy, &asz,
+	                          &bsx, &bsy, &bsz,
+	                          &csx, &csy, &csz);
+	xl = hs*asx + ks*bsx + ls*csx;
+	yl = hs*asy + ks*bsy + ls*csy;
+	zl = hs*asz + ks*bsz + ls*csz;
+
+	tl = sqrt(xl*xl + yl*yl);
+	phi = angle_between_2d(tl, zl+1.0/wavelength, 0.0, 1.0); /* 2theta */
+	azi = atan2(yl, xl); /* azimuth */
+
+	switch ( param ) {
+
+		case GPARAM_ASX :
+		return - hs * sin(phi) * cos(azi);
+
+		case GPARAM_BSX :
+		return - ks * sin(phi) * cos(azi);
+
+		case GPARAM_CSX :
+		return - ls * sin(phi) * cos(azi);
+
+		case GPARAM_ASY :
+		return - hs * sin(phi) * sin(azi);
 
-	xs = fs*p->fsx + ss*p->ssx;  /* pixels */
-	ys = fs*p->fsy + ss*p->ssy;  /* pixels */
+		case GPARAM_BSY :
+		return - ks * sin(phi) * sin(azi);
 
-	*px = (xs + p->cnx) * p->pixel_pitch + dx;  /* metres */
-	*py = (ys + p->cny) * p->pixel_pitch + dy;  /* metres */
+		case GPARAM_CSY :
+		return - ls * sin(phi) * sin(azi);
+
+		case GPARAM_ASZ :
+		return - hs * cos(phi);
+
+		case GPARAM_BSZ :
+		return - ks * cos(phi);
+
+		case GPARAM_CSZ :
+		return - ls * cos(phi);
+
+		/* Detector movements don't affect excitation error */
+		case GPARAM_DET_TX :
+		case GPARAM_DET_TY :
+		case GPARAM_DET_TZ :
+		case GPARAM_DET_RX :
+		case GPARAM_DET_RY :
+		case GPARAM_DET_RZ :
+		return 0.0;
+
+	}
+
+	ERROR("No r gradient defined for parameter %i\n", param);
+	abort();
 }
 
 
-static double r_dev(struct reflpeak *rp)
+/* Spot position gradients for diffraction physics (anything that changes the
+ * diffracted ray direction) */
+int fs_ss_gradient_physics(int param, Reflection *refl, UnitCell *cell,
+                         struct detgeom_panel *p, gsl_matrix *Minv,
+                         double fs, double ss, double mu,
+                         float *fsg, float *ssg)
 {
-	/* Excitation error term */
-	return get_exerr(rp->refl);
+	signed int h, k, l;
+	gsl_vector *dRdp;
+	gsl_vector *v;
+
+	get_indices(refl, &h, &k, &l);
+
+	dRdp = gsl_vector_calloc(3);
+
+	switch ( param ) {
+
+		case GPARAM_ASX :
+		gsl_vector_set(dRdp, 0, h);
+		break;
+
+		case GPARAM_BSX :
+		gsl_vector_set(dRdp, 0, k);
+		break;
+
+		case GPARAM_CSX :
+		gsl_vector_set(dRdp, 0, l);
+		break;
+
+		case GPARAM_ASY :
+		gsl_vector_set(dRdp, 1, h);
+		break;
+
+		case GPARAM_BSY :
+		gsl_vector_set(dRdp, 1, k);
+		break;
+
+		case GPARAM_CSY :
+		gsl_vector_set(dRdp, 1, l);
+		break;
+
+		case GPARAM_ASZ :
+		gsl_vector_set(dRdp, 2, h);
+		break;
+
+		case GPARAM_BSZ :
+		gsl_vector_set(dRdp, 2, k);
+		break;
+
+		case GPARAM_CSZ :
+		gsl_vector_set(dRdp, 2, l);
+		break;
+
+		default :
+		ERROR("Invalid physics gradient %i\n", param);
+		return 1;
+	}
+
+	v = gsl_vector_calloc(3);
+	gsl_blas_dgemv(CblasNoTrans, 1.0, Minv, dRdp, 0.0, v);
+
+	*fsg = mu*(gsl_vector_get(v, 1) - fs*gsl_vector_get(v, 0));
+	*ssg = mu*(gsl_vector_get(v, 2) - ss*gsl_vector_get(v, 0));
+
+	gsl_vector_free(v);
+	gsl_vector_free(dRdp);
+
+	return 0;
 }
 
 
-static double x_dev(struct reflpeak *rp, struct detgeom *det,
-                    double dx, double dy)
+/* Spot position gradients for panel motions (translation or rotation) */
+int fs_ss_gradient_panel(int param, Reflection *refl, UnitCell *cell,
+                         struct detgeom_panel *p, gsl_matrix *Minv,
+                         double fs, double ss, double mu,
+                         gsl_vector *t, double cx, double cy, double cz,
+                         float *fsg, float *ssg)
 {
-	/* Peak position term */
-	double xpk, ypk, xh, yh;
-	double fsh, ssh;
-	twod_mapping(rp->peak->fs, rp->peak->ss, &xpk, &ypk, rp->panel, dx, dy);
-	get_detector_pos(rp->refl, &fsh, &ssh);
-	twod_mapping(fsh, ssh, &xh, &yh, rp->panel, dx, dy);
-	return xh-xpk;
+	gsl_vector *v;
+	gsl_matrix *gM;  /* M^-1 * dM/dx * M^-1 */
+	gsl_matrix *dMdp = gsl_matrix_calloc(3, 3);
+
+	switch ( param ) {
+
+		case GPARAM_DET_TX :
+		gsl_matrix_set(dMdp, 0, 0, 1.0);
+		break;
+
+		case GPARAM_DET_TY :
+		gsl_matrix_set(dMdp, 1, 0, 1.0);
+		break;
+
+		case GPARAM_DET_TZ :
+		gsl_matrix_set(dMdp, 2, 0, 1.0);
+		break;
+
+		case GPARAM_DET_RX :
+		gsl_matrix_set(dMdp, 1, 0, cz-p->pixel_pitch*p->cnz);
+		gsl_matrix_set(dMdp, 2, 0, p->pixel_pitch*p->cny-cy);
+		gsl_matrix_set(dMdp, 1, 1, -p->pixel_pitch*p->fsz);
+		gsl_matrix_set(dMdp, 2, 1, p->pixel_pitch*p->fsy);
+		gsl_matrix_set(dMdp, 1, 2, -p->pixel_pitch*p->ssz);
+		gsl_matrix_set(dMdp, 2, 2, p->pixel_pitch*p->ssy);
+		break;
+
+		case GPARAM_DET_RY :
+		gsl_matrix_set(dMdp, 0, 0, p->pixel_pitch*p->cnz-cz);
+		gsl_matrix_set(dMdp, 2, 0, cx-p->pixel_pitch*p->cnx);
+		gsl_matrix_set(dMdp, 0, 1, p->pixel_pitch*p->fsz);
+		gsl_matrix_set(dMdp, 2, 1, -p->pixel_pitch*p->fsx);
+		gsl_matrix_set(dMdp, 0, 2, p->pixel_pitch*p->ssz);
+		gsl_matrix_set(dMdp, 2, 2, -p->pixel_pitch*p->ssx);
+		break;
+
+		case GPARAM_DET_RZ :
+		gsl_matrix_set(dMdp, 0, 0, cy-p->pixel_pitch*p->cny);
+		gsl_matrix_set(dMdp, 1, 0, p->pixel_pitch*p->cnx-cx);
+		gsl_matrix_set(dMdp, 0, 1, -p->pixel_pitch*p->fsy);
+		gsl_matrix_set(dMdp, 1, 1, p->pixel_pitch*p->fsx);
+		gsl_matrix_set(dMdp, 0, 2, -p->pixel_pitch*p->ssy);
+		gsl_matrix_set(dMdp, 1, 2, p->pixel_pitch*p->ssx);
+		break;
+
+		default:
+		ERROR("Invalid panel gradient %i\n", param);
+		return 1;
+
+	}
+
+	gM = matrix_mult3(Minv, dMdp, Minv);
+	gsl_matrix_free(dMdp);
+
+	v = gsl_vector_calloc(3);
+	gsl_blas_dgemv(CblasNoTrans, -1.0, gM, t, 0.0, v);
+	gsl_vector_free(t);
+	gsl_matrix_free(gM);
+
+	*fsg = mu*(gsl_vector_get(v, 1) - fs*gsl_vector_get(v, 0));
+	*ssg = mu*(gsl_vector_get(v, 2) - ss*gsl_vector_get(v, 0));
+
+	gsl_vector_free(v);
+
+	return 0;
 }
 
 
-static double y_dev(struct reflpeak *rp, struct detgeom *det,
-                    double dx, double dy)
+/* Returns the gradient of fs_dev and ss_dev w.r.t. any parameter.
+ * cx,cy,cz are the rotation axis coordinates (only 2 in use at any time)
+ * in metres (not pixels) */
+int fs_ss_gradient(int param, Reflection *refl, UnitCell *cell,
+                   struct detgeom_panel *p, gsl_matrix *Minv,
+                   double cx, double cy, double cz,
+                   float *fsg, float *ssg)
 {
-	/* Peak position term */
-	double xpk, ypk, xh, yh;
-	double fsh, ssh;
-	twod_mapping(rp->peak->fs, rp->peak->ss, &xpk, &ypk, rp->panel, dx, dy);
-	get_detector_pos(rp->refl, &fsh, &ssh);
-	twod_mapping(fsh, ssh, &xh, &yh, rp->panel, dx, dy);
-	return yh-ypk;
+	signed int h, k, l;
+	double xl, yl, zl, kpred;
+	double asx, asy, asz, bsx, bsy, bsz, csx, csy, csz;
+	gsl_vector *t;
+	gsl_vector *v;
+	gsl_matrix *M;
+	double mu;
+	double fs, ss;
+
+	get_indices(refl, &h, &k, &l);
+	kpred = get_kpred(refl);
+	cell_get_reciprocal(cell, &asx, &asy, &asz,
+	                          &bsx, &bsy, &bsz,
+	                          &csx, &csy, &csz);
+	xl = h*asx + k*bsx + l*csx;
+	yl = h*asy + k*bsy + l*csy;
+	zl = h*asz + k*bsz + l*csz;
+
+	/* Set up matrix equation */
+	M = gsl_matrix_alloc(3, 3);
+	v = gsl_vector_alloc(3);
+	t = gsl_vector_alloc(3);
+	if ( (M==NULL) || (v==NULL) || (t==NULL) ) {
+		ERROR("Failed to allocate vectors for gradient calculation\n");
+		return 1;
+	}
+
+	gsl_vector_set(t, 0, xl);
+	gsl_vector_set(t, 1, yl);
+	gsl_vector_set(t, 2, kpred+zl);
+
+	gsl_matrix_set(M, 0, 0, (p->cnx)*p->pixel_pitch);
+	gsl_matrix_set(M, 0, 1, (p->fsx)*p->pixel_pitch);
+	gsl_matrix_set(M, 0, 2, (p->ssx)*p->pixel_pitch);
+	gsl_matrix_set(M, 1, 0, (p->cny)*p->pixel_pitch);
+	gsl_matrix_set(M, 1, 1, (p->fsy)*p->pixel_pitch);
+	gsl_matrix_set(M, 1, 2, (p->ssy)*p->pixel_pitch);
+	gsl_matrix_set(M, 2, 0, (p->cnz)*p->pixel_pitch);
+	gsl_matrix_set(M, 2, 1, (p->fsz)*p->pixel_pitch);
+	gsl_matrix_set(M, 2, 2, (p->ssz)*p->pixel_pitch);
+
+	if ( gsl_linalg_HH_solve(M, t, v) ) {
+		ERROR("Failed to solve gradient equation\n");
+		return 1;
+	}
+	gsl_matrix_free(M);
+
+	mu = 1.0 / gsl_vector_get(v, 0);
+	fs = mu*gsl_vector_get(v, 1);
+	ss = mu*gsl_vector_get(v, 2);
+	gsl_vector_free(v);
+
+	if ( param <= GPARAM_CSZ ) {
+		gsl_vector_free(t);
+		return fs_ss_gradient_physics(param, refl, cell, p,
+		                              Minv, fs, ss, mu,
+		                              fsg, ssg);
+	} else {
+		return fs_ss_gradient_panel(param, refl, cell, p,
+		                            Minv, fs, ss, mu, t,
+		                            cx, cy, cz,
+		                            fsg, ssg);
+	}
 }
 
 
@@ -244,7 +478,6 @@ static int pair_peaks(struct image *image, Crystal *cr,
 
 		rps[n].refl = refl;
 		rps[n].peak = f;
-		rps[n].panel = &image->detgeom->panels[f->pn];
 		n++;
 
 	}
@@ -349,8 +582,7 @@ int refine_radius(Crystal *cr, struct image *image)
 
 
 static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
-                   struct image *image,
-                   double *total_x, double *total_y, double *total_z)
+                   struct image *image, gsl_matrix **Minvs)
 {
 	int i;
 	gsl_matrix *M;
@@ -359,6 +591,18 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 	double asx, asy, asz;
 	double bsx, bsy, bsz;
 	double csx, csy, csz;
+	const enum gparam rv[] = {
+		GPARAM_ASX,
+		GPARAM_ASY,
+		GPARAM_ASZ,
+		GPARAM_BSX,
+		GPARAM_BSY,
+		GPARAM_BSZ,
+		GPARAM_CSX,
+		GPARAM_CSY,
+		GPARAM_CSZ,
+	};
+	const int num_params = 9;
 
 	/* Number of parameters to refine */
 	M = gsl_matrix_calloc(num_params, num_params);
@@ -367,17 +611,21 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 	for ( i=0; i<n; i++ ) {
 
 		int k;
-		double gradients[num_params];
+		float fs_gradients[num_params];
+		float ss_gradients[num_params];
+		float r_gradients[num_params];
 		double w;
 
-		/* Excitation error terms */
-		w = EXC_WEIGHT * rps[i].Ih;
-
+		/* Calculate all gradients for this parameter */
 		for ( k=0; k<num_params; k++ ) {
-			gradients[k] = r_gradient(cell, rv[k], rps[i].refl,
-			                          image);
+			int pn = rps[i].peak->pn;
+			r_gradients[k] = r_gradient(rv[k], rps[i].refl, cell, image->lambda);
+			fs_ss_gradient(rv[k], rps[i].refl, cell, &image->detgeom->panels[pn],
+			               Minvs[pn], 0, 0, 0, &fs_gradients[k], &ss_gradients[k]);
 		}
 
+		/* Excitation error terms */
+		w = EXC_WEIGHT * rps[i].Ih;
 		for ( k=0; k<num_params; k++ ) {
 
 			int g;
@@ -390,7 +638,7 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 				/* Matrix is symmetric */
 				if ( g > k ) continue;
 
-				M_c = w * gradients[g] * gradients[k];
+				M_c = w * r_gradients[g] * r_gradients[k];
 				M_curr = gsl_matrix_get(M, k, g);
 				gsl_matrix_set(M, k, g, M_curr + M_c);
 				gsl_matrix_set(M, g, k, M_curr + M_c);
@@ -398,18 +646,13 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 			}
 
 			v_c = w * r_dev(&rps[i]);
-			v_c *= -gradients[k];
+			v_c *= -r_gradients[k];
 			v_curr = gsl_vector_get(v, k);
 			gsl_vector_set(v, k, v_curr + v_c);
 
 		}
 
-		/* Positional x terms */
-		for ( k=0; k<num_params; k++ ) {
-			gradients[k] = x_gradient(rv[k], rps[i].refl, cell,
-			                          rps[i].panel);
-		}
-
+		/* Positional fs terms */
 		for ( k=0; k<num_params; k++ ) {
 
 			int g;
@@ -422,26 +665,21 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 				/* Matrix is symmetric */
 				if ( g > k ) continue;
 
-				M_c = gradients[g] * gradients[k];
+				M_c = fs_gradients[g] * fs_gradients[k];
 				M_curr = gsl_matrix_get(M, k, g);
 				gsl_matrix_set(M, k, g, M_curr + M_c);
 				gsl_matrix_set(M, g, k, M_curr + M_c);
 
 			}
 
-			v_c = x_dev(&rps[i], image->detgeom, *total_x, *total_y);
-			v_c *= -gradients[k];
+			v_c = fs_dev(&rps[i], image->detgeom);
+			v_c *= -fs_gradients[k];
 			v_curr = gsl_vector_get(v, k);
 			gsl_vector_set(v, k, v_curr + v_c);
 
 		}
 
-		/* Positional y terms */
-		for ( k=0; k<num_params; k++ ) {
-			gradients[k] = y_gradient(rv[k], rps[i].refl, cell,
-			                          rps[i].panel);
-		}
-
+		/* Positional ss terms */
 		for ( k=0; k<num_params; k++ ) {
 
 			int g;
@@ -454,15 +692,15 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 				/* Matrix is symmetric */
 				if ( g > k ) continue;
 
-				M_c = gradients[g] * gradients[k];
+				M_c = ss_gradients[g] * ss_gradients[k];
 				M_curr = gsl_matrix_get(M, k, g);
 				gsl_matrix_set(M, k, g, M_curr + M_c);
 				gsl_matrix_set(M, g, k, M_curr + M_c);
 
 			}
 
-			v_c = y_dev(&rps[i], image->detgeom, *total_x, *total_y);
-			v_c *= -gradients[k];
+			v_c = ss_dev(&rps[i], image->detgeom);
+			v_c *= -ss_gradients[k];
 			v_curr = gsl_vector_get(v, k);
 			gsl_vector_set(v, k, v_curr + v_c);
 
@@ -472,14 +710,8 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 
 	int k;
 	for ( k=0; k<num_params; k++ ) {
-		double M_curr;
-		M_curr = gsl_matrix_get(M, k, k);
-		if ( (rv[k] == GPARAM_DETX) || (rv[k] == GPARAM_DETY) ) {
-			M_curr += 10.0;
-		} else {
-			M_curr += 1e-18;
-		}
-		gsl_matrix_set(M, k, k, M_curr);
+		double M_curr = gsl_matrix_get(M, k, k);
+		gsl_matrix_set(M, k, k, M_curr+1e-7);
 	}
 
 	//show_matrix_eqn(M, v);
@@ -513,9 +745,6 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 	csx += gsl_vector_get(shifts, 6);
 	csy += gsl_vector_get(shifts, 7);
 	csz += gsl_vector_get(shifts, 8);
-	*total_x += gsl_vector_get(shifts, 9);
-	*total_y += gsl_vector_get(shifts, 10);
-	*total_z += 0.0;
 
 	cell_set_reciprocal(cell, asx, asy, asz, bsx, bsy, bsz, csx, csy, csz);
 
@@ -527,8 +756,7 @@ static int iterate(struct reflpeak *rps, int n, UnitCell *cell,
 }
 
 
-static double pred_residual(struct reflpeak *rps, int n, struct detgeom *det,
-                            double dx, double dy)
+static double pred_residual(struct reflpeak *rps, int n, struct detgeom *det)
 {
 	int i;
 	double res = 0.0;
@@ -542,13 +770,13 @@ static double pred_residual(struct reflpeak *rps, int n, struct detgeom *det,
 
 	r = 0.0;
 	for ( i=0; i<n; i++ ) {
-		r += pow(x_dev(&rps[i], det, dx, dy), 2.0);
+		r += pow(det->panels[rps[i].peak->pn].pixel_pitch*fs_dev(&rps[i], det), 2.0);
 	}
 	res += r;
 
 	r = 0.0;
 	for ( i=0; i<n; i++ ) {
-		r += pow(y_dev(&rps[i], det, dx, dy), 2.0);
+		r += pow(det->panels[rps[i].peak->pn].pixel_pitch*ss_dev(&rps[i], det), 2.0);
 	}
 	res += r;
 
@@ -569,18 +797,15 @@ static void free_rps_noreflist(struct reflpeak *rps, int n)
 }
 
 
-int refine_prediction(struct image *image, Crystal *cr)
+int refine_prediction(struct image *image, Crystal *cr, Mille *mille)
 {
 	int n;
 	int i;
 	struct reflpeak *rps;
 	double max_I;
 	RefList *reflist;
-	double total_x = 0.0;
-	double total_y = 0.0;
-	double total_z = 0.0;
-	double orig_shift_x, orig_shift_y;
 	char tmp[256];
+	gsl_matrix **Minvs;
 
 	rps = malloc(image_feature_count(image->features)
 	                       * sizeof(struct reflpeak));
@@ -595,9 +820,7 @@ int refine_prediction(struct image *image, Crystal *cr)
 	}
 	crystal_set_reflections(cr, reflist);
 
-	crystal_get_det_shift(cr, &total_x, &total_y);
-	orig_shift_x = total_x;
-	orig_shift_y = total_y;
+	Minvs = make_panel_minvs(image->detgeom);
 
 	/* Normalise the intensities to max 1 */
 	max_I = -INFINITY;
@@ -620,29 +843,36 @@ int refine_prediction(struct image *image, Crystal *cr)
 	}
 
 	//STATUS("Initial residual = %e\n",
-	//       pred_residual(rps, n, image->detgeom, total_x, total_y));
+	//       pred_residual(rps, n, image->detgeom));
 
-	/* Refine */
-	for ( i=0; i<MAX_CYCLES; i++ ) {
+	/* Refine (max 10 cycles) */
+	for ( i=0; i<10; i++ ) {
 		update_predictions(cr);
-		if ( iterate(rps, n, crystal_get_cell(cr), image,
-		             &total_x, &total_y, &total_z) )
+		if ( iterate(rps, n, crystal_get_cell(cr), image, Minvs) )
 		{
 			crystal_set_reflections(cr, NULL);
 			return 1;
 		}
-		crystal_set_det_shift(cr, total_x, total_y);
 		//STATUS("Residual after %i = %e\n", i,
-		//       pred_residual(rps, n, image->detgeom, total_x, total_y));
+		//       pred_residual(rps, n, image->detgeom));
 	}
 	//STATUS("Final residual = %e\n",
-	//       pred_residual(rps, n, image->detgeom, total_x, total_y));
+	//       pred_residual(rps, n, image->detgeom));
 
 	snprintf(tmp, 255, "predict_refine/final_residual = %e",
-	         pred_residual(rps, n, image->detgeom, total_x, total_y));
+	         pred_residual(rps, n, image->detgeom));
 	crystal_add_notes(cr, tmp);
 
-	crystal_set_det_shift(cr, total_x, total_y);
+	if ( mille != NULL ) {
+		profile_start("mille-calc");
+		write_mille(mille, n, crystal_get_cell(cr), rps, image, Minvs);
+		profile_end("mille-calc");
+	}
+
+	for ( i=0; i<image->detgeom->n_panels; i++ ) {
+		gsl_matrix_free(Minvs[i]);
+	}
+	free(Minvs);
 
 	crystal_set_reflections(cr, NULL);
 	reflist_free(reflist);
@@ -650,9 +880,17 @@ int refine_prediction(struct image *image, Crystal *cr)
 	n = pair_peaks(image, cr, NULL, rps);
 	free_rps_noreflist(rps, n);
 	if ( n < 10 ) {
-		crystal_set_det_shift(cr, orig_shift_x, orig_shift_y);
+		if ( mille != NULL ) {
+			crystfel_mille_delete_last_record(mille);
+		}
 		return 1;
 	}
 
+	if ( mille != NULL ) {
+		profile_start("mille-write");
+		crystfel_mille_write_record(mille);
+		profile_end("mille-write");
+	}
+
 	return 0;
 }
diff --git a/libcrystfel/src/predict-refine.h b/libcrystfel/src/predict-refine.h
index 5607e356..1e72a048 100644
--- a/libcrystfel/src/predict-refine.h
+++ b/libcrystfel/src/predict-refine.h
@@ -29,17 +29,64 @@
 #ifndef PREDICT_REFINE_H
 #define PREDICT_REFINE_H
 
+#include <gsl/gsl_matrix.h>
+
+struct reflpeak;
+
+/** Enumeration of parameters which may want to be refined */
+enum gparam {
+	GPARAM_ASX,
+	GPARAM_ASY,
+	GPARAM_ASZ,
+	GPARAM_BSX,
+	GPARAM_BSY,
+	GPARAM_BSZ,
+	GPARAM_CSX,
+	GPARAM_CSY,
+	GPARAM_CSZ,
+	GPARAM_DET_TX,
+	GPARAM_DET_TY,
+	GPARAM_DET_TZ,
+	GPARAM_DET_RX,  /* Detector panel (group) rotation about +x */
+	GPARAM_DET_RY,  /* Detector panel (group) rotation about +y */
+	GPARAM_DET_RZ,  /* Detector panel (group) rotation about +z */
+};
+
+
+/* Weighting of excitation error term (m^-1) compared to position term (pixels) */
+#define EXC_WEIGHT (0.5e-7)
+
+
 #include "crystal.h"
+#include "crystfel-mille.h"
 
-struct image;
+struct reflpeak {
+	Reflection *refl;
+	struct imagefeature *peak;
+	double Ih;   /* normalised */
+};
 
 /**
  * \file predict-refine.h
  * Prediction refinement: refinement of indexing solutions before integration.
  */
 
-extern int refine_prediction(struct image *image, Crystal *cr);
+extern int refine_prediction(struct image *image, Crystal *cr, Mille *mille);
+
 extern int refine_radius(Crystal *cr, struct image *image);
 
+extern double r_dev(struct reflpeak *rp);
+
+extern double fs_dev(struct reflpeak *rp, struct detgeom *det);
+
+extern double ss_dev(struct reflpeak *rp, struct detgeom *det);
+
+extern double r_gradient(int param, Reflection *refl, UnitCell *cell,
+                         double wavelength);
+
+extern int fs_ss_gradient(int param, Reflection *refl, UnitCell *cell,
+                          struct detgeom_panel *p, gsl_matrix *panel_Minv,
+                          double cx, double cy, double cz,
+                          float *fsg, float *ssg);
 
 #endif	/* PREDICT_REFINE_H */
diff --git a/libcrystfel/src/utils.c b/libcrystfel/src/utils.c
index b69a816d..10e4c38e 100644
--- a/libcrystfel/src/utils.c
+++ b/libcrystfel/src/utils.c
@@ -97,6 +97,77 @@ void show_matrix(gsl_matrix *M)
 }
 
 
+void show_vector(gsl_vector *v)
+{
+	int i;
+
+	for ( i=0; i<v->size; i++ ) {
+		STATUS("[ ");
+		STATUS("%+9.3e ", gsl_vector_get(v, i));
+		STATUS("]\n");
+	}
+}
+
+
+gsl_matrix *matrix_mult(gsl_matrix *A, gsl_matrix *B)
+{
+	gsl_matrix *r = gsl_matrix_calloc(A->size1, A->size2);
+	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A, B, 0.0, r);
+	return r;
+}
+
+
+gsl_matrix *matrix_mult3(gsl_matrix *A, gsl_matrix *B, gsl_matrix *C)
+{
+	gsl_matrix *tmp = matrix_mult(B, C);
+	gsl_matrix *r = matrix_mult(A, tmp);
+	gsl_matrix_free(tmp);
+	return r;
+}
+
+
+gsl_matrix *matrix_invert(gsl_matrix *m)
+{
+	gsl_permutation *perm;
+	gsl_matrix *inv;
+	int s;
+
+	perm = gsl_permutation_alloc(m->size1);
+	if ( perm == NULL ) {
+		ERROR("Couldn't allocate permutation\n");
+		gsl_matrix_free(m);
+		return NULL;
+	}
+
+	inv = gsl_matrix_alloc(m->size1, m->size2);
+	if ( inv == NULL ) {
+		ERROR("Couldn't allocate inverse\n");
+		gsl_matrix_free(m);
+		gsl_permutation_free(perm);
+		return NULL;
+	}
+
+	if ( gsl_linalg_LU_decomp(m, perm, &s) ) {
+		ERROR("Couldn't decompose matrix\n");
+		gsl_matrix_free(m);
+		gsl_permutation_free(perm);
+		return NULL;
+	}
+
+	if ( gsl_linalg_LU_invert(m, perm, inv)  ) {
+		ERROR("Couldn't invert cell matrix:\n");
+		gsl_matrix_free(m);
+		gsl_permutation_free(perm);
+		return NULL;
+	}
+
+	gsl_permutation_free(perm);
+	gsl_matrix_free(m);
+
+	return inv;
+}
+
+
 static int check_eigen(gsl_vector *e_val, int verbose)
 {
 	int i;
@@ -399,6 +470,15 @@ void progress_bar(int val, int total, const char *text)
 }
 
 
+void rotate2d(double *x, double *y, double cx, double cy, double ang)
+{
+	double nx, ny;
+	nx = cx + (*x-cx)*cos(ang) - (*y-cy)*sin(ang);
+	ny = cy + (*x-cx)*sin(ang) + (*y-cy)*cos(ang);
+	*x = nx;  *y = ny;
+}
+
+
 double random_flat(gsl_rng *rng, double max)
 {
 	return max * gsl_rng_uniform(rng);
diff --git a/libcrystfel/src/utils.h b/libcrystfel/src/utils.h
index 9683039e..67940ad4 100644
--- a/libcrystfel/src/utils.h
+++ b/libcrystfel/src/utils.h
@@ -74,8 +74,12 @@ extern "C" {
 
 extern void show_matrix_eqn(gsl_matrix *M, gsl_vector *v);
 extern void show_matrix(gsl_matrix *M);
+extern void show_vector(gsl_vector *M);
 extern gsl_vector *solve_svd(gsl_vector *v, gsl_matrix *M, int *n_filt,
                             int verbose);
+extern gsl_matrix *matrix_mult2(gsl_matrix *A, gsl_matrix *B);
+extern gsl_matrix *matrix_mult3(gsl_matrix *A, gsl_matrix *B, gsl_matrix *C);
+extern gsl_matrix *matrix_invert(gsl_matrix *m);
 
 extern size_t notrail(char *s);
 extern int convert_int(const char *str, int *pval);
@@ -164,6 +168,8 @@ static inline int within_tolerance(double a, double b, double percent)
 	return 0;
 }
 
+extern void rotate2d(double *x, double *y, double cx, double cy, double ang);
+
 
 /* ----------------------------- Useful macros ------------------------------ */