#include "cadlevelset.h"


//              3___9___2
//             / `\    /´\ 
//            /    `\/´   \
//           /     / `\    \
//          /    5´    `\   7
//         6   /´        `8  \
//        /  /´            `\ \
//       / /´                `\\
//    0 /´________4____________`\1

// code / indice / nombre
static int code[64][3] =
{    
    {0, 0, 0},
    {1, 24, 2},
    {2, 26, 2},
    {3, 0, 0},    
    {4, 28, 2},
    {5, 0, 0},
    {6, 0, 0},
    {7, 0, 0},
    {8, 30, 2},
    {9, 0, 0},
    {10, 0, 0},
    {11, 20, 4},
    {12, 65, 4},
    {13, 0, 0},
    {14, 0, 0},
    {15, 0, 0},
    {16, 32, 2},
    {17, 0, 0},
    {18, 58, 4},
    {19, 0, 0},
    {20, 0, 0},
    {21, 16, 4},
    {22, 0, 0},
    {23, 0, 0},
    {24, 0, 0},
    {25, 0, 0},
    {26, 0, 0},
    {27, 0, 0},
    {28, 0, 0},
    {29, 0, 0},
    {30, 0, 0},
    {31, 0, 0},
    {32, 34, 2},
    {33, 48, 4},
    {34, 0, 0},
    {35, 0, 0},
    {36, 0, 0},
    {37, 0, 0},
    {38, 12, 4},
    {39, 0, 0},
    {40, 0, 0},
    {41, 0, 0},
    {42, 0, 0},
    {43, 0, 0},
    {44, 0, 0},
    {45, 0, 0},
    {46, 0, 0},
    {47, 0, 0},
    {48, 0, 0},
    {49, 0, 0},
    {50, 0, 0},
    {51, 0, 0},
    {52, 0, 0},
    {53, 0, 0},
    {54, 0, 0},
    {55, 0, 0},
    {56, 8, 4},
    {57, 0, 0},
    {58, 0, 0},
    {59, 0, 0},
    {60, 0, 0},
    {61, 0, 0},
    {62, 0, 0},
    {63, 0, 8},
};

static const int valid_code[] = 
{
  1, 2, 4, 8, 16, 32, 11, 21, 38, 56, 12, 18, 33, 63
};


