import java.io.*;
import java.util.Arrays;
import DE.*;
import Geometry.*;

/**
 * Class to hold information about a particular photo including its id, name,
 * and an array of point observations made.
 * Written by Chris Studholme
 * Licenced for redistribution under the GNU General Public Licence
 */
class PhotoInfo implements Serializable {
    /** File name of photo.
     */
    public String name;
    /** ID number of photo.
     */
    public int id;
    /** Width of photo (in pixels).
     */
    public int width;
    /** Height of photo (in pixels).
     */
    public int height;

    /** Observations of points visible in this photo.
     */
    public Observation[] points;
    /** Number of valid entries in points.
     */
    public int npoints;

    /** Sparse array of pixel observations.
     */
    public transient NVector[] pixels;

    /** Variable used for sorting.
     */
    public transient int maxoverlap;
    /** Variable used for sorting.
     */
    public transient int maxoverlapindex;
    /** Variable used for sorting.
     */
    public transient int maxpoints;

    /** 
     * Initialize object.
     * @param name file name of photo
     * @param id id number of photo
     * @param width width of photo in pixels
     * @param height height of photo in pixels
     */
    public PhotoInfo(String name, int id, int width, int height) {
	this.name=name;
	this.id=id;
	this.width=width;
	this.height=height;
	points=new Observation[2];
	npoints=0;
    }

    /**
     * Add an observation of a point for this photo.
     * @param o an observation of a point
     */
    public void addPoint(Observation o) {
	if (npoints>=points.length) {
	    // reallocate
	    Observation[] newpoints = new Observation[points.length*2];
	    System.arraycopy(points,0,newpoints,0,npoints);
	    points = newpoints;
	}
	points[npoints++]=o;
	o.photo = this;
    }
    /**
     * Return the file name of this photo.
     * @return String object
     */
    public String toString() {
	return name;
    }
    /**
     * Check if this photo equals another by comparing id numbers.
     * @param other other PhotoInfo object
     * @return true if they have the same id
     */
    public boolean equals(Object other) {
	if (!(other instanceof PhotoInfo))
	    return false;
	return id==((PhotoInfo)other).id;
    }
}

/**
 * Class to keep track of know information about a point on the surface of
 * some object.
 */
class PointInfo implements Comparable, Serializable  {
    /** Description of the surface point.
     */
    public String name;
    /** ID number for this point.
     */
    public int id;

    /** Observations of this point in various photos.
     */
    public Observation[] photos;
    /** Number of photos this point appears in.
     */
    public int nphotos;

    /** Index of first photo this point appears in.
     */
    public int firstphotoindex;
    /** First observation of this point. 
     */
    public Observation firstphoto;

    /** Index of second photo this point appears in.
     */
    public int secondphotoindex;
    /** Second observation of this point. 
     */
    public Observation secondphoto;

    /**
     * Initialize point info.
     * @param name description of point
     * @param id id number of point
     */
    public PointInfo(String name, int id) {
	this.name=name;
	this.id=id;
	photos=new Observation[2];
	nphotos=0;
	firstphotoindex=-1;
	secondphotoindex=-1;
    }
    /**
     * Add an observation of this point.
     * @param o an observation of this point
     */
    public void addPhoto(Observation o) {
	if (nphotos>=photos.length) {
	    // reallocate
	    Observation[] newphotos = new Observation[photos.length*2];
	    System.arraycopy(photos,0,newphotos,0,nphotos);
	    photos = newphotos;
	}
	photos[nphotos++]=o;
	o.point = this;
    }
    /**
     * Note that this point appears in the photo with the specified index.
     * This method sets firstphotoindex, firstphoto, secondphotoindex, and
     * secondphoto as needed.
     * @param index index of photo in which this point appears
     * @param o actual observation of this point in the photo
     */
    public void setPhotoIndex(int index, Observation o) {
	if (firstphotoindex==-1) {
	    firstphotoindex=index;
	    firstphoto = o;
	}
	else if (secondphotoindex==-1) {
	    secondphotoindex=index;
	    secondphoto = o;
	}
    }
    /**
     * Compare this point to another on the bases of which point appears in
     * a second photo first.  In the case of a tie, the point that appears in
     * the largest number of photos is returned as being ealier.
     * @param other PointInfo object to compare to
     * @return -1, 0, or 1
     */
    public int compareTo(Object other) {
	PointInfo pi = (PointInfo)other;
	if (pi.secondphotoindex==-1)
	    return -1;
	if (secondphotoindex==-1)
	    return 1;
	if (secondphotoindex<pi.secondphotoindex)
	    return -1;
	if (secondphotoindex>pi.secondphotoindex)
	    return 1;
	if (nphotos>pi.nphotos)
	    return -1;
	if (nphotos<pi.nphotos)
	    return 1;
	return 0;
    }
    /**
     * Return a description of this point.
     * @return String object
     */
    public String toString() {
	return name;
    }
    /**
     * Test if this object equals another PointInfo object by comparing id
     * numbers.
     * @param other object to compare to
     * @return true if the id number are the same
     */
    public boolean equals(Object other) {
	if (!(other instanceof PointInfo))
	    return false;
	return id==((PointInfo)other).id;
    }
}

