import processing.opengl.*;
import controlP5.*;

ControlP5 controlP5;
Slider s;
Slider sg;

int COUNT = 20; // gęstość siatki
float[][] cpX, cpY, cpZ; // punkty kontrolne
float ax, ay, az;
boolean light = true;
boolean grid = false;
boolean normals = true;
boolean points = true;
int typeX = 0;
int typeY = 0;

void setup()
{
  size(600,600,OPENGL);
  
  controlP5 = new ControlP5(this);
  s = controlP5.addSlider("rotation",0,360,126,3,582,332,17);
  s.setSliderMode(0);
  sg = controlP5.addSlider("grid_size",2,30,20,3,562,332,17);
  sg.setSliderMode(0);
  controlP5.addButton("control points",10,3,2,80,18).setId(1);
  controlP5.addButton("grid ",10,86,2,55,18).setId(2);
  controlP5.addButton("normals ",10,144,2,90,18).setId(3);
  controlP5.addButton("kind of bicubic path",10,237,2,100,18).setId(4);
  
  ax = ay = az = 0; 
  ax = 3 * PI/10;
  az = 7 * PI/10;
  
 
  cpX = new float[][] { { 0.23 * width, 0.41 * width, 0.59 * width, 0.77 * width },
                        { 0.23 * width, 0.41 * width, 0.59 * width, 0.77 * width },
                        { 0.23 * width, 0.41 * width, 0.59 * width, 0.77 * width },
                        { 0.23 * width, 0.41 * width, 0.59 * width, 0.77 * width } };
                     
  cpY = new float[][] { { 0.23 * height, 0.23 * height, 0.23 * height, 0.23 * height },                     
                        { 0.41 * height, 0.41 * height, 0.41 * height, 0.41 * height },                     
                        { 0.59 * height, 0.59 * height, 0.59 * height, 0.59 * height },                                          
                        { 0.77 * height, 0.77 * height, 0.77 * height, 0.77 * height } };
                              
  cpZ = new float[][] { { 0, 0, 0, 0},   
                        { 0, 0.3 * width, 0.3 * width, 0},   
                        { 0, 0.3 * width, 0.3 * width, 0},   
                        { 0, 0, 0, 0} };
}

float[] calculateNormals(float[] v1, float[] v2) {
  if (v1.length == 3 && v2.length == 3) {
    float[] n = new float[3]; 

    n[0] = v1[1] * v2[2] - v1[2] * v2[1];
    n[1] = v1[2] * v2[0] - v1[0] * v2[2];
    n[2] = v1[0] * v2[1] - v1[1] * v2[0];
    
    // normalizacja
    
    float l = sqrt(pow(n[0],2) + pow(n[1],2) + pow(n[2],2));
    if (l > 0) {
      n[0] /= l;
      n[1] /= l;
      n[2] /= l;
    }    
    
    return n;
  }
  return null; 
}

