#include "vor.h"
#include "background.h"

VoronoiDiagram::VoronoiDiagram(Triangulation *tri, Background *bg)
{
    _tri = tri;
    _bg = bg;
    
    set_angle(tri->get_angle());

    for (int i = 0; i < tri->getVertexCount(); i++)
        tri->get_vertex(i)->flag = -1;

    for (Triangle *t = tri->getTri(); t < tri->getTri()+ tri->getTriangleCount(); ++t)
        add_voronoi_vertex(t);

    for (Triangle *t = tri->getTri(); t < tri->getTri()+ tri->getTriangleCount(); ++t)
    {
        if (t->get_vertex(0) >=4 && t->get_vertex(1) >=4 && t->get_vertex(2) >=4)
        {
            for (int i = 0; i < 3; i++)
            {
                int v0 = t->get_vertex(i);
                Vertex *pv = tri->get_vertex(v0);
                if (pv->flag == -1)
                {
                    VoronoiCell *vc = new VoronoiCell(pv);
                    pv->flag = 1;
		    Triangle *tt = t;
                    Triangle *ss = NULL;
                    int ii = Triangle::next(i);
                    int jj;
                    // on circule dans la boule
                    while (ss != t)
                    {
                        int vic = get_voronoi_vertex(tt);
                        vc->_vlist.push_back(vic);
                        ii=Triangle::next(ii);
			ss = tt->get_adjacent(ii);
			jj = tt->get_opp(ii);
                        int v1 = tt->get_vertex(ii);
                        int vf0, vf1;
#if defined(Linf)
                        if (get_edge(tri, tt, ii, &vf0, &vf1))
                        {
                            if (vf0 != -1)
                                vc->_vlist.push_back(vf0);
                            if (vf1 != -1)
                                vc->_vlist.push_back(vf1);
                        }
                        else
                        {
                            build_face(tri, tt, ii, &vf0, &vf1);

                            if (vf0 != -1)
                                vc->_vlist.push_back(vf0);
                            if (vf1 != -1)
                                vc->_vlist.push_back(vf1);  
			}
#endif

			tt = ss;
                        ii = jj;
                    }
                    assert(vc != NULL);
                    add_cell(vc);
                }
            }
        }
    }
#if defined(ALIGN)
    for (int i = 0; i < _vertices.size(); i++)
    {
        tri->disalign(_vertices[i]->p);
    }
    tri->disalign();
#endif
    
/*#if defined(ANGLE)    
    for(int i = 0; i < _cells.size(); i++)
    {     
      	double x = _cells[i]->_center->p[0];
        double y = _cells[i]->_center->p[1];
        _cells[i]->_center->p[0] = x*cos(_angle) - y*sin(_angle);
        _cells[i]->_center->p[1] = x*sin(_angle) + y*cos(_angle);
	x = _cells[i]->_centroid->p[0];
        y = _cells[i]->_centroid->p[1];
        _cells[i]->_centroid->p[0] = x*cos(_angle) - y*sin(_angle);
        _cells[i]->_centroid->p[1] = x*sin(_angle) + y*cos(_angle);
    }
    
    for(int i = 0; i < _vertices.size(); i++)
    {   
      	double x = _vertices[i]->p[0];
        double y = _vertices[i]->p[1];
        _vertices[i]->p[0] = x*cos(_angle) - y*sin(_angle);
        _vertices[i]->p[1] = x*sin(_angle) + y*cos(_angle);
    }
#endif*/    
}

VoronoiDiagram::~VoronoiDiagram()
{
  for(int i = 0; i < _cells.size(); i++)
  {
    delete _cells[i]->_centroid;
    delete _cells[i];
  }
  for(int i = 0; i < _vertices.size(); i++)
  {
    delete _vertices[i];
  }
}