/**
 * Class to keep track of the details of an observation of a point in a
 * particular photo.
 */
class Observation implements Serializable {
    /** The photo where the observation was made.
     */
    public PhotoInfo photo;
    /** The point that was observed.
     */
    public PointInfo point;

    /** The pixel in the photo where the point was observed.
     */
    public NVector pixel;

    /**
     * Initialize with a particular pixel location.
     * @param pixel pixel location
     */
    public Observation(NVector pixel) {
	this.pixel = pixel;
    }
    /**
     * Compare this observation to another checking if photo, point, and
     * pixel all match.
     * @param other observation to compare to
     * @return true if the observations are the same
     */
    public boolean equals(Object other) {
	if (!(other instanceof Observation))
	    return false;
	return pixel.equals(((Observation)other).pixel)&&
	    photo.equals(((Observation)other).photo)&&
	    point.equals(((Observation)other).point);
    }
}

/**
 * Class to keep track of a number of observations of a number of points on
 * the surface of some object appearing in a number of photos.  This class
 * can be used as data for a numerical optimization algorithm.
 */
class ObservedPoints implements ObservedDataSet, Serializable {

    /** Surface points being observed.
     */
    public PointInfo[] points;
    /** Number of surface points observed.
     */
    public int npoints;

    /** Photos in which the surface points appear.
     */
    public PhotoInfo[] photos;
    /** Number of photos.
     */
    public int nphotos;

    /**
     * Initalize arrays to default lengths.
     */
    public ObservedPoints() {
	points = new PointInfo[4];
	npoints = 0;
	photos = new PhotoInfo[4];
	nphotos = 0;
    }

    /**
     * Register a new surface point.
     * @param name description of point
     * @param id id number for point
     */
    public void addPoint(String name, int id) {
	if (npoints>=points.length) {
	    // reallocate
	    PointInfo[] newpoints = new PointInfo[points.length*2];
	    System.arraycopy(points,0,newpoints,0,npoints);
	    points = newpoints;
	}
	points[npoints++] = new PointInfo(name,id);
    }

    /**
     * Register a new photo.
     * @param name file name of photo
     * @param id id number of photo
     * @param width width of photo in pixels
     * @param height height of photo in pixels
     */
    public void addPhoto(String name, int id, int width, int height) {
	if (nphotos>=photos.length) {
	    // reallocate
	    PhotoInfo[] newphotos = new PhotoInfo[photos.length*2];
	    System.arraycopy(photos,0,newphotos,0,nphotos);
	    photos = newphotos;
	}
	photos[nphotos++] = new PhotoInfo(name,id,width,height);
    }

    /**
     * Note the observation of a particular point at a particular location
     * (pixel) within a particular photo.  The coordinates of the pixel
     * will be multiplied by the width and height of the photo, respectively, 
     * before being stored.
     * @param pointid id of point
     * @param photoid id of photo
     * @param x x location of pixel in the range -0.5 to 0.5
     * @param y y location of pixel in the range -0.5 to 0.5
     */
    public void addObservation(int pointid, int photoid, 
			       double x, double y) {
	Observation o = new Observation(new NVector(x,y));
	// figure out what point this observation is for
	for (int i=0; i<npoints; ++i)
	    if (points[i].id==pointid) {
		points[i].addPhoto(o);
		break;
	    }
	// figure out what photo this observation is for
	for (int i=0; i<nphotos; ++i)
	    if (photos[i].id==photoid) {
		o.pixel.component[0] *= photos[i].width;
		o.pixel.component[1] *= photos[i].height;
		photos[i].addPoint(o);
		break;
	    }
    }