void bicubic_patch(int tu, int tv,float[][] cpX, float[][] cpY, float[][] cpZ) {
  float[][] duX, duY, duZ;
  float[][] dvX, dvY, dvZ;
  float[][][] NP; // wektory normalne
  
  float[][] mp0 = { {-1, 3,-3, 1 }, 
                    { 3,-6, 3, 0 }, 
                    {-3, 3, 0, 0 }, 
                    { 1, 0, 0, 0 } };
  
  float[][] mp1 = { {-0.5, 1.0,-0.5, 0.0 }, 
                   { 1.5,-2.5, 0.0, 1.0 }, 
                   {-1.5, 2.0, 0.5, 0.0 }, 
                   { 0.5,-0.5, 0.0, 0.0 } };                 
  
  float[][] mp2 = { {-1.0/6, 3.0/6,-3.0/6, 1.0/6 }, 
                    { 3.0/6,-6.0/6,   0, 4.0/6 }, 
                    {-3.0/6, 3.0/6, 3.0/6, 1.0/6 }, 
                    { 1.0/6,   0,   0,   0 } };                   
                   
  float[][] mp3 = { { 2,-3, 0, 1 }, 
                    { 1,-2, 1, 0 }, 
                    {-2, 3, 0, 0 }, 
                    { 1,-1, 0, 0 } }; 
  
  float[][] mp4 = { {-1,    1, -0.5, 2 }, 
                    { 2.5, -2.5,    0, 0 }, 
                    {-3,    2,  0.5, 0 }, 
                    { 1, -0.5,    0, 0 } };               
  
  float[][] mpu, mpv;
  
  if (tu == 1) mpu = mp1;                
  else if (tu == 2) mpu = mp2;
  else if (tu == 3) mpu = mp3;
  else if (tu == 4) mpu = mp4;
  else mpu = mp0;                
  
  if (tv == 1) mpv = mp1;                
  else if (tv == 2) mpv = mp2;
  else if (tv == 3) mpv = mp3;
  else if (tv == 4) mpv = mp4;
  else mpv = mp0;                
   
  float[][] fpX = new float[COUNT+1][COUNT+1]; 
  float[][] fpY = new float[COUNT+1][COUNT+1]; 
  float[][] fpZ = new float[COUNT+1][COUNT+1]; 
  
  Matrix PX = new Matrix(cpX);
  Matrix PY = new Matrix(cpY);
  Matrix PZ = new Matrix(cpZ);
  
  Matrix MPU = new Matrix(mpu);
  Matrix MPUT = MPU.transpose(); 
  
  Matrix MPV = new Matrix(mpv);
  Matrix MPVT = MPV.transpose(); 
  
  for (int i = 0; i <= COUNT; i++) {
    float u = float(i) / float(COUNT);
    float[][] mu = { {pow(u,3), pow(u,2), u, 1} };
    Matrix MU = new Matrix(mu);
    MU = MU.times(MPUT);
    Matrix MUX = MU.times(PX);
    Matrix MUY = MU.times(PY);
    Matrix MUZ = MU.times(PZ);
   
    for (int j = 0; j <= COUNT; j++) {
      float v = float(j) / float(COUNT);
      float[][] mv = { {pow(v,3)}, {pow(v,2)}, {v}, {1} };
      Matrix MV = new Matrix(mv);      
      MV = MPV.times(MV);
      Matrix X = MUX.times(MV);
      Matrix Y = MUY.times(MV);
      Matrix Z = MUZ.times(MV);
                  
      fpX[i][j] = X.getVal(0,0);
      fpY[i][j] = Y.getVal(0,0);
      fpZ[i][j] = Z.getVal(0,0);
    }
  }
  
  // obliczenie wektorów normalnych

  duX = new float[COUNT+1][COUNT+1]; 
  duY = new float[COUNT+1][COUNT+1]; 
  duZ = new float[COUNT+1][COUNT+1]; 

  for (int i = 0; i <= COUNT; i++) {
    float u = float(i) / float(COUNT);
    float[][] mu = { { 3 * pow(u,2), 2 * u, 1, 0} };
    Matrix MU = new Matrix(mu);
    MU = MU.times(MPUT);
    Matrix MUX = MU.times(PX);
    Matrix MUY = MU.times(PY);
    Matrix MUZ = MU.times(PZ);
   
    for (int j = 0; j <= COUNT; j++) {
      float v = float(j) / float(COUNT);
      float[][] mv = { {pow(v,3)}, {pow(v,2)}, {v}, {1} };
      Matrix MV = new Matrix(mv);      
      MV = MPV.times(MV);
      Matrix X = MUX.times(MV);
      Matrix Y = MUY.times(MV);
      Matrix Z = MUZ.times(MV);
                  
      duX[i][j] = X.getVal(0,0);
      duY[i][j] = Y.getVal(0,0);
      duZ[i][j] = Z.getVal(0,0);
    }
  }

  dvX = new float[COUNT+1][COUNT+1]; 
  dvY = new float[COUNT+1][COUNT+1]; 
  dvZ = new float[COUNT+1][COUNT+1]; 

  for (int i = 0; i <= COUNT; i++) {
    float u = float(i) / float(COUNT);
    float[][] mu = { {pow(u,3), pow(u,2), u, 1} };
    Matrix MU = new Matrix(mu);
    MU = MU.times(MPUT);
    Matrix MUX = MU.times(PX);
    Matrix MUY = MU.times(PY);
    Matrix MUZ = MU.times(PZ);
   
    for (int j = 0; j <= COUNT; j++) {
      float v = float(j) / float(COUNT);
      float[][] mv = { {3 * pow(v,2)}, { 2 * v}, {1}, {0} };
      Matrix MV = new Matrix(mv);      
      MV = MPV.times(MV);
      Matrix X = MUX.times(MV);
      Matrix Y = MUY.times(MV);
      Matrix Z = MUZ.times(MV);
                  
      dvX[i][j] = X.getVal(0,0);
      dvY[i][j] = Y.getVal(0,0);
      dvZ[i][j] = Z.getVal(0,0);
    }
  }

  NP = new float[COUNT+1][COUNT+1][3];   
  
  for (int i = 0; i < COUNT+1; i++) 
    for (int j = 0; j < COUNT+1; j++) {
      float[] p1 = {duX[i][j],duY[i][j],duZ[i][j]};
      float[] p2 = {dvX[i][j],dvY[i][j],dvZ[i][j]};
  
      float[] nP = calculateNormals(p1,p2);
      
      for (int k = 0; k < 3; k++) {
        NP[i][j][k] = nP[k];
      }
    }
    
   for (int i = 0; i < COUNT; i++) 
    for (int j = 0; j < COUNT; j++) {
      beginShape();         
        if (normals) {
          normal(NP[i][j][0],NP[i][j][1],NP[i][j][2]);
        }
        vertex(fpX[i][j],fpY[i][j],fpZ[i][j]);
        if (normals) {
          normal(NP[i+1][j][0],NP[i+1][j][1],NP[i+1][j][2]);
        }
        vertex(fpX[i+1][j],fpY[i+1][j],fpZ[i+1][j]);
        if (normals) {
          normal(NP[i][j+1][0],NP[i][j+1][1],NP[i][j+1][2]);
        }
        vertex(fpX[i][j+1],fpY[i][j+1],fpZ[i][j+1]);
      endShape(CLOSE);
      beginShape();
        if (normals) {
          normal(NP[i+1][j][0],NP[i+1][j][1],NP[i+1][j][2]);
        }
        vertex(fpX[i+1][j],fpY[i+1][j],fpZ[i+1][j]);
        if (normals) {
          normal(NP[i+1][j+1][0],NP[i+1][j+1][1],NP[i+1][j+1][2]);
        }
        vertex(fpX[i+1][j+1],fpY[i+1][j+1],fpZ[i+1][j+1]);
        if (normals) {
          normal(NP[i][j+1][0],NP[i][j+1][1],NP[i][j+1][2]);
        }
        vertex(fpX[i][j+1],fpY[i][j+1],fpZ[i][j+1]);
      endShape(CLOSE);        
    }
}

