RayTracer raytracer;

void setup() {
  size(300, 300);
  raytracer = new RayTracer(300,300);

  Material yellow = new Material(
  new Colour(0.0, 0.0, 0.0), 
  new Colour(0.5, 0.5, 0.0),
  new Colour(0.8, 0.8, 0.0),
  1000);
  
  Material red = new Material(
  new Colour(0.0, 0.0, 0.0), 
  new Colour(0.7, 0.2, 0.0),
  new Colour(0.7, 0.2, 0.0),
  1000);

  Material blue = new Material(
  new Colour(0.0, 0.0, 0.0), 
  new Colour(0.0, 0.1, 8.0),
  new Colour(0.0, 0.1, 8.0),
  1000);
  
  Material purple = new Material(
  new Colour(0.0, 0.0, 0.0), 
  new Colour(0.4, 0.0, 8.0),
  new Colour(0.4, 0.0, 8.0),
  1000);
  
  Material green = new Material(
  new Colour(0.0, 0.0, 0.0), 
  new Colour(0.0, 0.8, 0.0),
  new Colour(0.0, 0.8, 0.0),
  1000);

  raytracer.lights.add(new PointLight(new Point3D(4.5, 4.5, 4.0), 
  new Colour(0.4, 0.4, 0.4)));

  raytracer.lights.add( new PointLight(new Point3D(7.5, 4.5, 0), 
  new Colour(0.4, 0.4, 0.4) ) );

  raytracer.lights.add(new PointLight(new Point3D(7.5, 4.5, 10), 
  new Colour(0.4, 0.4, 0.4) ) );
  
  raytracer.lights.add(new PointLight(new Point3D(-7.5, 4.5, 10), 
  new Colour(0.4, 0.4, 0.4) ) );
  

  double[] factor = {
    1.0,1.0,1.0          };

  SceneNode sphere = raytracer.addObject(new UnitSphere(), red);
  
  raytracer.translate(sphere, new Vector3D(-1, -2, -2));
  
  SceneNode sphere2 = raytracer.addObject(new UnitSphere(), purple);
  
  raytracer.translate(sphere2, new Vector3D(2, 2, -2));
  
  SceneNode sphere3 = raytracer.addObject(new UnitSphere(), green);
  
  raytracer.translate(sphere3, new Vector3D(2, 2, -2));
  
  
  SceneNode square = raytracer.addObject(new UnitSquare(), yellow);
  
  raytracer.translate(square, new Vector3D(-3, -2, 6));
  
  raytracer.translate(square, new Vector3D(-1, 1, -1));
  
  raytracer.rotate(square, 'y', 30);

  frameRate(300);
}

class Camera {
   Point3D eye = new Point3D(0, 0, 3);
   Vector3D view = new Vector3D(0, 0, -1);
   Vector3D up = new Vector3D(0, 1, 0);
   double fov = 60;
}


class Point3D
{
  float [] data = new float[3];

  Point3D(float x, float y, float z) { 
    data[0] = x;
    data[1] = y;
    data[2] = z;
  }

  Point3D(Point3D other) {
    data[0] = other.data[0];
    data[1] = other.data[1];
    data[2] = other.data[2];
  }

  Point3D sub(Point3D other) {
    return new Point3D(data[0]-other.data[0],
    data[1]-other.data[1],
    data[2]-other.data[2]);
  }

  float dot(Vector3D other) {
    return data[0]*other.data[0] + 
      data[1]*other.data[1] + 
      data[2]*other.data[2];
  }

  float dot(Point3D other) {
    return data[0]*other.data[0] + 
      data[1]*other.data[1] + 
      data[2]*other.data[2];
  }

  String toString() {
    return "[" + data[0] + " " + data[1] + " " + data[2] + "]";
  }

}

class Vector3D {
  float [] data = new float[3];

  Vector3D(float x, float y, float z) {
    data[0]=x;
    data[1]=y;
    data[2]=z;
  }

  Vector3D(Vector3D other) {
    data[0] = other.data[0];
    data[1] = other.data[1];
    data[2] = other.data[2];
  }

  Vector3D(Point3D other) {
    data[0] = other.data[0];
    data[1] = other.data[1];
    data[2] = other.data[2];
  }

