import algebra.*;

/**
 * author: cdehais
 */
public class Transformation {

    Matrix worldToCamera;
    Matrix projection;
    Matrix calibration;

    public Transformation() {
        try {
            worldToCamera = new Matrix("W2C", 4, 4);
            projection = new Matrix("P", 3, 4);
            calibration = Matrix.createIdentity(3);
            calibration.setName("K");
        } catch (InstantiationException e) {
            /* should not be reached */
        }
    }

    public void setLookAt(Vector3 cam, Vector3 lookAt, Vector3 up) {
        try {
            // compute rotation

            Vector3 e3c = new Vector3(lookAt);
            e3c.subtract(cam);
            e3c.normalize();

            Vector3 e1c = up.cross(e3c);
            e1c.normalize();

            Vector3 e2c = e3c.cross(e1c);

            // insertion de la matrice de rotation (transposée), dans worldToCamera

            worldToCamera.set(0, 0, e1c.get(0));
            worldToCamera.set(0, 1, e1c.get(1));
            worldToCamera.set(0, 2, e1c.get(2));

            worldToCamera.set(1, 0, e2c.get(0));
            worldToCamera.set(1, 1, e2c.get(1));
            worldToCamera.set(1, 2, e2c.get(2));

            worldToCamera.set(2, 0, e3c.get(0));
            worldToCamera.set(2, 1, e3c.get(1));
            worldToCamera.set(2, 2, e3c.get(2));

            // le vecteur de zeros sous la rotation, dans worldToCamera

            worldToCamera.set(3, 0, 0);
            worldToCamera.set(3, 1, 0);
            worldToCamera.set(3, 2, 0);

            // le 1 en bas à droite, dans worldToCamera

            worldToCamera.set(3, 3, 1);

            // compute translation

            Matrix M = worldToCamera.getSubMatrix(0, 0, 3, 3);

            Vector t = M.multiply(cam);

            // insertion du vecteur translation, dans worldToCamera

            worldToCamera.set(0, 3, -t.get(0));
            worldToCamera.set(1, 3, -t.get(1));
            worldToCamera.set(2, 3, -t.get(2));

        } catch (Exception e) {
            /* unreached */
        }

        System.out.println("Modelview matrix:\n" + worldToCamera);
    }

    public void setProjection() {
        // matrice "identitée" 3x4
        projection.set(0, 0, 1);
        projection.set(1, 1, 1);
        projection.set(2, 2, 1);

        System.out.println("Projection matrix:\n" + projection);
    }

    public void setCalibration(double focal, double width, double height) {
        // focal sur la diagonale, sauf en bas à droite
        calibration.set(0, 0, focal);
        calibration.set(1, 1, focal);

        // translation de w/2 et h/2
        calibration.set(0, 2, width / 2);
        calibration.set(1, 2, height / 2);

        System.out.println("Calibration matrix:\n" + calibration);
    }

    /**
     * Projects the given homogeneous, 4 dimensional point onto the screen.
     * The resulting Vector as its (x,y) coordinates in pixel, and its z coordinate
     * is the depth of the point in the camera coordinate system.
     */
    public Vector3 projectPoint(Vector p) throws SizeMismatchException, InstantiationException {

        Vector ps = new Vector(3);

        // projection du point dans le plan
        ps = calibration.multiply(projection.multiply(worldToCamera.multiply(p)));

        double depth = ps.get(2);

        // "normalisation" des coordonnées du point
        ps.set(0, ps.get(0) / depth);
        ps.set(1, ps.get(1) / depth);
        // ps.scale(1 / depth);

        return new Vector3(ps);
    }

    /**
     * Transform a vector from world to camera coordinates.
     */
    public Vector3 transformVector(Vector3 v) throws SizeMismatchException, InstantiationException {
        /* Doing nothing special here because there is no scaling */
        Matrix R = worldToCamera.getSubMatrix(0, 0, 3, 3);
        Vector tv = R.multiply(v);
        return new Vector3(tv);
    }

}