void draw()
{
  background(0);
  
  if(light)
    lights();
  
  pushMatrix();
  
  translate(width/2,height/2,0);
  rotateX(ax);
  rotateY(ay);
  rotateZ(az);
  translate(-width/2,-height/2,0);

  noStroke();
  fill(#00ff00);
  
  if (points)
  for (int i = 0; i < 4; i++) 
    for (int j = 0; j < 4; j++) {
      translate(cpX[i][j],cpY[i][j],cpZ[i][j]);
      sphere(3);
      translate(-cpX[i][j],-cpY[i][j],-cpZ[i][j]);
    }

  if (grid) {
    noFill();
    stroke(#ff0000);
  } else {
    fill(#ffff00);
    noStroke();
  }
  
  bicubic_patch(typeX, typeY, cpX, cpY, cpZ);
  
  popMatrix();
  
  controlP5.draw();
}


void controlEvent(ControlEvent theEvent) {
  switch(theEvent.controller().id()) {
    case(1):
      points = !points;
    break;
    case(2):
      grid = !grid;
    break;
    case(3):
      normals = !normals;
    break;  
    case(4):
      nextType();
    break;  
  }
}

int type = 0;

void nextType() {
  type++;
  if (type == 4) type = 0;
  switch (type) {
     case 0:
      typeX=0; typeY=0;
      break;
     case 1:
      typeX=1; typeY=1;
      break;
     case 2:
      typeX=2; typeY=2;
      break;
     case 3:
      typeX=0; typeY=1;
      break;
  }
}

void rotation(float rot) {
  az = rot / 180 * PI;
}

void grid_size(float rot) {
  COUNT = (int)rot;
}