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

/**
 * A parameter representing the location, direction, orientation, and focal
 * length of a camera at the moment a particular photo was taken.  The camera
 * position is parameterized in one of two ways:<ul>
 *   <li>In polar coordinates with the radial distance, direction, and
 *       orientation (up direction) all being computed from the projection
 *       of two points located at (0,0,0) and (0,0,-1).</li>
 *   <li>In rectangular coordinates where the direction and orientation are
 *       computed from offsets from the mean values required to fit many
 *       projected points.</li></ul>
 * Written by Chris Studholme
 * Licenced for redistribution under the GNU General Public Licence
 */
public class CameraParameters implements Parameter, Serializable {

    /** Position of the camera.
     */
    public NVector pos;

    /** Direction of mean of points (unit length).
     */
    public NVector dirbase;
    /** Offset from dirbase in 2D (should be small).
     */
    public NVector dirofs;
    /** Actual direction of camera (unit length).
     */
    public NVector dir;
    /** Focal length of camera (1 for fixed focal length).
     */
    public double fl;

    /** Mean direction of up (unit length).
     */
    public NVector upbase;
    /** Offset from upbase expressed as an angle in radians (clockwise
     *  around dirbase axis).
     */
    public double upofs;
    /** Actual up direction (unit length and orthoginal to dir).
     */
    public NVector up;

    /** Is the focal length to be varied?
     */
    public boolean varyfocallength=false;

    /**
     * Initialize with defaults: pos is (0,0,0), dir is (1,0,0), up is
     * (0,0,1), focal length is 1, and offsets are all 0. 
     */
    public CameraParameters() {
	pos = new NVector(0,0,0);
	fl=1;
	dirofs = new NVector(0,0);
	dir = new NVector(1,0,0);
	upofs = 0;
	up = new NVector(0,0,1);
    }
    /**
     * Make this parameter a copy of some other parameter.
     * @param other parameter to copy
     */
    public CameraParameters(CameraParameters other) {
	pos = new NVector(other.pos);
	dirofs = new NVector(other.dirofs);
	dir = other.dir; // no need to copy
	fl = other.fl;
	upofs = other.upofs;
	up = other.up; // no need to copy
	varyfocallength = other.varyfocallength;
    }

    /**
     * Scale this parameter by factor and then add to a scaled version of
     * some other parameter.
     * @param factor scale factor for this
     * @param other other parameter to add (may be null)
     * @param otherfactor scale factor for other
     * @return new CameraParameters object representing results
     */
    public Parameter scaleadd(double factor,
                              Parameter other, 
			      double otherfactor) {
	CameraParameters result = new CameraParameters(this);
	// scale result
	result.pos = result.pos.scaleDirect(factor);
	result.dirofs = result.dirofs.scaleDirect(factor);
	result.upofs *= factor;
	if (varyfocallength)
	    result.fl *= factor;
	// add in other
	if (other instanceof CameraParameters) {
	    CameraParameters o = (CameraParameters)other;
	    result.pos = result.pos.addDirect(o.pos.scale(otherfactor));
	    result.dirofs = 
		result.dirofs.addDirect(o.dirofs.scale(otherfactor));
	    result.upofs += otherfactor*o.upofs;
	    if (varyfocallength)
		result.fl += otherfactor*o.fl;
	}
	return result;
    }
    
    /**
     * Make sure upofs is within the range -PI to PI and focal length is
     * non-negative. 
     */
    public void normalize() {
	while (upofs>Math.PI)  upofs-=2*Math.PI;
	while (upofs<-Math.PI) upofs+=2*Math.PI;
	if (fl<0) fl=0;
    }

    /**
     * Randomize this parameter.  Position is normally distributed about
     * center with the specified standard deviation.  The offsets are normally
     * distributed about 0 with a standard deviation of 0.02.  If 
     * varyfocallength is true, then focal length is normally distributed 
     * about 1 with a standard deviation of 0.5.  normalize() is called after
     * the variables have been set to ensure they are within the appropriate
     * ranges.
     * @param center mean position
     * @param stdev standard deviation in position
     */
    public void randomize(NVector center, double stdev) {
	pos = new NVector(DE.random.nextGaussian()*stdev,
			  DE.random.nextGaussian()*stdev,
			  DE.random.nextGaussian()*stdev).addDirect(center);
	dirofs.component[0] = DE.random.nextGaussian()/50;
	dirofs.component[1] = DE.random.nextGaussian()/50;
	upofs = DE.random.nextGaussian()/50;
	if (varyfocallength)
	    fl = 1 + DE.random.nextGaussian()/2;
	normalize();
    }

    /**
     * Find a unit vector that (when added to pos) passes through specified 
     * pixel.
     * @param pixel pixel location within image plane (in 2D)
     * @return unit vector in 3D
     */
    public NVector projectPixel(NVector pixel) {
	NVector right = dir.crossProduct(up);
	NVector result = dir.scale(fl).
	    addDirect(right.scale(pixel.component[0])).
	    addDirect(up.scale(pixel.component[1]));
	return result.normalizeDirect();
    }

    /**
     * Find pixel that projects to the given point.  
     * @param point a point in 3D
     * @return 2D pixel location within image plane
     */
    public NVector computePixel(NVector point) {
	NVector right = dir.crossProduct(up);
	NVector proj = point.subtract(pos);
	proj = proj.scaleDirect(1/fl/proj.innerProduct(dir));
	return new NVector(proj.innerProduct(right),
			   proj.innerProduct(up));
    }