void VoronoiDiagram::build_face(Triangulation* tri, Triangle *t0, int adj, int *v0, int *v1)
{
    Vertex *v[2];
    v[0] = new Vertex;
    v[1] = new Vertex;
    int ev0, ev1;
    Vertex *pev0, *pev1;    

    Triangle *t1 = t0->get_adjacent(adj);
    ev0 = t0->get_vertex(Triangle::next(adj));
    ev1 = t0->get_vertex(Triangle::prev(adj));
    pev0 = tri->get_vertex(ev0);
    pev1 = tri->get_vertex(ev1);
      
//     printf("edge : %lf %lf -- %lf %lf\n", pev0->p[0], pev0->p[1], pev1->p[0], pev1->p[1]);
        
    double dx = pev1->p[0]-pev0->p[0];
    double dy = pev1->p[1]-pev0->p[1];
    double rsquare;
    
    Vertex pc0, pc1;
    pc0.p[0]= t0->get_center()[0];
    pc0.p[1]= t0->get_center()[1];
    pc1.p[0]= t1->get_center()[0];
    pc1.p[1]= t1->get_center()[1];
    
    assert(pc0.p[0] != INFINITY && pc0.p[1] != INFINITY);
    assert(pc1.p[0] != INFINITY && pc1.p[1] != INFINITY);
    
    double min[2] = {INFINITY, INFINITY};
    double max[2] = {-INFINITY, -INFINITY};
    if (min[0] > pc0.p[0]) min[0] = pc0.p[0];
    if (min[0] > pc1.p[0]) min[0] = pc1.p[0];
    if (min[1] > pc0.p[1]) min[1] = pc0.p[1];
    if (min[1] > pc1.p[1]) min[1] = pc1.p[1];
    if (max[0] < pc0.p[0]) max[0] = pc0.p[0];
    if (max[0] < pc1.p[0]) max[0] = pc1.p[0];
    if (max[1] < pc0.p[1]) max[1] = pc0.p[1];
    if (max[1] < pc1.p[1]) max[1] = pc1.p[1];
    
    if (fabs(pev0->p[0]-pev1->p[0]) == /*1e-10*/0.0)
    {
      min[1] = -INFINITY;
      max[1] = INFINITY;
    }
    if (fabs(pev0->p[1]-pev1->p[1]) == /*1e-10*/0.0)
    {
      min[0] = -INFINITY;
      max[0] = INFINITY;
    }      
    
//     static int ccount = 0;    
//     printf("bbox vor : %.20lf %.20lf -- %.20lf %.20lf\n", min[0], min[1], max[0], max[1]);

    if (fabs(dx) == fabs(dy))
    {
        v[0] = v[1] = NULL;
	*v0 = *v1 = -1;
    }
    else
    {
#if defined(POWER)
        double rsquare1, rsquare2, dd1, dd2;
	double t = 0.0;
	if (fabs(dx) >= fabs(dy))
	{
	    t = (pev0->w-pev1->w)/(2*dx) + 0.5;    
	}
	else
	{
	    t = (pev0->w-pev1->w)/(2*dy) + 0.5;    
	}
	
	v[0]->p[0] = v[1]->p[0] = pev0->p[0]+t*(pev1->p[0]-pev0->p[0]);
	v[0]->p[1] = v[1]->p[1] = pev0->p[1]+t*(pev1->p[1]-pev0->p[1]);
	
// 	printf("init --> %lf, %lf\n", v[0]->p[0], v[0]->p[1]);
// 	printf("t --> %lf\n", t);
	
        if (fabs(dx) >= fabs(dy))
        {
            rsquare1 = t*fabs(dx);
            rsquare2 = (1.0-t)*fabs(dx);
	    
	    double ddp = std::min(pev0->p[1]+rsquare1, pev1->p[1]+rsquare2);
	    double ddm = std::max(pev0->p[1]-rsquare1, pev1->p[1]-rsquare2);
	    
            if (pev0->p[0] < pev1->p[0])
            {
                v[0]->p[1] = ddp;
                v[1]->p[1] = ddm;

            }
            else
            {
                v[0]->p[1] = ddm;
                v[1]->p[1] = ddp;
            }
        }
        else
        {
            rsquare1 = t*fabs(dy);
            rsquare2 = (1.0-t)*fabs(dy);

            double ddp = std::min(pev0->p[0]+rsquare1, pev1->p[0]+rsquare2);
            double ddm = std::max(pev0->p[0]-rsquare1, pev1->p[0]-rsquare2);

            if (pev0->p[1] < pev1->p[1])
            {
                v[0]->p[0] = ddm;
                v[1]->p[0] = ddp;
            }
            else
            {
	        v[0]->p[0] = ddp;
                v[1]->p[0] = ddm;
            }
        }        

//      printf("v0 --> %.20lf, %.20lf\n", v[0]->p[0], v[0]->p[1]);
//      printf("v1 --> %.20lf, %.20lf\n", v[1]->p[0], v[1]->p[1]);

        if (fabs(dx) >= fabs(dy))
        {
            if (v[0]->p[0] < min[0] || v[0]->p[0] > max[0] || v[0]->p[1] < min[1] || v[0]->p[1] > max[1])
            {
                delete v[0];
                v[0] = NULL;
            }
            if (v[1]->p[0] < min[0] || v[1]->p[0] > max[0] || v[1]->p[1] < min[1] || v[1]->p[1] > max[1])
            {
                delete v[1];
                v[1] = NULL;
            }
        }
        else
        {
            if (v[0]->p[0] < min[0] || v[0]->p[0] > max[0] || v[0]->p[1] < min[1] || v[0]->p[1] > max[1])
            {
                delete v[0];
                v[0] = NULL;
            }
            if (v[1]->p[0] < min[0] || v[1]->p[0] > max[0] || v[1]->p[1] < min[1] || v[1]->p[1] > max[1])
            {
                delete v[1];
                v[1] = NULL;
            }
        }
	
#else
        double rsquare, dd;
	v[0]->p[0] = v[1]->p[0] = (pev0->p[0]+pev1->p[0])/2.0;
	v[0]->p[1] = v[1]->p[1] = (pev0->p[1]+pev1->p[1])/2.0;
	
// 	printf("init --> %lf, %lf\n", v[0]->p[0], v[0]->p[1]);
	
        if (fabs(dx) >= fabs(dy))
        {
            rsquare = fabs(dx)/2.0;
            dd = rsquare-fabs(dy)/2.0;
            if (pev0->p[0] < pev1->p[0])
            {
                v[0]->p[1] += dd;
                v[1]->p[1] -= dd;
            }
            else
            {
                v[0]->p[1] -= dd;
                v[1]->p[1] += dd;
            }
        }
        else
        {
            rsquare = fabs(dy)/2.0;
            dd = rsquare-fabs(dx)/2.0;
            if (pev0->p[1] < pev1->p[1])
            {
                v[0]->p[0] -= dd;
                v[1]->p[0] += dd;
            }
            else
            {
                v[0]->p[0] += dd;
                v[1]->p[0] -= dd;
            }
        }
        

//         printf("v0 --> %.20lf, %.20lf\n", v[0]->p[0], v[0]->p[1]);
// 	printf("v1 --> %.20lf, %.20lf\n", v[1]->p[0], v[1]->p[1]);

        if (v[0]->p[0] < min[0] || v[0]->p[0] > max[0] || v[0]->p[1] < min[1] || v[0]->p[1] > max[1])
        {
            delete v[0];
            v[0] = NULL;
        }
        if (v[1]->p[0] < min[0] || v[1]->p[0] > max[0] || v[1]->p[1] < min[1] || v[1]->p[1] > max[1])
        {
            delete v[1];
            v[1] = NULL;
        }
#endif
        
//         printf("%d --> \n", ccount++);

//         if (v[0])
//             printf("v0 --> %lf %lf\n", v[0]->p[0], v[0]->p[1]);
//         if (v[1])
//             printf("v1 --> %lf %lf\n", v[1]->p[0], v[1]->p[1]);
	
	add_edge(ev0, ev1, v[0], v[1], v0, v1);
    }
}

