#include "vtr.h"
#include "../dgtd-performance.hpp"
#include "Constants.h"
#include <iostream>
using namespace std;
extern int Basdim;
extern int ConstBasis;

static int TetPolyOrderDim[4] = {6, 12, 30, 45};

static int EdgeIndex[6][2] = {
  {0, 1},
  {0, 2}, 
  {0, 3},
  {1, 2},
  {1, 3},
  {2, 3}
};

const int FACE_MAP[4][3] = {
  {1, 2, 3}, 
  {0, 2, 3}, 
  {0, 1, 3},
  {0, 1, 2}
};

static int FaceIndex[8][3] = {
  {2, 1, 3},
  {3, 1, 2},
  {2, 0, 3},
  {3, 0, 2},
  {1, 0, 3},
  {3, 0, 1},
  {1, 0, 2},
  {2, 0, 1}
};

const int TET_MAP[3][4] = {
  {0, 3, 2, 1},
  {0, 2, 3, 1},
  {0, 1, 3, 2}
};

vtr NodeBasisVtr(int i, vtr *avtr, fp_t vol){
  vtr rvtr;
  
  rvtr = avtr[i];
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

vtr EdgeBasisVtr(int i, int j, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;
  
  rvtr = avtr[j] * zeta[i];
  rvtr = rvtr - (avtr[i] * zeta[j]);
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

vtr GradBasisVtr(int i, int j, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;

  rvtr = avtr[j] * zeta[i];
  rvtr = rvtr + (avtr[i] * zeta[j]);
  rvtr = rvtr * 4.0;
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

/**  2    face{i,j,k}         4zj(zi grad(zk) - zk grad(zi))     4
*    2    face{i,j,k}         4zk(zi grad(zj) - zj grad(zi))     4
*/    
vtr FacialBasisVtrGroup2_0(int i, int j, int k, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;
  
  rvtr = avtr[k] * (zeta[i] * zeta[j]);
  rvtr = rvtr - (avtr[i] * (zeta[j] * zeta[k]));
  rvtr = rvtr * 4.0;
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

vtr FacialBasisVtrGroup2_1(int i, int j, int k, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;
  
  rvtr = avtr[j] * (zeta[k] * zeta[i]);
  rvtr = rvtr - (avtr[i] * (zeta[j] * zeta[k]));
  rvtr = rvtr * 4.0;
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

/**  3    edge{i,j}           4grad(zi^2 zj - zj^2 zi)           6
*    4    face{i,j,k}         8grad(zi zj zk)                    4
*/    

vtr EdgeBasisVtrGroup3(int i, int j, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;
    
  rvtr = (avtr[j] * zeta[i] * zeta[i] + avtr[i] * zeta[i] * zeta[j] * 2. - avtr[i] * zeta[j] * zeta[j] - avtr[j] * zeta[i] * zeta[j] * 2.) * 4.;
  // rvtr = rvtr * 4.0;
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

vtr FacialBasisVtrGroup4(int i, int j, int k, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;
  // face{i,j,k}, group 4
  rvtr = (avtr[k] * zeta[i] * zeta[j] + avtr[j] * zeta[i] * zeta[k] + avtr[i] * zeta[j] * zeta[k]) * 8.;
  // rvtr = rvtr * 4.0;
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

vtr FacialBasisVtrGroup5_0(int i, int j, int k, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;
  // face{i,j,k}, group 5
  rvtr = (avtr[k] * zeta[j] - avtr[j] * zeta[k]) * zeta[i] * zeta[i] * 4.;
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

vtr FacialBasisVtrGroup5_1(int i, int j, int k, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;
  // face{i,j,k}, group 5
  rvtr = (avtr[k] * zeta[i] - avtr[i] * zeta[k]) * zeta[j] * zeta[j] * 4.;
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

vtr FacialBasisVtrGroup5_2(int i, int j, int k, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;
  // face{i,j,k}, group 5
  rvtr = (avtr[j] * zeta[i] - avtr[i] * zeta[j]) * zeta[k] * zeta[k] * 4.;
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

vtr TetraBasisVtrGroup6(int i, int j, int k, int l, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;
  // tetra{0,1,2,3}, group 6
  rvtr = (avtr[j] * zeta[i] - avtr[i] * zeta[j]) * zeta[k] * zeta[l] * 8.;
  rvtr = rvtr / (3.0 * vol);

  return rvtr;
}

//TODO: Make dynamic
vtr BasisVtr(int id, vtr *avtr, fp_t vol, fp_t *zeta){
  vtr rvtr;

  // if (id < 0 || id >= tetraNumBasis) return rvtr;
  if(id < 0 || id >= TetPolyOrderDim[3]) 
    return rvtr;

  int index = id;

  if(index < 6) 
    return EdgeBasisVtr(EdgeIndex[index][0], EdgeIndex[index][1], avtr, vol, zeta);
  if(index >= 6 && index < 12) 
    return GradBasisVtr(EdgeIndex[index - 6][0], EdgeIndex[index - 6][1], avtr, vol, zeta);
	if(index >= 12 && index < 20 && index % 2 == 0) 
    return FacialBasisVtrGroup2_0(FACE_MAP[(index - 12)/2][0], FACE_MAP[(index - 12)/2][1], FACE_MAP[(index - 12)/2][2], avtr, vol, zeta); 
	if(index >= 12 && index < 20 && index % 2 == 1) 
    return FacialBasisVtrGroup2_1(FACE_MAP[(index - 12)/2][0], FACE_MAP[(index - 12)/2][1], FACE_MAP[(index - 12)/2][2], avtr, vol, zeta); 
  if(index >=20  && index < 26) 
    return EdgeBasisVtrGroup3(EdgeIndex[index - 20][0], EdgeIndex[index - 20][1], avtr, vol, zeta);
  if(index >= 26 && index < 30) 
    return FacialBasisVtrGroup4(FACE_MAP[index - 26][0], FACE_MAP[index - 26][1], FACE_MAP[index - 26][2], avtr, vol, zeta);
  if(index >= 30 && index < 42 && index % 3 == 0) 
    return FacialBasisVtrGroup5_0(FACE_MAP[(index - 30)/3][0], FACE_MAP[(index - 30)/3][1], FACE_MAP[(index - 30)/3][2], avtr, vol, zeta);
  if(index >= 30 && index < 42 && index % 3 == 1) 
    return FacialBasisVtrGroup5_1(FACE_MAP[(index - 30)/3][0], FACE_MAP[(index - 30)/3][1], FACE_MAP[(index - 30)/3][2], avtr, vol, zeta);
  if(index >= 30 && index < 42 && index % 3 == 2) 
    return FacialBasisVtrGroup5_2(FACE_MAP[(index - 30)/3][0], FACE_MAP[(index - 30)/3][1], FACE_MAP[(index - 30)/3][2], avtr, vol, zeta);
  if(index >= 42 && index < 45) 
    return TetraBasisVtrGroup6(TET_MAP[index - 42][0], TET_MAP[index - 42][1], TET_MAP[index - 42][2], TET_MAP[index - 42][3], avtr, vol, zeta);

}


  vtr CalcEfield(float *e, vtr *avtr, fp_t vol, fp_t *zeta, int PolyOrder){
    vtr efld, cv;
    int i;
    for(i = 0; i < TetPolyOrderDim[PolyOrder]; i ++){
      cv = BasisVtr(i, avtr, vol, zeta);
      efld = efld + cv * e[i];
    }
    return efld;
  }

  vtr CalcEfield(double *e, vtr *avtr, fp_t vol, fp_t *zeta, int PolyOrder){
    vtr efld, cv;
    int i;
    for(i = 0; i < TetPolyOrderDim[PolyOrder]; i ++){
      cv = BasisVtr(i, avtr, vol, zeta);
      efld = efld + cv * e[i];
    }
    return efld;
  }

void testVector(fp_t* basisVtr, vtr vtr2test, vtr *avtr, fp_t vol, fp_t *zeta, int PolyOrder){
  vtr cv;
  int i;
  for(i = 0; i < TetPolyOrderDim[PolyOrder]; i ++){
    cv = BasisVtr(i, avtr, vol, zeta);
    basisVtr[i] = dotP(cv, vtr2test);
    // cout << "(" << vtr2test.getx() << ", " << vtr2test.gety() << ", " << vtr2test.getz() << ")" << endl;
    // cout << "(" << zeta[0] << ", " << zeta[1] << ", " << zeta[2] << ", " << zeta[3] << ")" << endl;
  }
}