// loads triple v from components x, y, z
void twTripleInit(twTriple v, GLfloat x, GLfloat y, GLfloat z) {
    v[0] = x;
    v[1] = y;
    v[2] = z;
}

// prints a triple to stdout. The first argument can be used to label the
// triple.
void twTriplePrint(char* name, twTriple v) {
    printf("%s=(%f,%f,%f)\n",name,v[0],v[1],v[2]);
}

// copies a triple components to another triple.  V is copied to W
void twTripleCopy(twTriple w, twTriple v) {
    w[0]=v[0];
    w[1]=v[1];
    w[2]=v[2];
}

// returns the dot product of vector v and vector w.
GLfloat twDot(twTriple v, twTriple w) {
    return (v[0]*w[0]+v[1]*w[1]+v[2]*w[2]);
}

// returns the cosine of the angle between vector v and vector w.
// does not assume that they are normalized
GLfloat twCosAngle(twTriple v, twTriple w) {
    return twDot(v,w)/sqrt(twDot(v,v)*twDot(w,w));
}

// returns the length of the vector v
GLfloat twVectorLength(twTriple v) {
    return sqrt(twDot(v,v));
}

// creates vector v as a scalar multiple of w, multiplying each component
// by k.  It's safe to have v be the same object as w.
GLfloat twVectorScale(twTriple v, twTriple w, GLfloat k) {
    v[0]=w[0]*k;
    v[1]=w[1]*k;
    v[2]=w[2]*k;
}

// modifies the vector to be of unit length.  Returns with v unmodified if
// v is the zero vector.
void twVectorNormalize(twTriple v) {
    GLfloat len=twVectorLength(v);
    if(len==0.0) return;
    twVectorScale(v,v,1/len);
}

// computes u=v x w, the cross product of vectors v and w, in that order
void twCrossProduct(twTriple u, twTriple v, twTriple w) {
    u[0] = v[1]*w[2]-v[2]*w[1];
    u[1] = v[2]*w[0]-v[0]*w[2];
    u[2] = v[0]*w[1]-v[1]*w[0];
}

// computes v=A-B, the vector from point B to point A
void twVector(twTriple v, twTriple A, twTriple B) {
    v[0]=A[0]-B[0];
    v[1]=A[1]-B[1];
    v[2]=A[2]-B[2];
}

// Computes B=A+v, the point that is the sum of point A and vector v
void twPoint(twTriple B, twTriple A, twTriple v) {
    B[0]=A[0]+v[0];
    B[1]=A[1]+v[1];
    B[2]=A[2]+v[2];
}

// computes the squared distance between points A and B
GLfloat twPointDistance2(twTriple A, twTriple B) {
    twTriple v;
    twVector(v,A,B);
    return twDot(v,v);
}

// computes the distance between points A and B
GLfloat twPointDistance(twTriple A, twTriple B) {
  return sqrt(twPointDistance2(A,B));
}

GLfloat twPlaneNormal(twTriple N, twTriple C, twTriple D, twTriple E) {
    twTriple V, W;

    twVector(V,D,C);
    twVector(W,E,C);
    twCrossProduct(N,V,W);
    // double check
    GLfloat d1 = twDot(N,C);
    GLfloat d2 = twDot(N,D);
    GLfloat d3 = twDot(N,E);
    if(d1==d2 && d2==d3) return d1;
    twMessage(TW_GEOMETRY,"Dot Products differ: %f %f %f\n",d1, d2, d3);
}

// Computes Q=P+Vt, a point on the line defined by point P and vector V
void twPointOnLine(twTriple Q, twTriple P, twTriple V, GLfloat t) {
    Q[0] = P[0]+V[0]*t;
    Q[1] = P[1]+V[1]*t;
    Q[2] = P[2]+V[2]*t;
}

GLfloat twLinePlaneIntersection_old( twTriple P, twTriple V,
                                     twTriple A, twTriple B, twTriple C,
                                     twTriple IP) {
    twTriple N;

    GLfloat planeConstant = twPlaneNormal(N,A,B,C);
    //printf("Plane EQ = %fx + %fy + %fz = %f\n",N[0],N[1],N[2],planeConstant);
    GLfloat linearTerm = 0;
    GLfloat constantTerm = planeConstant;
    for(int i=0; i<3; i++) {
        linearTerm   += N[i]*V[i];
        constantTerm -= N[i]*P[i];
    }
    //printf("IP EQ = %ft = %f\n", linearTerm, constantTerm);
    GLfloat parameter = constantTerm/linearTerm;
    //printf("Intersection Parameter = %f\n",parameter);
    for(int i=0; i<3; i++) 
        IP[i] = parameter*V[i]+P[i];
    //twTriplePrint("IP", IP);
    return parameter;
}

bool twLinePlaneIntersection( twTriple P, twTriple V,
                              twTriple Q, twTriple N,
                              GLfloat& t, twTriple IP) {
    GLfloat vn = twDot(V,N);
    if(0==vn) return false;
    twTriple w;
    twVector(w,Q,P);
    t = twDot(w,N)/vn;
    twPointOnLine(IP,P,V,t);
    return true;
}