  float dot(Vector3D other) {
    return data[0]*other.data[0] + 
      data[1]*other.data[1] + 
      data[2]*other.data[2];
  }

  Vector3D cross(Vector3D other) {
    Vector3D returnVec = new Vector3D(
    data[1]*other.data[2] - data[2]*other.data[1],
    data[2]*other.data[0] - data[0]*other.data[2],
    data[0]*other.data[1] - data[1]*other.data[0]);

    return returnVec;
  }

  Vector3D mul(float s) {
    return new Vector3D(data[0]*s,data[1]*s,data[2]*s);
  }


  Vector3D add(Vector3D other) {
    return new Vector3D(data[0]+other.data[0],
    data[1]+other.data[1],
    data[2]+other.data[2]);
  }

  Vector3D add(Point3D other) {
    return new Vector3D(data[0]+other.data[0],
    data[1]+other.data[1],
    data[2]+other.data[2]);
  }


  Vector3D sub(Vector3D other) {
    return new Vector3D(data[0]-other.data[0],
    data[1]-other.data[1],
    data[2]-other.data[2]);
  }

  Vector3D sub(Point3D other) {
    return new Vector3D(data[0]-other.data[0],
    data[1]-other.data[1],
    data[2]-other.data[2]);
  }

  double length()
  {
    return sqrt(dot(this)); 
  }

  double normalize() {
    float denom = 1.0;
    float x = (data[0] > 0.0) ? data[0] : -data[0];
    float y = (data[1] > 0.0) ? data[1] : -data[1];
    float z = (data[2] > 0.0) ? data[2] : -data[2];

    if(x > y) {
      if(x > z) {
        if(1.0 + x > 1.0) {
          y = y / x;
          z = z / x;
          denom = 1.0 / (x * sqrt(1.0 + y*y + z*z));
        }
      } 
      else { /* z > x > y */

        if(1.0 + z > 1.0) {
          y = y / z;
          x = x / z;
          denom = 1.0 / (z * sqrt(1.0 + y*y + x*x));
        }
      }
    } 
    else {
      if(y > z) {
        if(1.0 + y > 1.0) {
          z = z / y;
          x = x / y;
          denom = 1.0 / (y * sqrt(1.0 + z*z + x*x));
        }
      } 
      else { /* x < y < z */
        if(1.0 + z > 1.0) {
          y = y / z;
          x = x / z;
          denom = 1.0 / (z * sqrt(1.0 + y*y + x*x));
        }
      }
    }

    if(1.0 + x + y + z > 1.0) {
      data[0] *= denom;
      data[1] *= denom;
      data[2] *= denom;
      return 1.0 / denom;
    }
    return 0.0;
  }

  String toString() {
    return "[" + data[0] + " " + data[1] + " " + data[2] + "]";
  }

}

class Colour{

  float [] data = new float[3];

  Colour(float r, float g, float b)
  {
    data[0] = r;
    data[1] = g;
    data[2] = b;
  } 

  Colour(Colour other) {
    data[0] = other.data[0];
    data[1] = other.data[1];
    data[2] = other.data[2];
  }

  void clamp()
  {
    for (int i = 0; i < 3; i++) {
      if (data[i] > 1.0)
      {
        data[i] = 1.0;
      } 
      else if (data[i] < 0.0)
      {
        data[i] = 0.0;
      } 
    }
  }

  Colour mul(double s)
  {
    Colour returnVal = new Colour((float)s*data[0], (float)s*data[1], (float)s*data[2]);
    return returnVal;
  }

  Colour mul(Colour other) {
    Colour returnVal = new Colour(data[0]*other.data[0], 
    data[1]*other.data[1], 
    data[2]*other.data[2]);
    return returnVal;
  }

  Colour add(Colour v)
  {
    Colour returnVal = new Colour(data[0]+v.data[0], data[1]+v.data[1], data[2]+v.data[2]);
    return returnVal;
  }

  String toString() {
    return "[" + data[0] + " " + data[1] + " " + data[2] + "]";
  }

}

class Vector4D {
  float [] data = new float[4];

  Vector4D(float x, float y, float z, float w) {
    data[0]=x;
    data[1]=y;
    data[2]=z;
    data[3]=w;
  }