    /**
     * Read observation data from a ascii file.  The file is expected to have
     * the following format:<ul>
     * <li>A list of points in the format: "id description".</li>
     * <li>A blank line.</li>
     * <li>A list of photos in the format: "id filename".</li>
     * <li>Another blank line.</li>
     * <li>A list of observations in the format: "pointid photoid x y".</li>
     * @param reader reader to obtain data from
     * @return number of observations returned
     */
    public int readData(BufferedReader reader) throws IOException {
	// read in list of points
	for (String line=reader.readLine(); !line.equals(""); 
	     line=reader.readLine()) {
	    String id = line.substring(0,line.indexOf(' '));
	    String name = line.substring(1+line.indexOf(' '));
	    addPoint(name,Integer.parseInt(id));
	}
	
	// read in list of photos
	for (String line=reader.readLine(); !line.equals(""); 
	     line=reader.readLine()) {
	    String id = line.substring(0,line.indexOf(' '));
	    line = line.substring(1+line.indexOf(' '));
	    String name = line.substring(0,line.indexOf(' '));
	    line = line.substring(1+line.indexOf(' '));
	    String width = line.substring(0,line.indexOf(' '));
	    String height = line.substring(1+line.indexOf(' '));
	    addPhoto(name,Integer.parseInt(id),
		     Integer.parseInt(width),
		     Integer.parseInt(height));
	}

	// read in observations
	int count=0;
	for (String line=reader.readLine(); line!=null; 
	     line=reader.readLine()) {
	    String pointid = line.substring(0,line.indexOf(' '));
	    line = line.substring(1+line.indexOf(' '));
	    String photoid = line.substring(0,line.indexOf(' '));
	    line = line.substring(1+line.indexOf(' '));
	    String x = line.substring(0,line.indexOf(' '));
	    String y = line.substring(1+line.indexOf(' '));
	    addObservation(Integer.parseInt(pointid),
			   Integer.parseInt(photoid),
			   Double.parseDouble(x)-0.5,
			   0.5-Double.parseDouble(y));
	    ++count;
	}	    
	return count;
    }

    /**
     * Sort photos first by greatest overlap and second by number of points.
     * Sort points first by order of second appearence (in a photo) and
     * second by the number of photos the points appears in.
     */
    public void sort() {
	// compute maximum overlap
	int max=0;
	for (int i=0; i<nphotos-1; ++i) {
	    photos[i].maxoverlap=0;
	    for (int j=i+1; j<nphotos; ++j) {
		int overlap=0;
		for (int k=0; k<photos[i].npoints; ++k)
		    for (int l=0; l<photos[j].npoints; ++l)
			if (photos[i].points[k].point.
			    equals(photos[j].points[l].point))
			    ++overlap;
		if (overlap>photos[i].maxoverlap) {
		    photos[i].maxoverlap=overlap;
		    photos[i].maxoverlapindex=j;
		}
	    }
	    photos[i].maxpoints =
		photos[i].npoints>photos[photos[i].maxoverlapindex].npoints ? 
		photos[i].npoints : photos[photos[i].maxoverlapindex].npoints;
	    if (photos[i].maxoverlap>photos[max].maxoverlap)
		max=i;
	    else if ((photos[i].maxoverlap==photos[max].maxoverlap)&&
		     (photos[i].maxpoints>photos[max].maxpoints))
		max=i;
	}
	// max is the index of the first of the pair that we want for our
	// first two photos
	if (max>0) {
	    PhotoInfo t = photos[max];
	    System.arraycopy(photos,0,photos,1,max);
	    photos[0] = t;
	}
	if (photos[0].maxoverlapindex>1) {
	    PhotoInfo t = photos[photos[0].maxoverlapindex];
	    System.arraycopy(photos,1,photos,2,photos[0].maxoverlapindex-1);
	    photos[1] = t;
	}
	// start keeping track of which photo each point first appears in
	for (int i=0; i<2; ++i)
	    for (int k=0; k<photos[i].npoints; ++k)
		photos[i].points[k].point.setPhotoIndex(i,photos[i].points[k]);
	// now sort photos[2] and later in order of maximum overlap with
	// existing set
	for (int i=2; i<nphotos; ++i) {
	    int best=0;
	    int bestindex=0;
	    for (int j=i; j<nphotos; ++j) {
		int overlap=0;
		for (int k=0; k<photos[j].npoints; ++k)
		    if (photos[j].points[k].point.firstphotoindex>=0)
			++overlap;
		if (overlap>best) {
		    best=overlap;
		    bestindex=j;
		}
	    }
	    // move bestindex to position i
	    if (bestindex>i) {
		PhotoInfo t = photos[bestindex];
		System.arraycopy(photos,i,photos,i+1,bestindex-i);
		photos[i] = t;
	    }
	    // note new points now seen
	    for (int k=0; k<photos[i].npoints; ++k)
		photos[i].points[k].point.setPhotoIndex(i,photos[i].points[k]);
	}

	// sort points
	Arrays.sort(points,0,npoints);

	// make sure first two points are as far apart in both of the first
	// two photos
	double maxdist=0;
	int maxi=0;
	int maxj=1;
	for (int i=0; (i<npoints)&&(points[i].secondphotoindex==1); ++i)
	    for (int j=i+1; (j<npoints)&&
		     (points[j].secondphotoindex==1); ++j) {
		double dist = 
		    points[i].firstphoto.pixel.
		    subtract(points[j].firstphoto.pixel).lengthSquare()
		    * points[i].secondphoto.pixel.
		    subtract(points[j].secondphoto.pixel).lengthSquare();
		if (dist>maxdist) {
		    maxdist=dist;
		    maxi=i;
		    maxj=j;
		}
	    }
	// make maxi first
	if (maxi>0) {
	    PointInfo p = points[maxi];
	    System.arraycopy(points,0,points,1,maxi);
	    points[0]=p;
	}
	// make maxj second
	if (maxj>1) {
	    PointInfo p = points[maxj];
	    System.arraycopy(points,1,points,2,maxj-1);
	    points[1]=p;
	}
    }