bool twPointInTriangle( twTriple I,
                        twTriple P, twTriple U, twTriple V,
                        GLfloat& s, GLfloat& t) {

    twTriple W;
    twVector(W,I,P);

    GLfloat uu = twDot(U,U);
    GLfloat uv = twDot(U,V);
    GLfloat vv = twDot(V,V);
    GLfloat wu = twDot(W,U);
    GLfloat wv = twDot(W,V);
    GLfloat denom = uv*uv-uu*vv;

    if(0 == uu || 0 == vv ) {
        printf("zero length vectors\n");
        return false;
    }
    if(0 == denom ) {
        printf("Degenerate triangle:  u and v point the same way.\n");
        return false;
    }

    s = (uv*wv-vv*wu)/denom;
    t = (uv*wu-uu*wv)/denom;
    return true;
    // test code.  Comment out the "return" statement on the previous
    // line to print the computed value of W2 to compare with the
    // original W.
    twTriple W2 = {s*U[0]+t*V[0],
                   s*U[1]+t*V[1],
                   s*U[2]+t*V[2]};
    // these two should be equal
    twTriplePrint("W",W);
    twTriplePrint("W2",W2);
    return true;
}

bool twLineTriangleIntersection( twTriple P, twTriple lineV,
                                 twTriple A, twTriple B, twTriple C,
                                 twTriple IP, 
                                 GLfloat& r, GLfloat& s, GLfloat& t) {
    twTriple U,V,N;
    twVector(U,B,A);
    twVector(V,C,A);
    twCrossProduct(N,U,V);

    if( !twLinePlaneIntersection(P,lineV,A,N,r,IP) ) {
        twMessage(TW_GEOMETRY,"Line from (%f,%f,%f) in dir (%f,%f,%f) doesn't intersect plane through triangle (%f,%f,%f), (%f,%f,%f), (%f,%f,%f)\n",
                  P[0],P[1],P[2],
                  lineV[0],lineV[1],lineV[2],
                  A[0],A[1],A[2],
                  B[0],B[1],B[2],
                  C[0],C[1],C[2]);
        return false;
    }
    return twPointInTriangle(IP,A,U,V,s,t);
}

// ================================================================

bool twNearestFragment(twFragment* fragments, int n,
                       twTriple P, twTriple V,
                       twTriple IP, GLfloat& r) {
    twTriple dir; // intersection point
    GLfloat s,t;
    
    twTripleCopy(dir,V);
    twVectorNormalize(dir);
    GLfloat minr = FLT_MAX;
    bool found=false;
    for(int i=0;i<n;i++) {
        /*
          twTriplePrint("A",fragments[i][0]);
          twTriplePrint("B",fragments[i][1]);
          twTriplePrint("C",fragments[i][2]);
        */
        if(!twLineTriangleIntersection(P,dir,
                                       fragments[i][0],
                                       fragments[i][1],
                                       fragments[i][2],
                                       IP,r,s,t)) continue;
        //printf("frag %d at %f\n",i,r);
        // negative intersections are not interesting
        if(r<0) continue;
        // we want to find the nearest intersection
        if(r>minr) continue;
        // printf("s=%f t=%f\n",s,t);
        if(0<=s && s<=1 && 0<=t && t <= 1) {
            found=true;
            minr = r;
            // printf("new nearest: frag=%d r=%f s=%f t=%f\n",i,r,s,t);
        }
    }
    r=minr;
    twPointOnLine(IP,P,dir,minr);
    return found;
}


// Computes the product of matrix M (a 4x4 matrix in column major order)
// and a vector v (4x1).  The last element of v is typically 1.0 for a
// point and 0.0 for a vector.  This function is not used.
void mult4(GLfloat* dest, GLfloat* M, GLfloat* v) {
    int i,j;
    GLfloat temp[4];
    
    for(i=0; i<4;i++) {
        temp[i]=0;
        for(j=0;j<4;j++) 
            temp[i] += M[j*4+i]*v[j];
    }
    for(i=0; i<4; i++) dest[i]=temp[i];
}

// Computes the product of matrix M (a 4x4 matrix in column major order)
// and a triple v (3x1).  The last row and column of M are ignored.  See
// mult4.
void mult3(twTriple dest, GLfloat* M, twTriple v) {
    int i,j;
    twTriple temp;
    
    for(i=0; i<3;i++) {
        temp[i]=0;
        for(j=0;j<3;j++) {
            // has to be 4 because the rank of the real matrix is 4,
            // even though we're using a 3x3 submatrix
            temp[i] += M[j*4+i]*v[j];
        }
    }
    for(i=0; i<3; i++) dest[i]=temp[i];
}

// Computes the product of matrix M (a 4x4 matrix in column major order)
// and a triple v (3x1).  The last row and column of M are ignored.  See
// mult4.
void mult3d(twTriple dest, GLdouble* M, twTriple v) {
    int i,j;
    twTriple temp;
    
    for(i=0; i<3;i++) {
        temp[i]=0;
        for(j=0;j<3;j++) {
            // has to be 4 because the rank of the real matrix is 4,
            // even though we're using a 3x3 submatrix
            temp[i] += M[j*4+i]*v[j];
        }
    }
    for(i=0; i<3; i++) dest[i]=temp[i];
}

// Prints a 4x4 column-major matrix, plus a name
void twPrintMatrix4x4(char* name, GLdouble M[16]) {
    printf("Matrix %s\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n",
           name,
           M[0],M[4],M[8],M[12],
           M[1],M[5],M[9],M[13],
           M[2],M[6],M[10],M[14],
           M[3],M[7],M[11],M[15]);
}