import java.io.Serializable;
import Geometry.*;
import DE.*;

/**
 * Class to represent camera locations and orientations for a series of photos,
 * along with a location in 3D of a variety of points on the surface of some
 * subject.  The following geometry is assumed:<ul>
 * <li>The first surface point is at (0,0,0).</li>
 * <li>The second surface point is at (0,0,-1).</li>
 * <li>The camera for the first photo is located in the half plane defined
 *     by y=0 and x<0 and such that the first and second points are projected
 *     perfectly.  The position of the camera is parameterized by elevation
 *     above the line z=-0.5.</li>
 * <li>The camera for the second photo is also positioned such that the first
 *     and second surface points are projected perfectly.  This camera
 *     position is parameterized by elevation above z=-0.5 and rotation about
 *     the z axis.</li></ul>
 * Written by Chris Studholme
 * Licenced for redistribution under the GNU General Public Licence
 */
public class PointParameters implements ParameterSet, Serializable {
    
    /** Math.log(10).
     */
    public static final double LOG10 = Math.log(10);

    /** Initial log base 10 of width parameter.
     */
    public static final double INITIAL_LOGWIDTH = -4;
    
    /** Estimated location of surface points.
     */
    public NVector[] points;
    /** Number of surface points known.
     */
    public int npoints;

    /** Position, direction, orientation, and focal length of camera for
     * each photo.
     */
    public CameraParameters[] photos;
    /** Number of photos currently being considered.
     */
    public int nphotos;

    /** The camera position for photos 0 and 1 is parameterized by angles.
     */
    public AngleParameter[] angles;

    /** Width parameter (ratio of pixel width to focal length).
     */
    private double width;
    /** Log base 10 of width.
     */
    private double logwidth;
    /** Is the width parameter being varied?
     */
    public boolean varywidth;

    /**
     * Set up parameter set for a specified number of points, a specified
     * number of photos and note whether width is to be varied or not.
     * @param npoints maximum number of points appearing in photos
     * @param nphotos number of photos to optimize over
     * @param varywidth true if width is to be a parameter
     */
    public PointParameters(int npoints, int nphotos, boolean varywidth) {
	this.npoints = npoints;
	points = new NVector[npoints];
	points[0] = new NVector(0,0,0);
	points[1] = new NVector(0,0,-1);
	this.nphotos = nphotos;
	photos = new CameraParameters[nphotos];
	for (int i=0; i<nphotos; ++i)
	    photos[i]=new CameraParameters();
	angles = new AngleParameter[2];
	angles[0] = new AngleParameter(0,0);
	angles[1] = new AngleParameter(0,0);
	logwidth=INITIAL_LOGWIDTH;
	this.varywidth = varywidth;
    }
    /**
     * Set up parameter set for a specified number of points and a specified
     * number of photos.  The width parameter is set to not vary.
     * @param npoints maximum number of points appearing in photos
     * @param nphotos number of photos to optimize over
     */
    public PointParameters(int npoints, int nphotos) {
	this(npoints,nphotos,false);
    }
    /**
     * Make this parameter set a copy of some other parameter set.
     * @param other parameter set to copy
     */
    public PointParameters(PointParameters other) {
	npoints = other.npoints;
	points = new NVector[npoints];
	System.arraycopy(other.points,0,points,0,npoints);
	nphotos = other.nphotos;
	photos = new CameraParameters[nphotos];
	for (int i=0; i<nphotos; ++i)
	    photos[i] = new CameraParameters(other.photos[i]);
	angles = new AngleParameter[2];
	System.arraycopy(other.angles,0,angles,0,angles.length);
	width = other.width;
	logwidth = other.logwidth;
	varywidth = other.varywidth;
    }
    /**
     * Construct a copy of this and return it.
     * @return new PointParameters(this)
     */
    public ParameterSet getCopy() {
	return new PointParameters(this);
    }

    /**
     * Set the number of photos to find locations for.  If the new value is
     * greater than the previous value, default new CameraParameters objects
     * are created for each new photo.  
     * @param nphotos number of photos to find locations for
     */
    public void setNumberOfPhotos(int nphotos) {
	if (nphotos>photos.length) {
	    // reallocate
	    CameraParameters[] newphotos = new CameraParameters[nphotos];
	    System.arraycopy(photos,0,newphotos,0,this.nphotos);
	    photos = newphotos;
	}
	for (int i=this.nphotos; i<nphotos; ++i)
	    photos[i] = new CameraParameters();
	this.nphotos = nphotos;
    }

    /**
     * Increase the number of photos by one and initalize the the new photo
     * to a random location.  The position is normally distributed about
     * center with a standard deviation of stdev.
     * @param center mean location of new photo
     * @param stdev standard deviation of location
     */
    public void addRandomPhoto(NVector center, double stdev) {
	setNumberOfPhotos(nphotos+1);
	photos[nphotos-1].randomize(center,stdev);
    }

