#include "Mesh.hpp"

Mesh::Mesh()
    : center(Point(.0, .0, .0))
{
}

size_t Mesh::add_vertice(const Point& pt) {
    return inner_add_vertice(pt, NormalVect());
}

size_t Mesh::add_vertice(const Point& pt, const Point& normal) {
    return inner_add_vertice(pt, NormalVect(normal, true));
}

void Mesh::add_face(const Face& face) {
    for(int f_id=0; f_id < 3; ++f_id) {
        if((unsigned)face[f_id] >= normals.size())
            throw std::out_of_range("Vertice out of range for face");
        if(!normals[face[f_id]].manual)
            normals[face[f_id]].dirty = true;

        faces_with_vert[face[f_id]].push_back(faces.size());
    }
    faces.push_back(face);
}
void Mesh::add_face(size_t f1, size_t f2, size_t f3) {
    add_face(Face(f1, f2, f3));
}

const Point& Mesh::get_center() const {
    return center;
}

void Mesh::set_center(const Point& pt) {
    center = pt;
}
void Mesh::translate(const Point& tr) {
    center += tr;
}

void Mesh::normalize_center(bool keep_position) {
    Point bary(0., 0., 0.);
    for(const Point& vert: vertices)
        bary += vert;
    bary = (1. / ((double)vertices.size())) * bary;

    for(Point& vert: vertices)
        vert -= bary;

    if(keep_position)
        translate(bary);
    else
        set_center(Point(0, 0, 0));
}

const std::vector<Point>& Mesh::get_vertices() const {
    return vertices;
}

const std::vector<Face>& Mesh::get_faces() const {
    return faces;
}

const Point& Mesh::get_normal(size_t vert_id) {
    if(vert_id >= normals.size())
        throw std::out_of_range("Normal out of range");

    NormalVect& norm = normals[vert_id];
    if(norm.dirty) {
        compute_normal(vert_id);
        norm.dirty = false;
    }
    return norm.vect;
}

size_t Mesh::inner_add_vertice(const Point& pt, const Mesh::NormalVect& normal)
{
    vertices.push_back(pt);
    normals.push_back(normal);
    faces_with_vert.push_back(std::vector<size_t>());
    return vertices.size() - 1;
}

void Mesh::compute_normal(size_t vert) {
    Point normal(0., 0., 0.);
    for(size_t f_id: faces_with_vert[vert]) {
        const Face& face = faces[f_id];
        Point e1 = vertices[face[1]] - vertices[face[0]],
              e2 = vertices[face[2]] - vertices[face[0]];
        normal += e1.cross_prod(e2);
    }

    normal.normalize();
    normals[vert].dirty = false;
    normals[vert].vect = normal;
}