    /**
     * Compute position and orientation such that the two projected points
     * match exactly.  The two points are assume to be located at (0,0,0)
     * and (0,0,-1), respectively.  Elevation is the angle in radians from the
     * line z=-0.5 and increasing as z increases.  Rotation is about the z-axis
     * with 0 being in the half-plane y=0 and x<0.
     * @param elevation elevation angle in radians
     * @param rotation rotation angle in radians
     * @param pixels array of two pixel locations (each 2D within image plane)
     */
    public void computePosition(double elevation, double rotation, 
				NVector[] pixels) {
	// project pixels with dir=(0,0,-1), up=(0,1,0), right=(1,0,0)
	NVector p0 = new NVector(pixels[0].component[0],
				 pixels[0].component[1],
				 -fl).normalizeDirect();
	NVector p1 = new NVector(pixels[1].component[0],
				 pixels[1].component[1],
				 -fl).normalizeDirect();
	p0 = p0.subtractDirect(p1);

	double theta = Math.acos(1-p0.lengthSquare()/2);
	double ct = Math.cos(theta);
	double st = Math.sin(theta);
	double cs = Math.cos(elevation);
	double ss = Math.sin(elevation);
	
	double radius = Math.sqrt(1 + ct*ct - 2*ss*ss*ct*ct +
				  2*ct*cs*Math.sqrt(1-ss*ss*ct*ct)) /2/st;

	// pos
	pos = new NVector(-radius*cs*Math.cos(rotation),
			  -radius*cs*Math.sin(rotation),
			  -0.5+radius*ss);

	// dir and up
	// make p0,p1,and p0xp1 into a coordinate system
	p0 = p0.makeOrthogonalTo(p1,true);
	NVector p2 = p0.crossProduct(p1);

	// find coordinates of dir and up in that system
	NVector d = new NVector(0,0,-1);
	NVector u = new NVector(0,1,0);
	double d0 = d.innerProduct(p0);
	double d1 = d.innerProduct(p1);
	double d2 = d.innerProduct(p2);
	double u0 = u.innerProduct(p0);
	double u1 = u.innerProduct(p1);
	double u2 = u.innerProduct(p2);

	// define new coordinate system
	p0 = new NVector(0,0,0).subtractDirect(pos).normalizeDirect();
	p1 = new NVector(0,0,-1).subtractDirect(pos).normalizeDirect();
	p0 = p0.subtractDirect(p1);
	p0 = p0.makeOrthogonalTo(p1,true);
	p2 = p0.crossProduct(p1);

	// map dir and up to new system
	dir = p0.scale(d0).add(p1.scale(d1)).add(p2.scale(d2)).scale(1/fl);
	up = p0.scale(u0).add(p1.scale(u1)).add(p2.scale(u2));
    }

    /**
     * Compute base direction and orientation such that the specified pixels
     * project as closely as possible to the specified points.  The actual
     * direction and orientation are then computed by applying the offsets
     * from these base values.  The position of the camera is unchanged by
     * this method.
     * @param points points in 3D
     * @param pixels pixel values in 2D corresponding points
     * @param npoints number of points in points and pixels arrays
     */
    public void computeOrientation(NVector[] points, 
				   NVector[] pixels,
				   int npoints) {
	// compute mean point and mean pixel
	NVector pointmean = new NVector(0,0,0);
	NVector pixelmean = new NVector(0,0);
	int count=0;
	for (int i=0; i<npoints; ++i) 
	    if (pixels[i]!=null) {
		pointmean = pointmean.addDirect(points[i]);
		pixelmean = pixelmean.addDirect(pixels[i]);
		++count;
	    }
	pointmean = pointmean.scaleDirect(1.0/count);
	pixelmean = pixelmean.scaleDirect(1.0/count);
	// compute dirbase
	dirbase = pointmean.subtract(pos).normalizeDirect();
	// compute upbase
	upbase = new NVector(0,0,0);
	for (int i=0; i<npoints; ++i)
	    if (pixels[i]!=null) {
		NVector u = points[i].subtract(pos);
		u = u.subtractDirect(dirbase.scale(u.innerProduct(dirbase)));
		NVector r = dirbase.crossProduct(u);
		// u and r are orthogonal to dirbase
		// length of u,r is an indication of how trustworthy it is
		double a = pixels[i].subtract(pixelmean).planarAngle(); 
		// rotate u clockwise by angle -a
		u = u.scaleDirect(Math.cos(-a)).
		    addDirect(r.scaleDirect(Math.sin(-a)));
		upbase.addDirect(u);
	    }
	upbase = upbase.normalizeDirect();
	if (upbase==null) {
	    // pick something orthogonal to dirbase 
	    // this should never (very rarely) happen
	    if (dirbase.component[2]==0)
		upbase = new NVector(0,0,1);
	    else
		upbase = new NVector(0,-dirbase.component[2],
				     dirbase.component[1]);
	}
	// compute up (unit length and orthogonal to dirbase at first)
	up = upbase.scale(Math.cos(upofs)).
	    addDirect(dirbase.crossProduct(upbase).scale(Math.sin(upofs)));
	// compute dir
	dir = dirbase.add(up.scale(dirofs.component[1])).
	    addDirect(dirbase.crossProduct(up).scale(dirofs.component[0]));
	dir = dir.normalizeDirect();
	// make sure up is orthogonal to dir and unit length
	up = up.subtractDirect(dir.scale(dir.innerProduct(up)));
	up = up.normalizeDirect();
    }


}