static const int idnt[][3][4] =
{
    // refine 1 --> 8 // code 63
    {{0, 4, 5, 6}, {0, 4, 5, 6}, {0, 4, 5, 6}},
    {{1, 7, 4, 8}, {1, 7, 4, 8}, {1, 7, 4, 8}},
    {{2, 5, 7, 9}, {2, 5, 7, 9}, {2, 5, 7, 9}},
    {{3, 6, 9, 8}, {3, 6, 9, 8}, {3, 6, 9, 8}},
    {{4, 6, 8, 9}, {5, 6, 4, 8}, {6, 8, 4, 7}},
    {{4, 8, 7, 9}, {5, 4, 7, 8}, {6, 4, 5, 7}},
    {{4, 7, 5, 9}, {5, 7, 9, 8}, {6, 5, 9, 7}},
    {{4, 5, 6, 9}, {5, 9, 6, 8}, {6, 9, 8, 7}},
    // refine 1 --> 4
    // face 0 // code 56
    {{1, 8, 7, 0}, {1, 8, 7, 0}, {1, 8, 7, 0}},
    {{2, 7, 9, 0}, {2, 7, 9, 0}, {2, 7, 9, 0}},
    {{3, 9, 8, 0}, {3, 9, 8, 0}, {3, 9, 8, 0}},
    {{7, 8, 9, 0}, {7, 8, 9, 0}, {7, 8, 9, 0}},
    // face 1 // code 38
    {{0, 5, 6, 1}, {0, 5, 6, 1}, {0, 5, 6, 1}},
    {{2, 9, 5, 1}, {2, 9, 5, 1}, {2, 9, 5, 1}},
    {{3, 6, 9, 1}, {3, 6, 9, 1}, {3, 6, 9, 1}},
    {{5, 9, 6, 1}, {5, 9, 6, 1}, {5, 9, 6, 1}},
    // face 2 // code 21
    {{0, 6, 4, 2}, {0, 6, 4, 2}, {0, 6, 4, 2}},
    {{1, 4, 8, 2}, {1, 4, 8, 2}, {1, 4, 8, 2}},
    {{3, 8, 6, 2}, {3, 8, 6, 2}, {3, 8, 6, 2}},
    {{6, 8, 4, 2}, {6, 8, 4, 2}, {6, 8, 4, 2}},
    // face 3 // code 11
    {{0, 4, 5, 3}, {0, 4, 5, 3}, {0, 4, 5, 3}},
    {{1, 7, 4, 3}, {1, 7, 4, 3}, {1, 7, 4, 3}},
    {{2, 5, 7, 3}, {2, 5, 7, 3}, {2, 5, 7, 3}},
    {{4, 7, 5, 3}, {4, 7, 5, 3}, {4, 7, 5, 3}},
    // refine 1 --> 2
    // arete 0 // code 1
    {{0, 4, 2, 3}, {0, 4, 2, 3}, {0, 4, 2, 3}}, // 24
    {{4, 1, 2, 3}, {4, 1, 2, 3}, {4, 1, 2, 3}},
    // arete 1 // code 2
    {{2, 5, 1, 3}, {2, 5, 1, 3}, {2, 5, 1, 3}},
    {{5, 0, 1, 3}, {5, 0, 1, 3}, {5, 0, 1, 3}},
    // arete 2 // code 4
    {{3, 6, 2, 1}, {3, 6, 2, 1}, {3, 6, 2, 1}},
    {{6, 0, 2, 1}, {6, 0, 2, 1}, {6, 0, 2, 1}},
    // arete 3 // code 8
    {{1, 7, 0, 3}, {1, 7, 0, 3}, {1, 7, 0, 3}},
    {{7, 2, 0, 3}, {7, 2, 0, 3}, {7, 2, 0, 3}},
    // arete 4 // code 16
    {{1, 8, 2, 0}, {1, 8, 2, 0}, {1, 8, 2, 0}},
    {{8, 3, 2, 0}, {8, 3, 2, 0}, {8, 3, 2, 0}},
    // arete 5  // code 32
    {{2, 9, 0, 1}, {2, 9, 0, 1}, {2, 9, 0, 1}}, // 34
    {{9, 3, 0, 1}, {9, 3, 0, 1}, {9, 3, 0, 1}},
    // non implemente pour l'instant
    // refine 1 --> 3
    // arete 0 1 // code 3
    {{0, 4, 5, 3}, {0, 4, 5, 3}, {0, 4, 5, 3}}, 
    {{4, 2, 5, 3}, {4, 2, 5, 3}, {4, 2, 5, 3}}, 
    {{4, 1, 2, 3}, {4, 1, 2, 3}, {4, 1, 2, 3}}, 
    // arete 0 2 // code 5
    {{0, 6, 4, 2}, {0, 6, 4, 2}, {0, 6, 4, 2}}, 
    {{6, 1, 4, 2}, {6, 1, 4, 2}, {6, 1, 4, 2}}, 
    {{6, 3, 1, 2}, {6, 3, 1, 2}, {6, 3, 1, 2}}, 
    // arete 0 3 // code 9
    {{1, 7, 4, 3}, {1, 7, 4, 3}, {1, 7, 4, 3}},  // 42
    {{7, 0, 4, 3}, {7, 0, 4, 3}, {7, 0, 4, 3}}, 
    {{7, 2, 1, 3}, {7, 2, 1, 3}, {7, 2, 1, 3}}, 
    // arete 0 4 // code 17
    {{1, 4, 8, 2}, {1, 4, 8, 2}, {1, 4, 8, 2}}, 
    {{4, 3, 8, 2}, {4, 3, 8, 2}, {4, 3, 8, 2}}, 
    {{4, 0, 3, 2}, {4, 0, 3, 2}, {4, 0, 3, 2}}, 
    // arete 0 5 // code 33
    {{0, 4, 2, 9}, {0, 4, 2, 9}, {0, 4, 2, 9}}, 
    {{0, 4, 9, 3}, {0, 4, 9, 3}, {0, 4, 9, 3}}, 
    {{4, 1, 2, 9}, {4, 1, 2, 9}, {4, 1, 2, 9}}, 
    {{4, 1, 9, 3}, {4, 1, 9, 3}, {4, 1, 9, 3}}, 
    // arete 1 2 // code 6
    {{0, 5, 6, 1}, {0, 5, 6, 1}, {0, 5, 6, 1}},  // 52
    {{5, 3, 6, 1}, {5, 3, 6, 1}, {5, 3, 6, 1}}, 
    {{5, 2, 3, 1}, {5, 2, 3, 1}, {5, 2, 3, 1}}, 
    // arete 1 3 // code 10
    {{2, 5, 7, 3}, {2, 5, 7, 3}, {2, 5, 7, 3}}, 
    {{5, 1, 7, 3}, {5, 1, 7, 3}, {5, 1, 7, 3}}, 
    {{5, 0, 1, 3}, {5, 0, 1, 3}, {5, 0, 1, 3}}, 
    // arete 1 4 // code 18
    {{2, 5, 1, 8}, {2, 5, 1, 8}, {2, 5, 1, 8}}, 
    {{2, 5, 8, 3}, {2, 5, 8, 3}, {2, 5, 8, 3}}, 
    {{5, 0, 1, 8}, {5, 0, 1, 8}, {5, 0, 1, 8}}, 
    {{5, 0, 8, 3}, {5, 0, 8, 3}, {5, 0, 8, 3}}, 
    // arete 1 5 // code 34
    {{2, 9, 5, 1}, {2, 9, 5, 1}, {2, 9, 5, 1}},  // 62
    {{9, 0, 5, 1}, {9, 0, 5, 1}, {9, 0, 5, 1}}, 
    {{9, 3, 0, 1}, {9, 3, 0, 1}, {9, 3, 0, 1}}, 
    // arete 2 3 // code 12
    {{1, 7, 0, 6}, {1, 7, 0, 6}, {1, 7, 0, 6}}, 
    {{1, 7, 6, 3}, {1, 7, 6, 3}, {1, 7, 6, 3}}, 
    {{7, 2, 0, 6}, {7, 2, 0, 6}, {7, 2, 0, 6}}, 
    {{7, 2, 6, 3}, {7, 2, 6, 3}, {7, 2, 6, 3}}, 
    // arete 2 4 // code 20
    {{3, 8, 6, 2}, {3, 8, 6, 2}, {3, 8, 6, 2}}, 
    {{8, 0, 6, 2}, {8, 0, 6, 2}, {8, 0, 6, 2}}, 
    {{8, 1, 0, 2}, {8, 1, 0, 2}, {8, 1, 0, 2}}, 
    // arete 2 5 // code 36
    {{3, 6, 9, 1}, {3, 6, 9, 1}, {3, 6, 9, 1}},  // 72
    {{6, 2, 9, 1}, {6, 2, 9, 1}, {6, 2, 9, 1}}, 
    {{6, 0, 2, 1}, {6, 0, 2, 1}, {6, 0, 2, 1}}, 
    // refine 1 --> 5
    // face 0 / arete 0
    {{1, 8, 7, 0}, {1, 8, 7, 0}, {1, 8, 7, 0}}, 
    {{2, 7, 9, 0}, {2, 7, 9, 0}, {2, 7, 9, 0}}, 
    {{3, 9, 8, 0}, {3, 9, 8, 0}, {3, 9, 8, 0}}, 
    {{7, 8, 9, 0}, {7, 8, 9, 0}, {7, 8, 9, 0}}, 
    // face 1
    {{0, 5, 6, 1}, {0, 5, 6, 1}, {0, 5, 6, 1}}, 
    {{2, 9, 5, 1}, {2, 9, 5, 1}, {2, 9, 5, 1}}, 
    {{3, 6, 9, 1}, {3, 6, 9, 1}, {3, 6, 9, 1}}, 
    {{5, 9, 6, 1}, {5, 9, 6, 1}, {5, 9, 6, 1}}, 
    // face 2
    {{0, 6, 4, 2}, {0, 6, 4, 2}, {0, 6, 4, 2}}, 
    {{1, 4, 8, 2}, {1, 4, 8, 2}, {1, 4, 8, 2}}, 
    {{3, 8, 6, 2}, {3, 8, 6, 2}, {3, 8, 6, 2}}, 
    {{6, 8, 4, 2}, {6, 8, 4, 2}, {6, 8, 4, 2}}, 
    // face 3
    {{0, 4, 6, 3}, {0, 4, 6, 3}, {0, 4, 6, 3}}, 
    {{1, 5, 4, 3}, {1, 5, 4, 3}, {1, 5, 4, 3}}, 
    {{2, 6, 5, 3}, {2, 6, 5, 3}, {2, 6, 5, 3}}, 
    {{4, 5, 6, 3}, {4, 5, 6, 3}, {4, 5, 6, 3}} 
};