    /**
     * Return a sparse array of pixels for given photo.
     * @param pi photo
     * @return sparse array of pixel values
     */
    public NVector[] getPixelArray(PhotoInfo pi) {
	if (pi.pixels==null) {
	    NVector[] pixels = new NVector[npoints];
	    for (int i=0; i<pi.npoints; ++i)
		for (int j=0; j<npoints; ++j)
		    if (pi.points[i].point.equals(points[j])) {
			pixels[j]=pi.points[i].pixel;
			break;
		    }
	    pi.pixels=pixels;
	}
	return pi.pixels;
    }

    /**
     * Return a sparse array of pixels scaled by the specified scale factor.
     * @param pi photo
     * @param scale scale factor
     * @return sparse array of pixel values
     */
    public NVector[] getPixelArray(PhotoInfo pi, double scale) {
	NVector[] orig = getPixelArray(pi);
	NVector[] result = new NVector[orig.length];
	for (int i=0; i<orig.length; ++i)
	    if (orig[i]!=null)
		result[i] = orig[i].scale(scale);
	return result;
    }

    /**
     * Compute the error of fit for the specified parameter set.
     * @param parameters parameter set to fit to data
     * @return error value
     */
    public double computeError(ParameterSet parameters) {
	PointParameters pp = (PointParameters)parameters;
	pp.normalize();

	double width = pp.getWidth();
	
	// start array of known points
	pp.points[0] = new NVector(0,0,0);
	pp.points[1] = new NVector(0,0,-1);
	int knownpoints=2;
	
	// location of first camera
	NVector[][] pixels = new NVector[pp.nphotos][];
	pixels[0] = getPixelArray(photos[0],width);
	pp.computeCameraPosition(0,pixels[0],2);
	
	// find location of all other cameras and points
	for (int i=1; i<pp.nphotos; ++i) {
	    // camera location
	    pixels[i] = getPixelArray(photos[i],width);
	    pp.computeCameraPosition(i,pixels[i],knownpoints);

	    // new known points
	    while ((knownpoints<npoints)&&
		   (points[knownpoints].secondphotoindex==i)) {
		int fpi = points[knownpoints].firstphotoindex;
		// compute intersection between two projected points
		NVector diff = pp.photos[fpi].pos.subtract(pp.photos[i].pos);
		if (diff.squareLength()==0)
		    return Double.POSITIVE_INFINITY;
		NVector proj0 = pp.photos[fpi].
		    projectPixel(pixels[fpi][knownpoints]);
		NVector proj1 = pp.photos[i].
		    projectPixel(pixels[i][knownpoints]);
		NVector cross = proj0.crossProduct(proj1);
		double crosslen = cross.squareLength();
		if (crosslen==0) {
		    // projections are parallel, too bad
		    return Double.POSITIVE_INFINITY;
		}
		// find location of minimum distance
		double dist0 = proj1.crossProduct(pp.photos[fpi].pos).
		    subtractDirect(proj1.crossProduct(pp.photos[i].pos)).
		    innerProduct(cross) / crosslen;
		double dist1 = proj0.crossProduct(pp.photos[fpi].pos).
		    subtractDirect(proj0.crossProduct(pp.photos[i].pos)).
		    innerProduct(cross) / crosslen;
		if ((dist0<=0)||(dist1<=0))
		    return Double.POSITIVE_INFINITY;
		pp.points[knownpoints++] =
		    pp.photos[fpi].pos.add(proj0.scale(dist0)).
		    add(pp.photos[i].pos.add(proj1.scale(dist1))).
		    scale(0.5);
	    }
	}
	
	// compute error
	double error=0;
	int count=0;
	for (int i=0; i<pp.nphotos; ++i) {
	    for (int j=0; j<knownpoints; ++j)
		if (pixels[i][j]!=null) {
		    // find pixel corresponding to point
		    NVector pixel = pp.photos[i].computePixel(pp.points[j]);
		    double diff = pixel.subtract(pixels[i][j]).lengthSquare();
		    error += diff/(width*width);
		    ++count;
		}
	}
	
	return Math.sqrt(error/count);
    }
}
