// cross_general.c  vector cross product generalized to higher dimensions   
// cos(angle between A C) = (A dot C) / (|A|*|C|)
//                        = just A dot C if normalized length = 1
// stated simply: the dot product of two orthogonal vectors is zero.
//
// Cross product A x B = C, C is orthogonal to A and B, cos(Pi/2)=0 
//
// 3 dimensions i, j, k unit vectors  C = |c1, c2, c3| = A x B
//   given A = |a1, a2, a3|   B = |b1, b2, b3|  orthogonal
//                                               
//          | i  j  k|  c1 =  det||a2,a3|,|b2,b3|| = a2*b3 - a3*b2
//  C = det |a1 a2 a3|  c2 = -det||a1,a3|,|b1,b3|| = a3*b1 - a1*b3
//          |b1 b2 b3|  c3 =  det||a1,a2|,|b1,b2|| = a1*b2 - a2*b1
//
//  where det||a2,a3|,|b2,b3|| = determinant of matrix |a2 a3|
//                                                     |b2 b3|
//
//
// Cross product A x B x C = D, D is orthogonal to A and B and C
// 4 dimensions i, j, k, l unit vectors  D = |d1, d2, d3, d4| = A x B x C
//   given A = |a1, a2, a3, a4|  and B and C  orthogonal
//
//          | i  j  k  l|  d1 =  det||a2,a3,a4|,|b2,b3,b4|,|c2,c3,c4||
//  D = det |a1 a2 a3 a4|  d2 = -det||a1,a3,a4|,|b1,b3,b4|,|c1,c3,c4||
//          |b1 b2 b3 b4|  d3 =  det||a1,a2,a4|,|b1,b2,b4|,|c1,c2,c4||
//          |c1 c2 c3 c4|  d4 = -det||a1,a2,a3|,|b1,b2,b3|,|c1,c2,c3||
//  D is orthogonal to A and B and C
//  note 3 by 3 sub matrices exclude row for index being computed
//
//        |a2 a3 a4|         |a1 a3 a4|        |a1 a2 a4|         |a1 a2 a3|
//  d1=det|b2 b3 b4|  d2=-det|b1 b3 b4|  d3=det|b1 b2 b4|  d4=-det|b1 b2 b3|
//        |c2 c3 c4|         |c1 c3 c4|        |c1 c2 c4|         |c1 c2 c3|
//
//  Also note D = B x C  can be computed as sums of products of
//  two terms, by setting  A = |1,1,1,1| because
//  multiplying by a term = 1, does not change the value
//
//  d1 = det|b3 b4| - det|b2 b4| + det|b2 b3|    be careful of minus signs
//          |c3 c4|      |c2 c4|      |c2 c3|    use typical + then -
//
//  d1 = b3*c4 - b4*c3 + b4*c2 - b2*c4 + b2*c3 - b3*c2
//  d2 = b4*c3 - b3*c4 + b1*c4 - b4*c1 + b3*c1 - b1*c3 
//  d3 = b2*c4 - b4*c2 + b4*c1 - b1*c4 + b1*c2 - b2*c1
//  d4 = b3*c2 - b2*c3 + b1*c3 - b3*c1 + b2*c1 _ b1*c2
//
//  By setting A to one of the four unit vectors, e.g. A = |1, 0, 0, 0|
//  then fewer terms need to be computed for D = B x C
//    d1 = 0
//    d2 = b4*c3 - b3*c4 
//    d3 = b2*c4 - b4*c2
//    d4 = b3*c2 - b2*c3
//
//
// Cross product E = A x B x C X D  orthogonal to A, B, C, and D
// 5 dimensions i, j, k, l, m unit vectors,  E = |e1, e2, e3, e4, e5|
//   given A = |a1, a2, a3, a4, a5| and B and C and D  orthogonal
//
//          | i  j  k  l  m|  e1 =  det|not row 1, not col 1|
//          |a1 a2 a3 a4 a5|  e2 = -det|not row 1, not col 2|
//   E = det|b1 b2 b3 b4 b5|  e3 =  det|not row 1, not col 3|
//          |c1 c2 c3 c4 c5|  e4 = -det|not row 1, not col 4|
//          |d1 d2 d3 d4 d5|  e5 =  det|not row 1, not col 5|
//  
//
//          |a2 a3 a4 a5|           |a1 a3 a4 a5|          |a1 a2 a4 a5|
//  e1 = det|b2 b3 b4 b5|  e2 = -det|b1 b3 b4 b5|  e3 = det|b1 b2 b4 b5|
//          |c2 c3 c4 c5|           |c1 c3 c4 c5|          |c1 c2 c4 c5|
//          |d2 d3 d4 d5|           |d1 d3 d4 d5|          |d1 d2 d4 d5|
//
//          |a1 a2 a3 a5|           |a1 a2 a3 a4|
//  e4 = det|b1 b2 b3 b5|  e5 = -det|b1 b2 b3 b4|
//          |c1 c2 c3 c5|           |c1 c2 c3 c4|
//          |d1 d2 d3 d5|           |d1 d2 d3 d4|
//
// in general, for any
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "simeq_plus.h"