int Mesh::split (Tetra *t)
{
    if (t->getState() == SPLITTED)
        return 0;

//     printf("split tetra %d\n", t->getIndex());
	
    int state = 0;

    for ( int i = 0; i < 6; i++ ) //calcul des nouveaux vertex si besoin
    {
        if (!t->edge_state(i))
        {
	    state = 1;
            Vertex *a = t->getEdgeVertex(i, 0);
            Vertex *b = t->getEdgeVertex(i, 1);
            Vertex *nv = new_vertex((a->x()+b->x())/2.0, (a->y()+b->y())/2.0, (a->z()+b->z())/2.0);
            _hes->addEdge(t, i, nv);
            t->split_edge(i);
            _sot->set_edge(t, i);
            _sot->build();
            for (int j = 0; j < _sot->size(); j++ )
            {
// 		printf("_sot size = %d\n", _sot->size());
                Tetra *tt = NULL;
                int ee;
                _sot->get_tetra(j, &tt, &ee);

                if (!tt->edge_state(ee))
		{
// 		    Tetra *pp = tt->getFather();    
// 		    if (pp != NULL && pp->edge_code() != FULL_SPLIT)
// 		       _storage_residual_stack[pp->getGen()].push(pp);
                    tt->split_edge(ee);
		}
            }
        }
        assert(t->edge_state(i));
    }

    t->setState(SPLITTED);
    return state;
}