  Vector4D(Vector4D other) {
    data[0] = other.data[0];
    data[1] = other.data[1];
    data[2] = other.data[2];
    data[3] = other.data[3];
  }
}

class Matrix4x4 {
  float data[][] = new float[4][4];

  Matrix4x4() {
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 4; j++) {
        data[i][j] = 0; 
      }
    }
    data[0][0] = 1.0;
    data[1][1] = 1.0;
    data[2][2] = 1.0;
    data[3][3] = 1.0;
  }

  Matrix4x4(Matrix4x4 other) {
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 4; j++) {
        data[i][j] = other.data[i][j]; 
      }
    }
  }

  Vector4D getRow(int row)
  {
    return new Vector4D(data[row][0], data[row][1], data[row][2], 
    data[row][3]);
  }

  Vector4D getColumn(int col) {
    return new Vector4D(data[0][col], data[1][col], data[2][col], 
    data[3][col]);
  }

  Matrix4x4 transpose(){
    Matrix4x4 M = new Matrix4x4(); 
    for (int i = 0; i < 4; i++) {
      for (int j = 0; j < 4; j++) {
        M.data[i][j] = this.data[j][i]; 
      }
    }
    return M;
  }

  Matrix4x4 mul(Matrix4x4 other) {
    Matrix4x4 ret = new Matrix4x4();

    for(int i = 0; i < 4; ++i) {
      Vector4D row = getRow(i);

      for(int j = 0; j < 4; ++j) {
        ret.data[i][j] = row.data[0] * other.data[0][j] + row.data[1] * other.data[1][j] + 
          row.data[2] * other.data[2][j] + row.data[3] * other.data[3][j];
      }
    }

    return ret;
  }

  Vector3D mul(Vector3D other) {
    return new Vector3D(
    other.data[0] * data[0][0] + other.data[1] * data[0][1] + other.data[2] * data[0][2],
    other.data[0] * data[1][0] + other.data[1] * data[1][1] + other.data[2] * data[1][2],
    other.data[0] * data[2][0] + other.data[1] * data[2][1] + other.data[2] * data[2][2]);

  }

  Point3D mul(Point3D other) {
    return new Point3D(
    other.data[0] * data[0][0] + other.data[1] * data[0][1] + other.data[2] * data[0][2] + data[0][3],
    other.data[0] * data[1][0] + other.data[1] * data[1][1] + other.data[2] * data[1][2] + data[1][3],
    other.data[0] * data[2][0] + other.data[1] * data[2][1] + other.data[2] * data[2][2] + data[2][3]);
  }

  String toString() {
    String s = ""; 
    for(int i=0; i<data.length; i++) {
      for(int j=0; j<data[i].length;j++) {
        s += data[i][j] + " "; 
      }
      s+= "\n";
    }
    return s;
  }

}

Vector3D transNorm(Matrix4x4 M, Vector3D n) {
  return new Vector3D(
  n.data[0] * M.data[0][0] + n.data[1] * M.data[1][0] + n.data[2] * M.data[2][0],
  n.data[0] * M.data[0][1] + n.data[1] * M.data[1][1] + n.data[2] * M.data[2][1],
  n.data[0] * M.data[0][2] + n.data[1] * M.data[1][2] + n.data[2] * M.data[2][2]);
}

class Material {
  // Ambient components for Phong shading.
  Colour ambient; 
  // Diffuse components for Phong shading.
  Colour diffuse;
  // Specular components for Phong shading.
  Colour specular;
  // Specular expoent.
  float specular_exp;

  Material( Colour am, Colour dif, Colour spec, float exp){
    ambient = am;
    diffuse = dif;
    specular = spec;
    specular_exp = exp;

  }
}

class Intersection {
  Point3D point;
  Vector3D normal;
  Material mat;
  double t_value;
  boolean none;
  Intersection(){
  }
}

class Ray3D {

  Point3D origin;
  Vector3D dir;

  Intersection intersection;

  Colour col;

  Ray3D(){
    origin = new Point3D(0,0,0);
    dir = new Vector3D(0,0,0);
    col = new Colour(0,0,0);
    intersection = new Intersection();
    intersection.none = true;
  }

  Ray3D( Point3D p, Vector3D v ) {
    origin = p;
    dir = v;
    intersection = new Intersection();
    intersection.none = true;
  }

}