static void cross3(double A[], double B[], double C[])
{
  C[0] = A[1]*B[2] - A[2]*B[1];
  C[1] = A[2]*B[0] - A[0]*B[2];
  C[2] = A[0]*B[1] - A[1]*B[0];
} // end cross3

static void cross3d(double M3[], double C[])
{
  double A2[4]; // (n-1)*(n-1) matrix  n times
  int n=3;
  int i, j, k, ii, jj;

  for(k=0; k<n; k++)
  {
    for(i=1; i<n; i++) // skip top row  all 1.0
    {
      jj = 0;
      for(j=0; j<n; j++)
      {
        if(j==k) continue; // skip row k
        jj = jj +1;
        // printf("k=%1d, i=%1d, j=%1d, ii=%1d, jj=%1d, ix=%d \n",
	//       k, i, j, i-1, jj-1, i*n+j);
        A2[(i-1)*(n-1)+(jj-1)] = M3[i*n+j];
      } // j
    } // i
    mat_put(n-1, A2);
    C[k] = determinant(n-1, A2);
    if(k%2==1) C[k] = - C[k];
    printf("k=%d, det=%f \n", k, C[k]);
  } // k
} // end cross3d

static void cross4d(double M4[], double C[])
{
  double A3[9]; // (n-1)*(n-1) matrix  n times
  int n=4;
  int i, j, k, ii, jj;

  for(k=0; k<n; k++)
  {
    for(i=1; i<n; i++) // skip top row  all 1.0
    {
      jj = 0;
      for(j=0; j<n; j++)
      {
        if(j==k) continue; // skip row k
        jj = jj +1;
        // printf("k=%1d, i=%1d, j=%1d, ii=%1d, jj=%1d, ix=%d \n",
	//       k, i, j, i-1, jj-1, i*n+j);
        A3[(i-1)*(n-1)+(jj-1)] = M4[i*n+j];
      } // j
    } // i
    mat_put(n-1, A3);
    C[k] = determinant(n-1, A3);
    if(k%2==1) C[k] = - C[k];
    printf("k=%d, det=%f \n", k, C[k]);
  } // k
} // end cross4d

static void cross(int n, double Mn[], double D[])
{
  double A[(n-1)*(n-1)]; // (n-1)*(n-1) matrix  n times
  int i, j, k, ii, jj;

  for(k=0; k<n; k++)
  {
    for(i=1; i<n; i++) // skip top row  all 1.0
    {
      jj = 0;
      for(j=0; j<n; j++)
      {
        if(j==k) continue; // skip row k
        jj = jj +1;
        // printf("k=%1d, i=%1d, j=%1d, ii=%1d, jj=%1d, ix=%d \n",
	//       k, i, j, i-1, jj-1, i*n+j);
        A[(i-1)*(n-1)+(jj-1)] = Mn[i*n+j];
      } // j
    } // i
    // mat_put(n-1, A);
    D[k] = determinant(n-1, A);
    if(k%2==1) D[k] = - D[k];
    printf("k=%d, det=%f \n", k, D[k]);
  } // k
} // end cross

void cross5(double A5[], double B5[], double C5[], double D5[], double E5[])
{
  // build M5 for cross(5, M5, E), must be unique for each dimension
  const int n = 5;
  double M5[n*n];
  int i;
  for(i=0; i<n; i++)
  {
    M5[i] = 1.0;
    M5[n+i] = A5[i];
    M5[2*n+i] = B5[i];
    M5[3*n+i] = C5[i];
    M5[4*n+i] = D5[i];
  }
  cross(n, M5, E5);
} // end cross5