void Mesh::collect_neighbors (Tetra *t)
{
    for ( int i = 0; i < 6; i++ )
    {
        _sot->set_edge(t, i);
        _sot->build();
        for (int j = 0; j < _sot->size(); j++ )
        {
            Tetra *tt = NULL;
            int ee;
            _sot->get_tetra(j, &tt, &ee);

            if (tt->getFlag() != t->getIndex())
            {
                tt->setFlag(t->getIndex());
		if (tt->getState() != SPLITTED)  
		  _conforming_stack.push(tt);
            }
        }
    }
}

int Mesh::valid_scheme(Tetra *t)
{
    int ecode = t->edge_code();
    if (ecode != valid_code[0] && ecode != valid_code[1] && ecode != valid_code[2]
            && ecode != valid_code[3] && ecode != valid_code[4] && ecode != valid_code[5]
            && ecode != valid_code[6] && ecode != valid_code[7] && ecode != valid_code[8]
            && ecode != valid_code[9] && ecode != valid_code[10] && ecode != valid_code[11]
            && ecode != valid_code[12] && ecode != valid_code[13]
       )
      return 0;
    else
      return 1;
}

void Mesh::refining()
{    
    printf("--> split edges\n");
    
    while (!_refining_stack.empty())
    {
        Tetra *t = _refining_stack.front();
        _refining_stack.pop();
        split(t);
        _refined_stack.push(t);
    }
    
     printf("--> collect neighbours\n");
    
    while (!_refined_stack.empty())
    {
        Tetra *t = _refined_stack.front();
        _refined_stack.pop();
        collect_neighbors(t);
    }
    
    printf("--> conforming stack size %d\n", (int)_conforming_stack.size());
    
    while (!_conforming_stack.empty())
    {
        printf("--> split edges\n");
        while (!_conforming_stack.empty())
        {
            Tetra *t = _conforming_stack.front();
            _conforming_stack.pop();
            if (t->edge_code() != SPLITTED && !valid_scheme(t))
            {
                split(t);
                _refined_stack.push(t);
            }
        }
        
        printf("--> collect neighbours\n");
        while (!_refined_stack.empty())
        {
            Tetra *t = _refined_stack.front();
            _refined_stack.pop();
            collect_neighbors(t);
        }
        
        printf("--> conforming stack size %d\n", (int)_conforming_stack.size());
    }
    
    printf("--> subdivide\n");
    
    for ( int i = 0; i < tetra_size(); i++ )  // boucle sur tout les tetraedres
    {
        Tetra *t = get_tetra(i);
	if (t->getState() != REFINED)
	  subdivide(t);
    }    
    compute_adjacencies();
}


