/**
 *Fit Sphere
 *Takes point selections from ROI manager and returns the
 *centroid and radius of a best fit sphere
 *Ported from Angelo Tardugno's C++
 *And based on a paper that we can't locate right now...
 *
 *This program is free software: you can redistribute it and/or modify
 *it under the terms of the GNU General Public License as published by
 *the Free Software Foundation, either version 3 of the License, or
 *(at your option) any later version.
 *
 *This program is distributed in the hope that it will be useful,
 *but WITHOUT ANY WARRANTY; without even the implied warranty of
 *MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *GNU General Public License for more details.
 *
 *You should have received a copy of the GNU General Public License
 *along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *@author Michael Doube and Angelo Tardugno
 *@version 0.1
 */

import Jama.Matrix;
import ij.IJ;
import ij.ImagePlus;
import ij.process.ImageProcessor;
import ij.plugin.filter.PlugInFilter;
import ij.gui.*;
import ij.plugin.frame.*;
import ij.measure.Calibration;
import java.awt.Rectangle;
import java.awt.List;

public class Fit_Sphere implements PlugInFilter {
	ImagePlus imp;
	RoiManager roiMan = RoiManager.getInstance();
			
	public int setup(String arg, ImagePlus imp) {
	    this.imp = imp;
	    if (roiMan == null){
	    	IJ.run("ROI Manager...");
	    	IJ.error("Please populate ROI Manager with point ROIs");
	    }
	    return DOES_ALL + STACK_REQUIRED;
	}
	
	public void run(ImageProcessor ip) {
		Calibration cal = imp.getCalibration();
		double vW = cal.pixelWidth;
		double vH = cal.pixelHeight;
		double vD = cal.pixelDepth;
		String units = cal.getUnit();
		int no_p = roiMan.getCount();
		List listRoi = roiMan.getList();
		double[] datapoints = new double[3*no_p];
		double xSum = 0, ySum = 0, zSum = 0;
		Roi[] roiList = roiMan.getRoisAsArray();
		int j = 0;
		for (int i = 0; i<roiMan.getCount(); i++){
			Roi roi = roiList[i];
			if (roi.getType() == 10){
				String label = listRoi.getItem(i);
				Rectangle xy = roi.getBoundingRect();
				datapoints[3*j] = xy.getX()*vW;
				datapoints[3*j+1] = xy.getY()*vH;
				datapoints[3*j+2] = roiMan.getSliceNumber(label)*vD;
				xSum += datapoints[3*j];
				ySum += datapoints[3*j+1];
				zSum += datapoints[3*j+2];
				j++;
			}
		}
		no_p = j;
		if (no_p < 5) IJ.error("ROI Manager contains < 5 point ROIs./n" +
				"Add some point ROIs and try again.");
		double[] centroid = {xSum/no_p, ySum/no_p, zSum/no_p};
		double[] r = new double[no_p];
		double g_new = 100.0;
		double g_old = 1.0;
		double radius = 0;
		for (int i = 0; i < no_p; i++)
			radius += Math.sqrt(Math.pow(datapoints[3*i]-centroid[0],2) + Math.pow(datapoints[3*i+1]-centroid[1],2)
					+ Math.pow(datapoints[3*i+2]-centroid[2],2));
		radius /= no_p;
		while (Math.abs(g_new-g_old) > 1e-10){
			Matrix J = new Matrix(no_p, 4);
			double[][] Jp = J.getArray();
			Matrix d = new Matrix(no_p, 1);
			double[][] dp = d.getArray(); //dp is a pointer to d's values
			g_old = g_new;
			for (int i = 0; i < no_p; i++){
				r[i] = Math.sqrt(Math.pow(datapoints[3*i]-centroid[0],2) + Math.pow(datapoints[3*i+1]-centroid[1],2) 
				+ Math.pow(datapoints[3*i+2]-centroid[2],2));
				dp[i][0] = r[i] - radius;
				Jp[i][0] = -(datapoints[3*i]-centroid[0])/r[i];
				Jp[i][1] = -(datapoints[3*i+1]-centroid[1])/r[i];
				Jp[i][2] = -(datapoints[3*i+2]-centroid[2])/r[i];
				Jp[i][3] = -1;
			}
			d = d.times(-1);
			Matrix J1 = J;
			J = J.transpose();
			Matrix J2 = J.times(J1);
			Matrix Jd = J.times(d);
			Matrix x = J2.inverse().times(Jd);
			double[][] xp = x.getArray();
			centroid[0] += xp[0][0];
			centroid[1] += xp[1][0];
			centroid[2] += xp[2][0];
			radius += xp[3][0];
			d = d.times(-1);
			Matrix G = J.times(d);
			double[][] Gp = G.getArray();
			g_new = 0.0;
			for (int i = 0; i < 4; i++)
					g_new += Gp[i][0];
		}
		IJ.write("Centroid = ("+centroid[0]+", "+centroid[1]+", "+centroid[2]+"): Radius = "+radius+" "+units);
	}	
}