int VoronoiDiagram::add_voronoi_vertex(Triangle *t)
{
        Vertex *ct = new Vertex;
        ct->p[0] = _tri->get_center(t)[0];
        ct->p[1] = _tri->get_center(t)[1];
// 	printf("%lf %lf\n", ct->p[0], ct->p[1]);
	_voronoi_vertices.insert(std::pair<Triangle *, Vertex*>(t, ct));
	return add_vertex(ct);
}

int VoronoiDiagram::add_edge(int ev0, int ev1, Vertex *pv0, Vertex *pv1, int *v0, int *v1)
{
    *v0 = *v1 = -1;
    if (pv0 != NULL)
        *v0 = add_vertex(pv0);
    if (pv1 != NULL)
        *v1 = add_vertex(pv1);

    if (ev0 < ev1)
        _edge_map.insert(std::pair<std::pair<int, int>, std::pair<Vertex *, Vertex *> >(std::pair<int, int>(ev0, ev1), std::pair<Vertex *, Vertex *>(pv0, pv1)));
    else
        _edge_map.insert(std::pair<std::pair<int, int>, std::pair<Vertex *, Vertex *> >(std::pair<int, int>(ev1, ev0), std::pair<Vertex *, Vertex *>(pv1, pv0)));
}

int VoronoiDiagram::get_edge(Triangulation* tri, Triangle *t0, int adj, int *v0, int *v1)
{
    *v0 = *v1 = -1;
    
        int ev0, ev1;
    Vertex *pev0, *pev1;    
//     for(int i = 0; i < 3; i++)
//       if (t0->get_adjacent(i) == t1)
//       {
// 	ev0 = t0->get_vertex(Triangle::next(i));
// 	ev1 = t0->get_vertex(Triangle::prev(i));
// 	pev0 = tri->get_vertex(ev0);
// 	pev1 = tri->get_vertex(ev1);
//       }
      
    ev0 = t0->get_vertex(Triangle::next(adj));
    ev1 = t0->get_vertex(Triangle::prev(adj));
    pev0 = tri->get_vertex(ev0);
    pev1 = tri->get_vertex(ev1);
      
    if (ev0 < ev1)
        _item = _edge_map.find(std::pair<int, int>(ev0, ev1));
    else
        _item = _edge_map.find(std::pair<int, int>(ev1, ev0));

    if (_item == _edge_map.end())
        return 0;
    else
    {
	Vertex *pv0, *pv1;
        if (ev0 < ev1)
        {
            pv0 = _item->second.first;
            pv1 = _item->second.second;
        }
        else
        {
            pv0 = _item->second.second;
            pv1 = _item->second.first;
        }
	
	
        if (pv0 != NULL && pv1 != NULL)
        {
            *v0 = pv0->flag;
            *v1 = pv1->flag;
            return 1;
        }
        else if (pv1 != NULL)
        {
            *v0 = -1;
	    *v1 = pv1->flag;
	    return 1;
        }
        else if (pv0 != NULL)
        {
	    *v0 = pv0->flag;
            *v1 = -1;
	    return 1;
        }
        else
        {
           *v0 = -1;
            *v1 = -1;
            return 1;
        }
        return 0;
    }
}