void Mesh::subdivide( Tetra *t)
{
  int ecode = t->edge_code();
  
//   printf("tetra %d --> code %d\n", t->getIndex(), ecode);
  
  int index = code[ecode][1];
  int nbt = code[ecode][2];
  int conf = 0;
  
  if (ecode != 0)
    assert(nbt != 0);
  
    if (nbt != 0)
    {
        Vertex *nv[10];
        nv[0] = t->getVertex(0);
        nv[1] = t->getVertex(1);
        nv[2] = t->getVertex(2);
        nv[3] = t->getVertex(3);
        nv[4] = NULL;
        nv[5] = NULL;
        nv[6] = NULL;
        nv[7] = NULL;
        nv[8] = NULL;
        nv[9] = NULL;

        for ( int i = 0; i < 6; i++ ) //calcul des nouveaux vertex si besoin
        {
            if (t->edge_state(i))
            {
                hashEdgeKey *k1 = new hashEdgeKey(t, i, NULL);
                hashEdgeKey *k2 = _hes->query(k1);
                assert(k2 != NULL);
                delete k1;

                Vertex *m = k2->middle();
                assert(m != NULL);
                nv[4+i] = m;
            }
        }

        if (ecode == FULL_SPLIT)
        {
	    Vertex ab;
	    double dl = INFINITY;    
	    for (int i = 0; i < 3; i++)
	    {
	      ab.setVector(nv[4+i], nv[9-i]);
	      double tmp = ab.sq_norm();
	      if (tmp < dl)
	      {
		conf = i;
		dl = tmp;
	      }
	    }
        }

        for ( int i = 0; i < nbt; i++ )
        {
            int ii = index+i;
            Tetra * nt = new_tetra(nv[idnt[ii][conf][0]], nv[idnt[ii][conf][1]], nv[idnt[ii][conf][2]], nv[idnt[ii][conf][3]]);
            t->setChild(i, nt);
            nt->setFather(t);
// 	    if (nt->getRefDepth()<0)
            nt->setRefDepth(0);
            _new_tetra_stack.push(nt);
        }
        t->setState(REFINED);
        _father_stack.push(t);

        for ( int i = 0; i < t->inc_size(); i++ )
        {
            Vertex *v = t->getInc(i);
            for ( int j = 0; j < 8; j++ )
            {
                Tetra *c = t->getChild(j);
                if (c == NULL)
                    break;
                if (c->contain(v))
                {
                    c->addInc(v);
                    break;
                }
            }
        }
    }  
}

void Mesh::compute_adjacencies()
{
    printf("--> compute adjacencies\n");
    printf("%d child tetras\n", (int)_new_tetra_stack.size());
    _hfs->clear();
    while (!_new_tetra_stack.empty())
    {
        Tetra *t = _new_tetra_stack.front();
        _new_tetra_stack.pop();
	for(int i = 0; i < 4; i++)
	    _hfs->addFace(t, i);
    }
    
    while (!_father_stack.empty())
    {
        Tetra *t = _father_stack.front();
        _father_stack.pop();
	for(int i = 0; i < 4; i++)
	{
	  Tetra *adj = t->getAdj(i);
	  int opp = t->getOpp(i);
	  if (adj != NULL && adj->getState() == NOT_REFINED)    
	    _hfs->addFace(adj, opp);
	}
    }
    
//     check_adjacencies();    
}