interface LightSource {
  void shade(Ray3D ray);
  Point3D getPosition();
}

class PointLight implements LightSource {
  private Point3D pos;
  private Colour colAm;
  private Colour colDif;
  private Colour colSpec;

  PointLight(Point3D pos, Colour col) {
    this.pos = pos;
    colAm = col;
    colDif = col;
    colSpec = col;
  }

  PointLight(Point3D pos, Colour am, Colour dif, Colour spec) {
    this.pos = pos;
    colAm = am;
    colDif = dif;
    colSpec = spec;
  }

  void shade(Ray3D ray) {
    // Vectors needed for the diffuse and specular components.
    Vector3D LVec = new Vector3D(pos.sub(ray.intersection.point));
    LVec.normalize();

    float n_LVec = ray.intersection.normal.dot(LVec);
    if (n_LVec < 0.0){
      n_LVec = 0.0;
    }

    // Calculations of Ambient Component.
    Colour ambient = ray.intersection.mat.ambient.mul(colAm);
    ray.col = ray.col.add(ambient);

    // Calculations of Diffuse Component.
    Colour diffuse = ray.intersection.mat.diffuse.mul(colDif).mul(n_LVec);
    ray.col = ray.col.add(diffuse);


    Vector3D r = ray.intersection.normal.mul(2*n_LVec).sub(LVec);
    r.normalize();

    Point3D temp = ray.origin.sub(ray.intersection.point);
    Vector3D VVec = new Vector3D(temp.data[0],temp.data[1],temp.data[2]);
    VVec.normalize();

    float r_VVec = r.dot(VVec);
    if (r_VVec < 0.0) r_VVec = 0.0;

    Colour specular = 
      ray.intersection.mat.specular.mul(colSpec).mul(pow(r_VVec, 
    ray.intersection.mat.specular_exp));

    ray.col = ray.col.add(specular);

    // Clamp ray colour (0<=ray.col<=1).
    ray.col.clamp();
  }

  Point3D getPosition()
  {
    return pos; 
  }



}

interface SceneObject {
  boolean intersect(Ray3D ray, Matrix4x4 worldToModel, Matrix4x4 modelToWorld);
}

class UnitSquare implements SceneObject {
  boolean intersect(Ray3D ray, Matrix4x4 worldToModel, Matrix4x4 modelToWorld){
    // ****************************************************************
    // NOTE: See intersection.pdf to better understand calculations
    // ****************************************************************			

    // Normal of UnitSquare.
    Vector3D normal = new Vector3D(0,0,1);
    // Convert ray to model system.
    Vector3D m_dir = worldToModel.mul(ray.dir);
    Point3D m_p_0 = worldToModel.mul(ray.origin);

    Vector3D p_0 = new Vector3D(m_p_0.data[0],m_p_0.data[1],m_p_0.data[2]);

    float V_d = normal.dot(m_dir);
    float V_0 = -(normal.dot(p_0));

    // ray is parallel to plane. no intersection.
    if(V_d == 0)
      return false;


    float t = V_0/V_d;

    if(t < 0.0 || abs(t)<0.000001)
      return false;


    // Another intersection is closer and we should not change the values.
    if (!ray.intersection.none && t > ray.intersection.t_value || t<0.000001)
      return false;

    Point3D p = new Point3D(p_0.data[0]+(m_dir.data[0]*(t)), 
    p_0.data[1]+(m_dir.data[1]*(t)), 
    p_0.data[2]+(m_dir.data[2]*(t)));

    /* No intersection if point of intersection is 
     	   outside the bounds of square.*/
    if((-0.5 <= p.data[0] && p.data[0] <= 0.5 &&
      -0.5 <= p.data[1] && p.data[1] <= 0.5)) {
      ray.intersection.point = modelToWorld.mul(p);
      // Use worldToModel since it is the inverse of modelToWorld.
      ray.intersection.normal = transNorm(worldToModel,normal);
      ray.intersection.normal.normalize();
      ray.intersection.t_value = t;
      ray.intersection.none = false;
      return true;
    }
    return false;
  }

}