void VoronoiDiagram::compute_centroid()
{
    for (int i = 0; i < _cells.size(); i++)
    {
        Vertex *pv0, *pv1;
        VoronoiCell *vc = _cells[i];
        Vertex *c = vc->_center;
        int vs = vc->_vlist.size();
	vc->_inertia_moment = 0.0;
	
#if defined (DENSITY_CENTROID)
	double exp = 5;
	double phi_c = 1.0-_bg->get_size(c->p);
	for (int ii = 0; ii < exp; ii++)
	  phi_c *= phi_c;
#else
	double phi_c = 1.0;
#endif
	
	double div = 0.0;
        for (int j = 0; j < vs; j++)
        {
            pv0 = get_vertex(vc->_vlist[j]);
            pv1 = get_vertex(vc->_vlist[(j+1)%vs]);
	    
#if defined (DENSITY_CENTROID)
            double phi_1 = 1.0-_bg->get_size(pv0->p);
            double phi_2 = 1.0-_bg->get_size(pv1->p);
	    for (int ii = 0; ii < exp; ii++)
	    {
	      phi_1 *= phi_1;
	      phi_2 *= phi_2;
	    }
#else
            double phi_1 = 1.0;
            double phi_2 = 1.0;
#endif
	    
	    double area = (pv0->p[0] - c->p[0])*(pv1->p[1] - c->p[1]) - (pv0->p[1] - c->p[1])*(pv1->p[0] - c->p[0]);
	    
#if defined (DENSITY_CENTROID)
	    double vol = area * phi_c + area * phi_1 + area * phi_2;   
#else
            double vol = area;
#endif

	    
// 	    assert(vol >= 0.0);
	    double p[2];
#if defined (DENSITY_CENTROID)
            p[0] = (c->p[0]+pv0->p[0]+pv1->p[0])/3.0;
            p[1] = (c->p[1]+pv0->p[1]+pv1->p[1])/3.0;
#else
            p[0] = (phi_c*c->p[0]+phi_1*pv0->p[0]+phi_2*pv1->p[0])/(phi_c+phi_1+phi_2);
            p[1] = (phi_c*c->p[1]+phi_1*pv0->p[1]+phi_2*pv1->p[1])/(phi_c+phi_1+phi_2);
#endif

	    double d = dist(c->p, p);
	    d *= d;
	    vc->_inertia_moment += d*vol;    
	    vc->_centroid->p[0] += p[0]*vol;
            vc->_centroid->p[1] += p[1]*vol;    
	    div += vol;    
        }
        vc->_centroid->p[0] /= div;
	vc->_centroid->p[1] /= div;
	
// 	double minx = INFINITY;
// 	double miny = INFINITY;
// 	double maxx = -INFINITY;
// 	double maxy = -INFINITY;
//
//         for (int j = 0; j < vs; j++)
//         {
//             pv0 = get_vertex(vc->_vlist[j]);
// 	    if (pv0->p[0] < minx)
// 	      minx = pv0->p[0];
// 	    if (pv0->p[0] > maxx)
// 	      maxx = pv0->p[0];
// 	    if (pv0->p[1] < miny)
// 	      miny = pv0->p[1];
// 	    if (pv0->p[1] > maxy)
// 	      maxy = pv0->p[1];
//         }
//         vc->_centroid->p[0] = (maxx+minx)/2.0;
// 	vc->_centroid->p[1] = (maxy+miny)/2.0;
    }
}