void Mesh::check_adjacencies()
{
    int count = 0;
    for ( int i = 0; i < tetra_size(); i++ )  // boucle sur tout les tetraedres
    {
        Tetra *t = get_tetra(i);
        if (t->getState() == NOT_REFINED)
        {
	    if (t->getFather() != NULL)
	      count++;
            for (int i = 0; i < 4; i++)
            {
                Tetra *adj = t->getAdj(i);
                int opp = t->getOpp(i);
                if (t->getAdj(i) != NULL)
		{
// 		    assert(adj->getState() != REFINED);
                    assert(adj->getAdj(opp) == t);
		}
            }
        }
    }
    printf("%d child tetras\n", count);
}


int Mesh::equilibrate_refinement()
{
    printf("--> equilibrate refinement\n");
    
    std::queue<Tetra *> refine_stack;
    std::queue<Tetra *> neigh_stack;
    
    for ( int i = 0; i < tetra_size(); i++ )  // boucle sur tout les tetraedres
    {
        Tetra *t = get_tetra(i);
        if (t->getRefDepth() > 0)
            refine_stack.push(t);
    }
    
    while (!refine_stack.empty())  // boucle sur tous les tetraedres
    {
        Tetra *t = refine_stack.front();
        // on verifie chacun des tetraedres
        if (t->getRefDepth() > 0)
        {
            for (int i = 0; i < 6; i++)
            {
                _sot->set_edge(t, i);
                _sot->build();
                for (int j = 0; j < _sot->size(); j++ )
                {
                    Tetra *tt = NULL;
                    int ee;
                    _sot->get_tetra(j, &tt, &ee);

                    if (tt->getRefDepth() < t->getRefDepth()-1)
                    {
                        tt->setRefDepth(t->getRefDepth()-1);
                        assert(tt->getRefDepth() >= 0);
                        refine_stack.push(tt);
                    }
                }
	    }
//             for (int i = 0; i < 4; i++)
//             {
//                 _bot->set_vertex(t->getVertex(i));
//                 _bot->build();
//                 for (int j = 0; j < _bot->size(); j++ )
//                 {
//                     Tetra *tt = NULL;
//                     int ee;
//                     _bot->get_tetra(j, &tt, &ee);
// 
//                     if (tt->getRefDepth() < t->getRefDepth()-1)
//                     {
//                         tt->setRefDepth(t->getRefDepth()-1);
//                         assert(tt->getRefDepth() >= 0);
//                         refine_stack.push(tt);
//                     }
//                 }
// 	    }
        }
	refine_stack.pop();
    }    
}

int Mesh::init_stack()
{
    assert(_refining_stack.empty());
    for ( int i = 0; i < tetra_size(); i++ )  // boucle sur tout les tetraedres
    {
        Tetra *t = get_tetra(i);
        if (t->getState() == NOT_REFINED && t->getRefDepth() > 0)
            _refining_stack.push(t);
    }
}

void Mesh::refine_triangulation()
{
    equilibrate_refinement();
    exportVTK();
    
    init_stack();
    refining();
    exportVTK();
}


void Mesh::refinement()
{
    _max_generation = 0;

    int generation = 0;

    for ( int loop = 0; loop < 1; loop++ )
    {
        printf("Loop number %d\n", loop);

        //////////////////////////////////////////////////////////
        //                   assignation des flags              //
        //////////////////////////////////////////////////////////

        srand((unsigned)time(0));
        int aleat;
        int count = 0;

        while ( count < 20 )
        {
            aleat = (rand()%tetra_size());
	    Tetra *t = get_tetra(aleat);
	    t->setRefDepth(3);
            count++;
        }


        refine_triangulation();
    }
}