class UnitSphere implements SceneObject {
  boolean intersect(Ray3D ray, Matrix4x4 worldToModel, Matrix4x4 modelToWorld){
    // ****************************************************************
    // NOTE: See intersection.pdf to better understand calculations
    // ****************************************************************			

    // Convert ray to model system.
    Vector3D m_dir = worldToModel.mul(ray.dir);
    Point3D m_p_0 = worldToModel.mul(ray.origin);

    Vector3D p_0 = new Vector3D(m_p_0.data[0],m_p_0.data[1],m_p_0.data[2]);
    
    float p0_v = p_0.dot(m_dir);
    float p0_p0 = p_0.dot(p_0);
    float v_v = m_dir.dot(m_dir);
    
    float d1 = pow(2.0*p0_v, 2.0) - 4*(v_v)*(p0_p0-1);
    
    if(d1 < 0) {
       return false; 
    }
    
    double t = (d1 == 0) ? -p0_v/v_v : (-2*p0_v - sqrt(d1))/(2*v_v);
    
    
    if(!ray.intersection.none && t > ray.intersection.t_value || t<0.000001) {
      return false;
    }
    
    Point3D p = new Point3D((float)(p_0.data[0] + t*m_dir.data[0]), 
					(float)(p_0.data[1] + t*m_dir.data[1]), 
					(float)(p_0.data[2] + t*m_dir.data[2]));

    ray.intersection.none = false;
    
    Vector3D theNormal = new Vector3D(p.data[0], p.data[1], p.data[2]);
    
    theNormal.normalize();
    
    ray.intersection.point = modelToWorld.mul(p);
    
    ray.intersection.normal = transNorm(worldToModel,theNormal);
    ray.intersection.normal.normalize();
    ray.intersection.t_value = t;
    return true;
  }

}

class SceneNode {
  SceneObject object;
  Material material;
  Matrix4x4 trans;
  Matrix4x4 invTrans;

  SceneNode(SceneObject obj, Material mat, Matrix4x4 trans, Matrix4x4 invTrans) {
    object = obj;
    material = mat;
    this.trans = trans;
    this.invTrans = invTrans;
  } 

}

class RayTracer {
  int height;
  int width;
  ArrayList objects;
  ArrayList lights;
  Matrix4x4 modelToWorld;
  Matrix4x4 worldToModel;
  Colour[][] colBuffer;

  RayTracer(int height,int width)
  {
    objects = new ArrayList();
    lights = new ArrayList();
    modelToWorld = new Matrix4x4();
    worldToModel = new Matrix4x4();
    this.height = height;
    this.width = width;
    colBuffer = new Colour[(int)width][(int)height];
  }

  SceneNode addObject(SceneObject obj, Material mat) {
    SceneNode node = new SceneNode(obj, mat, new Matrix4x4(), new Matrix4x4());
    objects.add(node);
    return node; 
  }

  void translate(SceneNode node, Vector3D trans) {
    Matrix4x4 translation = new Matrix4x4();

    translation.data[0][3] = trans.data[0];
    translation.data[1][3] = trans.data[1];
    translation.data[2][3] = trans.data[2];

    node.trans = node.trans.mul(translation);

    translation.data[0][3] = -trans.data[0];
    translation.data[1][3] = -trans.data[1];
    translation.data[2][3] = -trans.data[2];

    node.invTrans = node.invTrans.mul(translation);

  }
  
  void rotate(SceneNode node, char axis, float angle ) {
    Matrix4x4 rotation = new Matrix4x4();
    float toRadian = 2*PI/360.0;
    int i;
	
    for (i = 0; i < 2; i++) {
      switch(axis) {
	case 'x':
		rotation.data[0][0] = 1;
                rotation.data[1][1] = cos(angle*toRadian);
		rotation.data[1][2] = -sin(angle*toRadian);
		rotation.data[2][1] = sin(angle*toRadian);
		rotation.data[2][2] = cos(angle*toRadian);
		rotation.data[3][3] = 1;
		break;
        case 'y':
		rotation.data[0][0] = cos(angle*toRadian);
		rotation.data[0][2] = sin(angle*toRadian);
		rotation.data[1][1] = 1;
		rotation.data[2][0] = -sin(angle*toRadian);
		rotation.data[2][2] = cos(angle*toRadian);
		rotation.data[3][3] = 1;
		break;
        case 'z':
	        rotation.data[0][0] = cos(angle*toRadian);
		rotation.data[0][1] = -sin(angle*toRadian);
		rotation.data[1][0] = sin(angle*toRadian);
		rotation.data[1][1] = cos(angle*toRadian);
		rotation.data[2][2] = 1;
		rotation.data[3][3] = 1;
		break;
	}
	if (i == 0) {
	    node.trans = node.trans.mul(rotation); 	
            angle = -angle;
        } 
	else {
	    node.invTrans = rotation.mul(node.invTrans); 
	}	
    } 
    
    
  }