void VoronoiDiagram::print()
{
    printf("**** Voronoi diagramm ****\n");
  for(int i = 0; i < _cells.size(); i++)
  {
    printf("--> cell %d has %d vertices :", i, (int)_cells[i]->_vlist.size());
    for(int j = 0; j < _cells[i]->_vlist.size(); j++)
      printf(" %d", _cells[i]->_vlist[j]);
    printf("\n");
  }
}

void VoronoiDiagram::export_vtk()
{
      static int count = 0;
    char filename[100];
  sprintf(filename, "test_vor_%d.vtk", count++);
//     printf(" write file --> %s\n", filename);   

    FILE *fp_rw = fopen (filename, "w");
    
        fprintf (fp_rw, "# vtk DataFile Version 2.0\n");
    fprintf (fp_rw, "Really cool data\n");
    fprintf (fp_rw, "ASCII\n");
    fprintf (fp_rw, "DATASET POLYDATA\n");
    fprintf (fp_rw, "POINTS %d float\n", (int)_vertices.size() + (int)_cells.size());
    
    for(int i = 0; i < _cells.size(); i++)
    {
      fprintf(fp_rw, "%lf %lf %lf\n", _cells[i]->_center->p[0], _cells[i]->_center->p[1], 0.0);
    }
    
    for(int i = 0; i < _vertices.size(); i++)
    {
        fprintf(fp_rw, "%lf %lf %lf\n", _vertices[i]->p[0], _vertices[i]->p[1], 0.0);
    }

    fprintf (fp_rw, "VERTICES %d %d\n", (int)_cells.size(), (int)_cells.size()+1);
    fprintf(fp_rw, "%d", (int)_cells.size());
    for (int j = 0; j < _cells.size(); j++)
        fprintf(fp_rw, " %d", j);
    fprintf(fp_rw, "\n");

    int vf = 0;

    for (int i = 0; i < _cells.size(); i++)
        vf += _cells[i]->_vlist.size()+1;

    fprintf (fp_rw, "POLYGONS %d %d\n", (int)_cells.size(), vf);
    for (int i = 0; i < _cells.size(); i++)
    {
        fprintf(fp_rw, "%d", (int)_cells[i]->_vlist.size());
        for (int j = 0; j < _cells[i]->_vlist.size(); j++)
            fprintf(fp_rw, " %d", _cells[i]->_vlist[j] + (int)_cells.size());
        fprintf(fp_rw, "\n");
    }

    fprintf (fp_rw, "POINT_DATA %d\n", (int)_vertices.size() + (int)_cells.size());
    fprintf (fp_rw, "SCALARS value float 1\n");
    fprintf (fp_rw, "LOOKUP_TABLE default\n");
    for (int i = 0; i < _cells.size(); i++)
      if (_cells[i]->_center->flag == 1)
        fprintf(fp_rw, " %lf\n", _cells[i]->_inertia_moment);
      else
	fprintf(fp_rw, " %lf\n", 0.0);
    for (int i = 0; i < _vertices.size(); i++)
        fprintf(fp_rw, " %lf\n", 0.0);
    
#if defined (POWER)
    fprintf (fp_rw, "SCALARS weight float 1\n");
    fprintf (fp_rw, "LOOKUP_TABLE default\n");
    for (int i = 0; i < _cells.size(); i++)
      if (_cells[i]->_center->flag == 1)
        fprintf(fp_rw, " %lf\n", _cells[i]->_center->w);
      else
	fprintf(fp_rw, " %lf\n", 0.0);
    for (int i = 0; i < _vertices.size(); i++)
        fprintf(fp_rw, " %lf\n", 0.0);
#endif
	
    fclose(fp_rw);
}