static double dot3(double A[], double B[])
{
  return A[0]*B[0] + A[1]*B[1] + A[2]*B[2];
} // end dot3

static double dot4(double A[], double B[])
{
  return A[0]*B[0] + A[1]*B[1] + A[2]*B[2] + A[3]*B[3];
} // end dot3

static double dotn(int n, double A[], double B[])
{
  double sum = 0.0;
  int i;
  for(i=0; i<n; i++) sum = sum + A[i]*B[i];
  return sum;
} // end dotn

int main()
{
  double M3[9]= { 1.0, 1.0, 1.0, 1.0, 2.0, 4.0, 10.0, 21.0, 32.0};
  double M4[16]= { 1.0, 1.0, 1.0, 1.0,
                   1.0, 1.0, 1.0, 1.0,  // A  all 1.0
                   1.0, 2.0, 4.0, 5.0,
                   10.0, 21.0, 32.0, 43.0};
  double M4u[16]= { 1.0, 1.0, 1.0, 1.0,
                    1.0, 0.0, 0.0, 0.0,  // A  unit vector
                    1.0, 2.0, 4.0, 5.0,
                    10.0, 21.0, 32.0, 43.0};
  double A[] = {1.0, 2.0, 4.0};
  double B[] = {10.0, 21.0, 32.0};
  double C[3];
  double Cd[3];
  double detM3;
  double A4[] = {1.0, 1.0, 1.0, 1.0};
  double A4u[] = {1.0, 0.0, 0.0, 0.0};
  double B4[] = {1.0, 2.0, 4.0, 5.0};
  double C4[] = {10.0, 21.0, 32.0, 43.0};
  double detM4;
  double Dd[4];
  double A5[] = {2.0, 3.0, 5.0, 8.0, 12.0};
  double B5[] = {1.0, 4.0, 6.0, 9.0, 13.0};
  double C5[] = {3.0, 5.0, 14.0, 8.0, 16.0};
  double D5[] = {6.0, 9.0, 8.0, 15.0, 19.0};
  double E5[5];

  printf("cross_general.c running \n");
  mat_put(3, M3);
  detM3 = determinant(3, M3);
  printf("detM3=%f \n", detM3);
  printf("A= |%f, %f, %f| \n", A[0], A[1], A[2]);
  printf("B= |%f, %f, %f| \n", B[0], B[1], B[2]);
  cross3(A, B, C);
  printf("C=A x B= |%f, %f, %f| \n", C[0], C[1], C[2]);
  printf("dot3(A,C)= %f \n", dot3(A,C));
  printf("dot3(B,C)= %f \n", dot3(B,C));
  printf(" \n");

  cross3d(M3, Cd);
  printf("Cd=M3,A x B= |%f, %f, %f| \n", Cd[0], Cd[1], Cd[2]);
  printf("dot3(A,Cd)= %f \n", dot3(A,Cd));
  printf("dot3(B,Cd)= %f \n", dot3(B,Cd));
  printf(" \n");

  printf("using A4 = |1,1,1,1| Dd = A4 x B4 x C4 \n");
  cross4d(M4, Dd);
  printf("A4= |%f, %f, %f, %f| \n", A4[0], A4[1], A4[2], A4[3]);
  printf("B4= |%f, %f, %f, %f| \n", B4[0], B4[1], B4[2], B4[3]);
  printf("C4= |%f, %f, %f, %f| \n", C4[0], C4[1], C4[2], C4[3]);
  printf("Dd=M4,A4 x B4 X C4 = |%f, %f, %f, %f| \n", Dd[0], Dd[1], Dd[2], Dd[3]);
  printf("dot4(A4,Dd)= %f \n", dot4(A4,Dd));
  printf("dot4(B4,Dd)= %f \n", dot4(B4,Dd));
  printf("dot4(C4,Dd)= %f \n", dot4(C4,Dd));
  printf(" \n");

  printf("using cross(n,M4,Dd) \n");
  cross(4, M4, Dd);
  printf("Dd(M4) = |%f, %f, %f, %f| \n", Dd[0], Dd[1], Dd[2], Dd[3]);
  printf(" \n");

  printf("using equations Dd = B x C with  A=|1,1,1,1|\n");
  Dd[0] = B4[2]*C4[3] - B4[3]*C4[2] + B4[3]*C4[1] - B4[1]*C4[3] + 
          B4[1]*C4[2] - B4[2]*C4[1];
  Dd[1] = B4[3]*C4[2] - B4[2]*C4[3] + B4[0]*C4[3] - B4[3]*C4[0] +
          B4[2]*C4[0] - B4[0]*C4[2]; 
  Dd[2] = B4[1]*C4[3] - B4[3]*C4[1] + B4[3]*C4[0] - B4[0]*C4[3] +
          B4[0]*C4[1] - B4[1]*C4[0];
  Dd[3] = B4[2]*C4[1] - B4[1]*C4[2] + B4[0]*C4[2] - B4[2]*C4[0] + 
          B4[1]*C4[0] - B4[0]*C4[1];
  printf("Dd=A4 x B4 x C4= |%f, %f, %f, %f| \n", Dd[0], Dd[1], Dd[2], Dd[3]);
  printf("dot4(A4,Dd)= %f \n", dot4(A4,Dd));
  printf("dot4(B4,Dd)= %f \n", dot4(B4,Dd));
  printf("dot4(C4,Dd)= %f \n", dot4(C4,Dd));
  printf(" \n");

  printf("using A4u = |1,0,0,0| unit vector, Dd = Au x B4 x C4 \n");
  cross4d(M4u, Dd);
  printf("A4u= |%f, %f, %f, %f| \n", A4u[0], A4u[1], A4u[2], A4u[3]);
  printf("B4= |%f, %f, %f, %f| \n", B4[0], B4[1], B4[2], B4[3]);
  printf("C4= |%f, %f, %f, %f| \n", C4[0], C4[1], C4[2], C4[3]);
  printf("Dd=M4u,A4u x B4 X C4 = |%f, %f, %f, %f| \n", Dd[0], Dd[1], Dd[2], Dd[3]);
  printf("dot4(A4u,Dd)= %f \n", dot4(A4u,Dd));
  printf("dot4(B4,Dd)= %f \n", dot4(B4,Dd));
  printf("dot4(C4,Dd)= %f \n", dot4(C4,Dd));
  printf(" \n");

  printf("using equations Dd = B x C with  A=|1,0,0,0| unit vector\n");
  Dd[0] = 0.0;
  Dd[1] = B4[3]*C4[2] - B4[2]*C4[3]; 
  Dd[2] = B4[1]*C4[3] - B4[3]*C4[1];
  Dd[3] = B4[2]*C4[1] - B4[1]*C4[2];
  printf("Dd=A4u x B4 x C4= |%f, %f, %f, %f| \n", Dd[0], Dd[1], Dd[2], Dd[3]);
  printf("dot4(A4u,Dd)= %f \n", dot4(A4u,Dd));
  printf("dot4(B4,Dd)= %f \n", dot4(B4,Dd));
  printf("dot4(C4,Dd)= %f \n", dot4(C4,Dd));
  printf(" \n");

  printf("cross5(A5, B5, C5, D5, E5) \n");
  printf("A5= |%f, %f, %f, %f, %f| \n", A5[0], A5[1], A5[2], A5[3], A5[4]);
  printf("B5= |%f, %f, %f, %f, %f| \n", B5[0], B5[1], B5[2], B5[3], B5[4]);
  printf("C5= |%f, %f, %f, %f, %f| \n", C5[0], C5[1], C5[2], C5[3], C5[4]);
  printf("D5= |%f, %f, %f, %f, %f| \n", D5[0], D5[1], D5[2], D5[3], D5[4]);
  cross5(A5, B5, C5, D5, E5);
  printf("E5= |%f, %f, %f, %f, %f| \n", E5[0], E5[1], E5[2], E5[3], E5[4]);
  printf("dotn(5, A5, E5)=%f \n", dotn(5, A5, E5));
  printf("dotn(5, B5, E5)=%f \n", dotn(5, B5, E5));
  printf("dotn(5, C5, E5)=%f \n", dotn(5, C5, E5));
  printf("dotn(5, D5, E5)=%f \n", dotn(5, D5, E5));

  printf("cross_general.c finished \n");
  return 0;
} // end cross_general.c