  private Colour shadeRay(Ray3D ray, int depth) {
    traverseScene(ray);

    if(!ray.intersection.none) {
      computeShading(ray);
      return ray.col;
    }

    return new Colour(0,0,0);
  }

  private void computeShading(Ray3D ray) {
    LightSource curLight;
    for(int i=0; i<lights.size(); i++) {
      curLight = (LightSource) lights.get(i);

      curLight.shade(ray);

    }


  }
  private void traverseScene(Ray3D ray) {

    for(int i=0; i<objects.size(); i++) {
      SceneNode node = (SceneNode)objects.get(i);
      modelToWorld = modelToWorld.mul(node.trans);
      worldToModel = node.invTrans.mul(worldToModel);

      if(node.object.intersect(ray, worldToModel, modelToWorld)) {
        ray.intersection.mat = node.material;
      } 
    }

    for(int i=objects.size()-1; i>=0;i--) {
      SceneNode node = (SceneNode)objects.get(i);
      worldToModel = node.trans.mul(worldToModel);
      modelToWorld = modelToWorld.mul(node.invTrans);
    }

  }

  private Matrix4x4 initInvViewMatrix(Point3D eye, Vector3D view, Vector3D up) {
    Matrix4x4 mat = new Matrix4x4(); 
    Vector3D w;
    view.normalize();
    up = up.sub(view.mul(up.dot(view)));

    up.normalize();
    w = view.cross(up);

    mat.data[0][0] = w.data[0];
    mat.data[1][0] = w.data[1];
    mat.data[2][0] = w.data[2];
    mat.data[0][1] = up.data[0];
    mat.data[1][1] = up.data[1];
    mat.data[2][1] = up.data[2];
    mat.data[0][2] = -view.data[0];
    mat.data[1][2] = -view.data[1];
    mat.data[2][2] = -view.data[2];
    mat.data[0][3] = eye.data[0];
    mat.data[1][3] = eye.data[1];
    mat.data[2][3] = eye.data[2];
    return mat; 
  }
}


int i = 0;
int j = 0;
Camera cam = new Camera();
void draw() {
  

  double factor = (height/2.0)/tan((float)cam.fov*PI/360.0);

  Matrix4x4 viewToWorld = raytracer.initInvViewMatrix(cam.eye,cam.view,cam.up);
  int size = (int)(height * width);

  for(int iPrime=i; (iPrime < height && iPrime < i + 10); iPrime++)  {
      for(int jPrime = j; (jPrime < width && jPrime < j + 10); jPrime++) {
        Point3D origin = new Point3D(0,0,0);
        Point3D imagePlane = new Point3D(
        (float)((-width/2.0 + 0.5 + jPrime)/factor),
        (float)((-height/2.0 + 0.5 + iPrime)/factor),
            -1.0);
    
        Ray3D ray = new Ray3D();
    
        origin = viewToWorld.mul(origin);
        imagePlane = viewToWorld.mul(imagePlane);
    
        ray.origin = origin;
    
        ray.dir.data[0] = imagePlane.data[0] - origin.data[0];
        ray.dir.data[1] = imagePlane.data[1] - origin.data[1];
        ray.dir.data[2] = imagePlane.data[2] - origin.data[2];
    
        Colour col = raytracer.shadeRay(ray, 0);
    
        color c = color(col.data[0] * 255, col.data[1] * 255, col.data[2] * 255);  
    
        //this.colBuffer[i][j] = col;
        
        set(iPrime,jPrime,c);
      }
  }
  
  if(i+10 > width) {
     i = 0; 
     j += 10;
  } else {
     i++;
  }
}