float
max_dihedral_angle(const TetMesh& mesh)
{
	// Measure maximum dihedral angle of tet mesh.
	float maxAngle = 0.0f;
	for (size_t t = 0; t < mesh.tSize(); ++t) {
        std::vector<float> angles;
        mesh.getTet(t).dihedralAngles(angles);
        for (size_t i = 0; i < angles.size(); ++i) {
		    if (angles[i] > maxAngle) {
			    maxAngle = angles[i];
		    }
        }
	}
	return maxAngle * 180.0f / M_PI;
}
// remove tets with corners where phi=0 but are outside the volume
static void
remove_exterior_tets(TetMesh& mesh,
                     const std::vector<float> &vphi,
                     const SDF& sdf)
{
    for (size_t t=0; t<mesh.tSize(); ) {
        int i, j, k, l; assign(mesh.T(t), i, j, k, l);
        assert(vphi[i]<=0 && vphi[j]<=0 && vphi[k]<=0 && vphi[l]<=0);
        if (vphi[i]==0 && vphi[j]==0 && vphi[k]==0 && vphi[l]==0
            && sdf((mesh.V(i)+mesh.V(j)+mesh.V(k)+mesh.V(l))/4)>0) {
            mesh.T(t) = mesh.tets().back();
            mesh.tets().pop_back();
        } else {
            ++t;
        }
    }
}
// Fix some edge crossings by warping vertices if it's admissible
static void
warp_vertices(const float threshold,
              TetMesh& mesh,
              std::vector<float>& vphi)
{
    assert(threshold>=0 && threshold<=0.5);
    std::vector<float> warp(mesh.vSize(), FLT_MAX);
    std::vector<int> warp_nbr(mesh.vSize(), -1);
    std::vector<Vec3f> d(mesh.vSize(), Vec3f(0,0,0));
    // it's wasteful to iterate through tets just to look at edges; oh well
    for (size_t t=0; t<mesh.tSize(); ++t) {
        for (int u=0; u<3; ++u) {
            int i = mesh.T(t)[u];
            for (int v=u+1; v<4; ++v) {
                int j = mesh.T(t)[v];
                if ((vphi[i]<0 && vphi[j]>0) || (vphi[i]>0 && vphi[j]<0)) {
                    float alpha = vphi[i]/(vphi[i]-vphi[j]);
                    if (alpha < threshold) { // warp i?
                        float d2 = alpha*dist2(mesh.V(i), mesh.V(j));
                        if (d2 < warp[i]) {
                            warp[i] = d2;
                            warp_nbr[i] = j;
                            d[i] = alpha*(mesh.V(j)-mesh.V(i));
                        }
                    } else if (alpha > 1-threshold) { // warp j?
                        float d2 = (1-alpha)*dist2(mesh.V(i), mesh.V(j));
                        if (d2 < warp[j]) {
                            warp[j] = d2;
                            warp_nbr[j] = i;
                            d[j] = (1-alpha)*(mesh.V(i)-mesh.V(j));
                        }
                    }
                }
            }
        }
    }
    // do the warps (also wasteful to loop over all vertices; oh well)
    for (size_t i=0; i<mesh.vSize(); ++i) if(warp_nbr[i]>=0) {
        mesh.V(i) += d[i];
        vphi[i] = 0; 
    }
}
void
make_tet_mesh(TetMesh& mesh,
              const SDF& sdf,
              FeatureSet& featureSet,
              bool optimize,
              bool intermediate,
              bool unsafe)
{
    // Initialize exact arithmetic
    initialize_exact();

    // Initialize mesh from acute lattice.
    std::vector<float> vphi; // SDF value at each lattice vertex.
    mesh.verts().resize(0);
    mesh.tets().resize(0);

    std::cout<<"  cutting from lattice"<<std::endl;
    cut_lattice(sdf, mesh, vphi);

    if (intermediate) {
        static const char* cutLatticeFile = "1_cut_lattice.tet";
        static const char* cutLatticeInfo = "1_cut_lattice.info";
        mesh.writeToFile(cutLatticeFile);
        mesh.writeInfoToFile(cutLatticeInfo);
        std::cout << "Mesh written to " << cutLatticeFile << std::endl;
    }

    // Warp any vertices close enough to phi=0 to fix sign crossings.
    static const float WARP_THRESHOLD = 0.3;
    std::cout<<"  warping vertices"<<std::endl;
    warp_vertices(WARP_THRESHOLD, mesh, vphi);

    if (intermediate) {
        static const char* warpVerticesFile = "2_warp_vertices.tet";
        static const char* warpVerticesInfo = "2_warp_vertices.info";
        mesh.writeToFile(warpVerticesFile);
        mesh.writeInfoToFile(warpVerticesInfo);
        std::cout << "Mesh written to " << warpVerticesFile << std::endl;
    }

    // Cut through tets that still poke out of the level set
    std::cout<<"  trimming spikes"<<std::endl;
    trim_spikes(mesh, vphi);

    if (intermediate) {
        static const char* trimSpikesFile = "3_trim_spikes.tet";
        static const char* trimSpikesInfo = "3_trim_spikes.info";
        mesh.writeToFile(trimSpikesFile);
        mesh.writeInfoToFile(trimSpikesInfo);
        std::cout << "Mesh written to " << trimSpikesFile << std::endl;
    }

    // At this point, there could be some bad tets with all four vertices on
    // the surface but which lie outside the level set.  Get rid of them.
    std::cout<<"  removing exterior tets"<<std::endl;
    remove_exterior_tets(mesh, vphi, sdf);

    // Compact mesh and clear away unused vertices.
    std::cout<<"  compacting mesh"<<std::endl;
    mesh.compactMesh();

    if (intermediate) {
        static const char* removeExtFile = "4_remove_exterior.tet";
        static const char* removeExtInfo = "4_remove_exterior.info";
        static const char* removeExtObj = "4_remove_exterior.obj";
        mesh.writeToFile(removeExtFile);
        mesh.writeInfoToFile(removeExtInfo);
        std::vector<Vec3f> objVerts;
        std::vector<Vec3i> objTris;
        mesh.getBoundary(objVerts, objTris);
        write_objfile(objVerts, objTris, removeExtObj);
        std::cout << "Mesh written to " << removeExtFile << std::endl;
    }

    // Compute maximum dihedral angle of mesh (unoptimized, no features).
    std::cout << "  Maximum dihedral angle = " << max_dihedral_angle(mesh) 
        << std::endl;

    if (featureSet.numFeatures() > 0 || optimize)
    {
        // Identify boundary vertices
        std::vector<int> boundary_verts;
        std::vector<Vec3i> boundary_tris;
        std::cout << "  identifying boundary" << std::endl;
        mesh.getBoundary(boundary_verts, boundary_tris);
        assert(!boundary_verts.empty());
        
        // Snap vertices to given features
        std::vector<int> feature_endpoints;
        std::map<int, int> vertex_feature_map;
        if (featureSet.numFeatures() > 0)
        {
            // Move vertices to match desired features
            std::cout << "  matching features" << std::endl;
            clock_t featureTime = clock();

            match_features(mesh, featureSet, sdf.dx,
                           boundary_verts, boundary_tris,
                           feature_endpoints, vertex_feature_map,
                           unsafe);

            featureTime = clock() - featureTime;
        
            std::cout << "  features matched in " 
                      << ((float)featureTime)/CLOCKS_PER_SEC 
                      << " seconds." << std::endl;
            std::cout << "  Maximum dihedral angle = "
                      << max_dihedral_angle(mesh) << std::endl;

            if (optimize && intermediate)
            {
                static const char* matchFeaturesFile = "5_match_features.tet";
                static const char* matchFeaturesInfo = "5_match_features.info";
                mesh.writeToFile(matchFeaturesFile);
                mesh.writeInfoToFile(matchFeaturesInfo);
                std::cout << "Mesh written to " << matchFeaturesFile 
                          << std::endl;
            }
        } 

        // Finish by optimizing the tetmesh, if desired.
        if (optimize) 
        {
            std::cout << "  optimizing mesh" << std::endl;
            clock_t optTime = clock();

            optimize_tet_mesh(mesh, sdf, 
                              boundary_verts, 
                              featureSet,
                              feature_endpoints,
                              vertex_feature_map);

            optTime = clock() - optTime;
            std::cout << "  Mesh optimization completed in " 
                      << ((float)optTime)/CLOCKS_PER_SEC
                      << " seconds." << std::endl;
            std::cout<< "  Maximum dihedral angle = "
                     << max_dihedral_angle(mesh) << std::endl;
        }

        // DEBUGGING
        // Check for inverted tets
        for (size_t t = 0; t < mesh.tSize(); ++t)
        {
            Tet tet = mesh.getTet(t);
            float signedVolume = tet.volume();
            if (signedVolume <= 0.0)
            {
                std::cerr << "Tet #" << t << " is inverted! " <<
                    "Volume = " << signedVolume << "; " <<
                    "Aspect = " << tet.aspectRatio() << std::endl <<
                    "{" << tet[0] << "} {" << tet[1] << "} {" << tet[2] <<
                    "} {" << tet[3] << "}" << std::endl;
            }
        }
    }
}
// Find all tets with vertices where a corner has vphi>0 and trim them back.
static void
trim_spikes(TetMesh& mesh,
            std::vector<float> &vphi)
{
    // keep track of edges we cut - store the index of the new vertex
    std::map<Vec2i,int> cut_map;
    // we have a separate list for the results of trimming tets
    std::vector<Vec4i> new_tets;
    // go through the existing tets to see what we have to trim
    for (int t=0; t<(int)mesh.tSize(); ++t) {
        int p, q, r, s; assign(mesh.T(t), p, q, r, s);
        if (vphi[p]<=0 && vphi[q]<=0 && vphi[r]<=0 && vphi[s]<=0)
            continue; // this tet doesn't stick out

        // We have a plethora of cases to deal with. Let's sort the vertices
        // by their phi values and break ties with index, remembering if we
        // have switched orientation or not, to cut down on the number of
        // cases to handle. Use a sorting network to do the job.
        bool flipped=false;
        if (vphi[p]<vphi[q] || (vphi[p]==vphi[q] && p<q)) {
            std::swap(p, q);
            flipped = !flipped;
        }
        if (vphi[r]<vphi[s] || (vphi[r]==vphi[s] && r<s)) {
            std::swap(r, s);
            flipped = !flipped;
        }
        if (vphi[p]<vphi[r] || (vphi[p]==vphi[r] && p<r)) {
            std::swap(p, r);
            flipped = !flipped;
        }
        if (vphi[q]<vphi[s] || (vphi[q]==vphi[s] && q<s)) {
            std::swap(q, s);
            flipped = !flipped;
        }
        if (vphi[q]<vphi[r] || (vphi[q]==vphi[r] && q<r)) {
            std::swap(q, r);
            flipped = !flipped;
        }

        // sanity checks
        assert(vphi[p]>=vphi[q] && vphi[q]>=vphi[r] && vphi[r]>=vphi[s]); //sort
        assert(vphi[p]>0); // we already skipped interior tets
        assert(vphi[s]<=0); // we never generate tets with all positive phi

        // now do the actual trimming
        if (vphi[s]==0) { // +++0 entirely outside
            mesh.T(t)=mesh.tets().back(); // overwrite with last tet
            mesh.tets().pop_back(); // get rid of the last one
            --t; // decrement to cancel the next for-loop increment

        } else if (vphi[r]>0) { // +++- just one vertex inside, three out
            // replace this tet with one clipped back to the isosurface
            int ps = cut_edge(p, s, mesh, vphi, cut_map),
                qs = cut_edge(q, s, mesh, vphi, cut_map),
                rs = cut_edge(r, s, mesh, vphi, cut_map);
            if (flipped)
                mesh.T(t) = Vec4i(qs, ps, rs, s);
            else
                mesh.T(t) = Vec4i(ps, qs, rs, s);

        } else if(vphi[q]<0) { // +--- just one vertex outside, three in
            // Have to tetrahedralize the resulting triangular prism.
            // Note that the quad faces have to be split in a way that will
            // be consistent with face-adjacent tets: our sort of pqrs with
            // consistently broken ties makes this work. We cut the quad to the
            // deepest vertex (phi as negative as possible).
            int pq = cut_edge(p, q, mesh, vphi, cut_map),
                pr = cut_edge(p, r, mesh, vphi, cut_map),
                ps = cut_edge(p, s, mesh, vphi, cut_map);
            if (flipped) {
                mesh.T(t) = Vec4i(q, pq, r, s);
                new_tets.push_back(Vec4i(r, pq, pr, s));
                new_tets.push_back(Vec4i(s, pq, pr, ps));
            } else {
                mesh.T(t)=Vec4i(pq, q, r, s);
                new_tets.push_back(Vec4i(pq, r, pr, s));
                new_tets.push_back(Vec4i(pq, s, pr, ps));
            }

        } else if (vphi[q]>0 && vphi[r]<0) { // ++-- two vertices out, two in
            int pr = cut_edge(p, r, mesh, vphi, cut_map),
                ps = cut_edge(p, s, mesh, vphi, cut_map),
                qr = cut_edge(q, r, mesh, vphi, cut_map),
                qs = cut_edge(q, s, mesh, vphi, cut_map);
            if (flipped) {
                mesh.T(t) = Vec4i(qr, pr, r, s);
                new_tets.push_back(Vec4i(qs, pr, qr, s));
                new_tets.push_back(Vec4i(ps, pr, qs, s));
            } else {
                mesh.T(t) = Vec4i(pr, qr, r, s);
                new_tets.push_back(Vec4i(pr, qs, qr, s));
                new_tets.push_back(Vec4i(pr, ps, qs, s));
            }

        } else if (vphi[q]==0 && vphi[r]==0) { // +00- 1 out, 1 in, 2 surface
            int ps = cut_edge(p, s, mesh, vphi, cut_map);
            if (flipped)
                mesh.T(t) = Vec4i(q, ps, r, s);
            else
                mesh.T(t) = Vec4i(ps, q, r, s);

        } else if (vphi[q]==0) { // +0-- 1 out, 2 in, 1 surface
            int pr = cut_edge(p, r, mesh, vphi, cut_map),
                ps = cut_edge(p, s, mesh, vphi, cut_map);
            if (flipped) {
                mesh.T(t) = Vec4i(q, pr, r, s);
                new_tets.push_back(Vec4i(q, ps, pr, s));
            } else {
                mesh.T(t) = Vec4i(pr, q, r, s);
                new_tets.push_back(Vec4i(ps, q, pr, s));
            }

        } else { // ++0- two out, one in, one surface
            int ps = cut_edge(p, s, mesh, vphi, cut_map),
                qs = cut_edge(q, s, mesh, vphi, cut_map);
            if (flipped)
                mesh.T(t) = Vec4i(qs, ps, r, s);
            else
                mesh.T(t) = Vec4i(ps, qs, r, s);
        }
    }
    // append all the remaining new tets
    for (size_t t=0; t<new_tets.size(); ++t)
        mesh.tets().push_back(new_tets[t]);
}