    /**
     * Set the width parameter.
     * @param width new width value
     */
    public void setWidth(double width) {
	this.width=width;
	logwidth=Math.log(width)/LOG10;
    }

    /**
     * Set the width to a random value that is normally distributed about
     * mean with the standard deviation in the log base 10 of the width equal
     * to stdev.
     * @param mean mean value of width
     * @param stdev standard deviation of log base 10 of width
     */
    public void setWidthRandom(double mean, double stdev) {
	logwidth=Math.log(mean)/LOG10;
	logwidth+=DE.random.nextGaussian()*stdev;
	width = Math.exp(logwidth*LOG10);
    }

    /**
     * Get the value of the width parameter (ratio of pixel width to focal
     * length).
     * @return width value
     */
    public double getWidth() {
	return width;
    }

    /**
     * Make sure parameters are within appropriate ranges.  In particular,
     * all angles are checked, width is set from logwidth, and the third and
     * later photo parameters are normallized.
     */
    public void normalize() {
	if (angles[0].elevation>Math.PI/2)  angles[0].elevation=Math.PI/2;
	if (angles[0].elevation<-Math.PI/2) angles[0].elevation=-Math.PI/2;
	if (angles[1].elevation>Math.PI/2)  angles[1].elevation=Math.PI/2;
	if (angles[1].elevation<-Math.PI/2) angles[1].elevation=-Math.PI/2;
	while (angles[1].rotation>Math.PI)  angles[1].rotation-=2*Math.PI;
	while (angles[1].rotation<-Math.PI) angles[1].rotation+=2*Math.PI;
	width = Math.exp(logwidth*LOG10);
	for (int i=2; i<nphotos; ++i)
	    photos[i].normalize();
    }

    /** 
     * Randomize width and angle parameters (but not photo positions).  Width
     * is only randomized with varywidth is true.
     * @param widthstdev desired standard deviation in log base 10 of width
     */
    public void randomize(double widthstdev) {
	if ((varywidth)&&(widthstdev>0))
	    logwidth += DE.random.nextGaussian()*widthstdev;
	angles[0] = new AngleParameter(DE.random.nextGaussian()*Math.PI/6,0);
	angles[1] = new AngleParameter(DE.random.nextGaussian()*Math.PI/6,
				       DE.random.nextGaussian()*Math.PI/4);
	normalize();
    }

    /**
     * Randomize the positions of the photos. 
     * @param xstdev standard deviation in x dimension
     * @param ystdev standard deviation in y dimension
     * @param zstdev standard deviation in z dimension
     * @param anglestdev standard deviation for angle parameters
     */
    public void randomizePhotos(double xstdev, double ystdev, 
				double zstdev, double anglestdev) {
	angles[0].elevation += DE.random.nextGaussian()*anglestdev;
	angles[1].elevation += DE.random.nextGaussian()*anglestdev;
	angles[1].rotation += DE.random.nextGaussian()*anglestdev;
	for (int i=2; i<nphotos; ++i) {
	    photos[i].pos.component[0] += DE.random.nextGaussian()*xstdev;
	    photos[i].pos.component[1] += DE.random.nextGaussian()*ystdev;
	    photos[i].pos.component[2] += DE.random.nextGaussian()*zstdev;
	}
    }

    /**
     * Compute the position and orientation for the specified photo given
     * the pixels locations of some known points.
     * @param index photo position and orientation to compute
     * @param pixels pixel locations of some known points (some entries may be null)
     * @param knownpoints consider only the first this many entries in pixels
     */
    public void computeCameraPosition(int index,
				      NVector[] pixels,
				      int knownpoints) {
	if (index==0)
	    photos[0].computePosition(angles[0].elevation,0,pixels);
	else if (index==1)
	    photos[1].computePosition(angles[1].elevation,
				      angles[1].rotation,pixels);
	else
	    photos[index].computeOrientation(points,pixels,knownpoints);
    }

    /**
     * Number of parameters that can vary in this parameter set.
     * @return number of parameters
     */
    public int size() {
	return nphotos + (varywidth?1:0);
    }

    /**
     * Get the specified parameter.  There is one parameter for each photo
     * location to be optimized over and possible one parameter for the width
     * variable.
     * @param index parameter index
     * @return a parameter object
     */
    public Parameter get(int index) {
	if (index<2)
	    return angles[index];
	else if (index<nphotos)
	    return photos[index];
	else
	    return new DoubleParameter(logwidth);
    }

    /**
     * Set the specified parameter.
     * @param value new parameter object to set
     * @param index parameter index
     */
    public void set(Parameter value, int index) {
	if (index<2)
	    angles[index]=(AngleParameter)value;
	else if (index<nphotos)
	    photos[index]=(CameraParameters)value;
	else
	    logwidth=((DoubleParameter)value).doubleValue();
    }

    /**
     * Return the angle parameters for the first two photos as a string.
     * @return String object
     */
    public String toString() {
	return angles[0]+" "+angles[1];
    }
}
