#include "Subdivision.h"
#include "Renderer.h"

#include <vector>
#include <iostream>
#include <assert.h>

Subdivision Subdivision::m_instance;

void Subdivision::performSubdivision(char type)
{
    //Step1
    this->calculateVertexDegree();
    //Step2
    this->calculateIncidentTriangles();
    //Step3
    this->generateAdjacencyTable();
    //Step4
    this->generateLabels();
    //Step5
    this->generateNewVertices();
    this->generateNewTriangles();
    //Step 6
    this->generateNewVertexCoords(type);

    delete Renderer::getInstance()->triangles;
    delete Renderer::getInstance()->points;

    Renderer::getInstance()->triangles = newTriangles;
    Renderer::getInstance()->points = newPoints;

    newTriangles = NULL;
    newPoints = NULL;
    labels.clear();
    adjacencies.clear();
    pointDegrees.clear();

    for (int i = 0; i < (int) Renderer::getInstance()->points->size(); i++) {
        Renderer::getInstance()->points->at(i).incidentTriangles.clear();
    }
}

/*
 * Step 1
 */
void Subdivision::calculateVertexDegree()
{
    std::vector<Point>::iterator p_iter = Renderer::getInstance()->points->begin();

    //Allocate space for all the vertices, zero it out
    for (; p_iter != Renderer::getInstance()->points->end(); p_iter++) {
        pointDegrees.push_back(0);
    }

    std::vector<Triangle>::iterator t_iter = Renderer::getInstance()->triangles->begin();
    
    //Calculate degrees for each vertex
    for (; t_iter != Renderer::getInstance()->triangles->end(); t_iter++) {
        pointDegrees[t_iter->a1]++;
        pointDegrees[t_iter->a2]++;
        pointDegrees[t_iter->a3]++;
    }

    //Display the degree of all vertices
    /*for (unsigned int i = 0; i < pointDegrees.size(); i++) {
        std::cout << "Index: " << i << ": " 
            << pointDegrees[i] << std::endl;
    }*/

}

/*
 * Step 2
 */
void Subdivision::calculateIncidentTriangles() 
{
    std::vector<Triangle> *triangles = Renderer::getInstance()->triangles;
    std::vector<Point> * points = Renderer::getInstance()->points;
    
    for (unsigned int i = 0; i < Renderer::getInstance()->triangles->size(); i++) {
        points->at((*triangles)[i].a1).incidentTriangles.push_back(i);
        points->at((*triangles)[i].a2).incidentTriangles.push_back(i);
        points->at((*triangles)[i].a3).incidentTriangles.push_back(i);
    }

    //Display the incident triangles
    /*std::vector<unsigned int>::iterator ui_iter;
    for (unsigned int i = 0; i < points->size(); i++) 
    {
        std::cout << "Incident Tris for Point: " << i << std::endl << " ";
        ui_iter = (*points)[i].incidentTriangles.begin();
        for (; ui_iter != (*points)[i].incidentTriangles.end(); ui_iter++)
        {
            std::cout << *ui_iter << " ";
        }
        std::cout << "\n";
    }*/

}

/*
 * Step 3
 */
void Subdivision::generateAdjacencyTable() 
{
    std::vector<Triangle> *triangles = Renderer::getInstance()->triangles;
    std::vector<Point> *points = Renderer::getInstance()->points;
    std::vector<unsigned int>::iterator ui_iter; 

    int a1,a2,a3;

    Adjacency tris;

    for (unsigned int i = 0; i < triangles->size(); i++) {
        a1 = (*triangles)[i].a1;
        a2 = (*triangles)[i].a2;
        a3 = (*triangles)[i].a3;

        //Find the adjacent tri to a1 -- a2 a3
        ui_iter = points->at(a2).incidentTriangles.begin();
        for ( ; ui_iter != points->at(a2).incidentTriangles.end(); ui_iter++) {
            if ( *ui_iter != i && (*triangles)[*ui_iter].contains(a3) ) {
                tris.a1 = *ui_iter;
                break;
            }
        }

        //Find the adjacent tri to a2 -- a3 a1
        ui_iter = points->at(a3).incidentTriangles.begin();
        for ( ; ui_iter != points->at(a3).incidentTriangles.end(); ui_iter++) {
            if ( *ui_iter != i && (*triangles)[*ui_iter].contains(a1) ) {
                tris.a2 = *ui_iter;
                break;
            }
        }

        //Find the adjacent tri to a3 -- a1 a2
        ui_iter = points->at(a1).incidentTriangles.begin();
        for ( ; ui_iter != points->at(a1).incidentTriangles.end(); ui_iter++) {
            if ( *ui_iter != i && (*triangles)[*ui_iter].contains(a2) ) {
                tris.a3 = *ui_iter;
                break;
            }
        }

        //Add to Adjacency Table
        this->adjacencies.push_back(tris);
    }

    /*std::vector<Adjacency>::iterator adj_iter = this->adjacencies.begin();
    for (int i = 0; adj_iter != this->adjacencies.end(); adj_iter++, i++) {
        std::cout << "Adjacency " << i << ": " 
            << adj_iter->a1 << " " << adj_iter->a2 
            << " " << adj_iter->a3 << std::endl;
    }*/

}

/*
 * Step 4
 */

void Subdivision::applyToAdjacent(Triangle& tdest, 
                                  int tdest_index, 
                                  int edgeVertex1, 
                                  int edgeVertex2, int ui_current) 
{
    switch (tdest.indexOf(edgeVertex1,edgeVertex2)) {
        case 1:
            labels.at(tdest_index).a1 = ui_current;
            break;
        case 2:
            labels.at(tdest_index).a2 = ui_current;
            break;
        case 3:
            labels.at(tdest_index).a3 = ui_current;
            break;
        default:
            assert(1 != 1);
            break;
    }
}

void Subdivision::generateLabels() {
    Adjacency a;
    
    std::vector<Triangle> *triangles = Renderer::getInstance()->triangles;
    unsigned int tri_index;

    std::vector<Adjacency>::iterator adj_iter = this->adjacencies.begin();
    
    int ui_current = 0;

    a.a1 = a.a2 = a.a3 = -1;

    //Set all to negative 1
    for (; adj_iter != this->adjacencies.end(); adj_iter++) {
        this->labels.push_back(a);
    }

    //Assign edge numbers
    for (tri_index = 0; tri_index < triangles->size(); tri_index++)
    {
        if (labels.at(tri_index).a1 == -1) {
            //Set the opposite edge to this value
            labels.at(tri_index).a1 = ui_current;

            //get the adjacent Triangle
            int adjTri = adjacencies.at(tri_index).a1;
            Triangle &tdest = triangles->at(adjTri);
            Triangle &tsrc = triangles->at(tri_index);
            
            this->applyToAdjacent(tdest,adjTri,tsrc.a2,tsrc.a3,ui_current);

            ui_current++;
        }

        if (labels.at(tri_index).a2 == -1) {
            //Set the opposite edge to this value
            labels.at(tri_index).a2 = ui_current;

            //get the adjacent Triangle
            int adjTri = adjacencies.at(tri_index).a2;
            Triangle &tdest = triangles->at(adjTri);
            Triangle &tsrc = triangles->at(tri_index);
            
            this->applyToAdjacent(tdest,adjTri,tsrc.a1,tsrc.a3,ui_current);

            ui_current++;
        }

        if (labels.at(tri_index).a3 == -1) {
            //Set the opposite edge to this value
            labels.at(tri_index).a3 = ui_current;

            //get the adjacent Triangle
            int adjTri = adjacencies.at(tri_index).a3;
            Triangle &tdest = triangles->at(adjTri);
            Triangle &tsrc = triangles->at(tri_index);
            
            this->applyToAdjacent(tdest,adjTri,tsrc.a1,tsrc.a2,ui_current);

            ui_current++;
        }

    }

    /*std::vector<Adjacency>::iterator lab_iter = this->labels.begin();
    for (; lab_iter != this->labels.end(); lab_iter++) {
        std::cout << "Edge nums: " << lab_iter->a1 << " " << lab_iter->a2 
            << " " << lab_iter->a3 << std::endl;
    }  */  

}

/*
 * Step 5
 */
void Subdivision::generateNewVertices() {
    newPoints = new std::vector<Point>;
    *newPoints = *(Renderer::getInstance()->points);
    Point p;

    p.x = p.y = p.z = 0;

    //std::cout << (int)newPoints->size() << std::endl;

    int numEdges = (3*(int)Renderer::getInstance()->triangles->size()) / 2;

    for (int i = 0; i < numEdges; i++) {
        newPoints->push_back(p);
    }

    //std::cout << (int) newPoints->size() << std::endl;
}

void Subdivision::generateNewTriangles() {
    std::vector<Triangle> *triangles = Renderer::getInstance()->triangles;
    //std::vector<Triangle>::iterator t_iter;
    unsigned int tri_index;
    Triangle tTemp;
    int numVertices = (int) Renderer::getInstance()->points->size();
   
    newTriangles = new std::vector<Triangle>;

    for (tri_index = 0; tri_index < triangles->size(); tri_index++) {
        Triangle &current = (*triangles)[tri_index];
        
        //Generate the first triangle -> a1, a1a2, a3a1
        tTemp.a1 = current.a1;
        tTemp.a2 = labels.at(tri_index).a3 + numVertices;
        tTemp.a3 = labels.at(tri_index).a2 + numVertices;
        newTriangles->push_back(tTemp);


        //Generate the second triangle -> a1a2, a2a3, a3a1
        tTemp.a1 = labels.at(tri_index).a3 + numVertices;
        tTemp.a2 = labels.at(tri_index).a1 + numVertices;
        tTemp.a3 = labels.at(tri_index).a2 + numVertices;
        newTriangles->push_back(tTemp);

        //Generate the third triangle -> a1a2, a2, a2a3
        tTemp.a1 = labels.at(tri_index).a3 + numVertices;
        tTemp.a2 = current.a2;
        tTemp.a3 = labels.at(tri_index).a1 + numVertices;
        newTriangles->push_back(tTemp);

        //Generate the fourth triangle -> a1a3, a2a3, a3
        tTemp.a1 = labels.at(tri_index).a2 + numVertices;
        tTemp.a2 = labels.at(tri_index).a1 + numVertices;
        tTemp.a3 = current.a3;
        newTriangles->push_back(tTemp);
    }

    //std::cout << "new Triangles size: " << newTriangles.size() << std::endl;

}

/*
 * Step 6
 */
void Subdivision::generateNewVertexCoords(char type) {
    if (type == 'L') {
        //this->splitTriangles();
        this->loopSubdivide();
    } else if (type == 'B') {
        //this->splitTriangles();
        this->butterflySubdivide();
    } else {
        this->splitTriangles();
    }
}

Point Subdivision::pointOppositeTo(Triangle &t, int in1, int in2, std::vector<Point> *points) {
    switch (t.indexOf(in1,in2)) {
        case 1:
            return points->at(t.a1);
        case 2:
            return points->at(t.a2);
        case 3:
            return points->at(t.a3);
        default:
            assert(0 != 0);
            //Shut up compiler
            Point p;
            return p;
    }
}

Point Subdivision::farOppositePoints(Triangle &tAdj, int in1, int in2) {
    std::vector<Point> *points = Renderer::getInstance()->points;
    std::vector<Triangle> *triangles = Renderer::getInstance()->triangles;

    Point temp;

    unsigned int ui_num;

    for (int i=0; i < (int) points->at(in1).incidentTriangles.size(); i++) {
        if (triangles->at(points->at(in1).incidentTriangles[i]) == tAdj) {
            ui_num = points->at(in1).incidentTriangles[i];
            break;
        }
    }

    switch (tAdj.indexOf(in1,in2)) {
        case 1: //a2a3 = adjacent edge
            temp = temp + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui_num).a2),tAdj.a1,tAdj.a3,points) 
                 * this->bfly_edge_far_weight);
            temp = temp + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui_num).a3),tAdj.a1,tAdj.a2,points) 
                 * this->bfly_edge_far_weight);
            break;
        case 2: //a1a3 = adjacent edge
            temp = temp + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui_num).a1),tAdj.a2,tAdj.a3,points) 
                 * this->bfly_edge_far_weight);
            temp = temp + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui_num).a3),tAdj.a2,tAdj.a1,points) 
                 * this->bfly_edge_far_weight);
            break;
        case 3: //a1a2 = adjacent edge
            temp = temp + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui_num).a1),tAdj.a2,tAdj.a3,points) 
                 * this->bfly_edge_far_weight);
            temp = temp + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui_num).a2),tAdj.a1,tAdj.a3,points) 
                 * this->bfly_edge_far_weight);
            break;
        default:
            assert(0 != 0);
            break;
    }

    return temp;

}

void Subdivision::butterflySubdivide() {
    std::vector<Point> *points = Renderer::getInstance()->points;
    std::vector<Triangle> *triangles = Renderer::getInstance()->triangles;

    //Copy the old point locations
    for (unsigned int p_index = 0; p_index < (unsigned int) points->size(); p_index++) {
        newPoints->at(p_index) = points->at(p_index);
    }

    unsigned int ui_edge = 0; //Current edge we be using
    
    for (unsigned int ui = 0; ui < labels.size(); ui++) {
        Adjacency &a = labels.at(ui);

        if (a.a1 == ui_edge) {
            Point Q;
            Triangle &t = triangles->at(ui);
            Triangle tAdj = triangles->at(adjacencies.at(ui).a1);

            //Adjacent Edges
            Q = points->at(t.a2) + points->at(t.a3);
            Q = Q * this->bfly_edge_adjacent_weight;

            //Opposite Edges
            Q = Q + (points->at(t.a1) * this->bfly_edge_opposite_weight);
            Q = Q + 
                (pointOppositeTo(tAdj,t.a2,t.a3,points) 
                 * this->bfly_edge_opposite_weight);

            //Far Edges
            //a1a2
            Q = Q + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui).a3),t.a1,t.a2,points) 
                 * this->bfly_edge_far_weight);
            //a1a3
            Q = Q + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui).a2),t.a1,t.a3,points) 
                 * this->bfly_edge_far_weight);
            
            //opposite Triangle
            Q = Q + farOppositePoints(tAdj,t.a2,t.a3);

            newPoints->at(ui_edge + points->size()) = Q;
            ui_edge++;

        }

        if (a.a2 == ui_edge) {
            Point Q;
            Triangle &t = triangles->at(ui);
            Triangle tAdj = triangles->at(adjacencies.at(ui).a2);

            //Adjacent Edges
            Q = points->at(t.a1) + points->at(t.a3);
            Q = Q * this->bfly_edge_adjacent_weight;

            //Opposite Edges
            Q = Q + (points->at(t.a2) * this->bfly_edge_opposite_weight);
            Q = Q + 
                (pointOppositeTo(tAdj,t.a1,t.a3,points) 
                 * this->bfly_edge_opposite_weight);

            //Far Edges
            //a1a2
            Q = Q + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui).a3),t.a1,t.a2,points) 
                 * this->bfly_edge_far_weight);
            //a1a3
            Q = Q + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui).a1),t.a2,t.a3,points) 
                 * this->bfly_edge_far_weight);
            
            //opposite Triangle
            Q = Q + farOppositePoints(tAdj,t.a1,t.a3);

            newPoints->at(ui_edge + points->size()) = Q;
            ui_edge++;

        }

        if (a.a3 == ui_edge) {
            Point Q;
            Triangle &t = triangles->at(ui);
            Triangle tAdj = triangles->at(adjacencies.at(ui).a3);

            //Adjacent Edges
            Q = points->at(t.a1) + points->at(t.a2);
            Q = Q * this->bfly_edge_adjacent_weight;

            //Opposite Edges
            Q = Q + (points->at(t.a3) * this->bfly_edge_opposite_weight);
            Q = Q + 
                (pointOppositeTo(tAdj,t.a1,t.a2,points) 
                 * this->bfly_edge_opposite_weight);

            //Far Edges
            //a1a2
            Q = Q + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui).a2),t.a1,t.a3,points) 
                 * this->bfly_edge_far_weight);
            //a1a3
            Q = Q + 
                (pointOppositeTo(triangles->at(adjacencies.at(ui).a1),t.a2,t.a3,points) 
                 * this->bfly_edge_far_weight);
            
            //opposite Triangle
            Q = Q + farOppositePoints(tAdj,t.a1,t.a2);

            newPoints->at(ui_edge + points->size()) = Q;
            ui_edge++;

        }
    }


    //this->splitTriangles();
}

void Subdivision::loopSubdivide() {
    std::vector<Point> *points = Renderer::getInstance()->points;
    std::vector<Triangle> *triangles = Renderer::getInstance()->triangles;
    
    //Traverse all points and generate their new locations
    for (unsigned int p_index = 0; p_index < points->size(); p_index++) {
        Point &p = points->at(p_index);
        Point temp;

        for (unsigned int ui = 0; ui < p.incidentTriangles.size(); ui++) {
            Triangle t = triangles->at(p.incidentTriangles[ui]);

            temp += points->at(t.a1);
            temp += points->at(t.a2);
            temp += points->at(t.a3);

            //Subtract off self -- is this faster than using a bunch of if's?
            //Probably, since there would be at least one false if (or one true)
            //And that would piss off the pipeline
            temp = temp - p;
        }

        //All points were counted twice
        temp = temp / 2.;

        //Use point Degrees to figure out next step
        if (pointDegrees.at(p_index) == 3) {
            //std::cout << "DEGREE THREE!!!!\n\n";
            temp = temp * this->d3_loop_neighbor_weight;
            temp += p * this->d3_loop_center_weight;
        } else {
            temp = temp * this->dn_loop_neighbor_weight(pointDegrees.at(p_index));
            temp += p * this->dn_loop_center_weight;
        }

        newPoints->at(p_index) = temp;

        /*std::cout << "Old: ";
        p.printMe();
        std::cout << "\nNew: ";
        temp.printMe();
        std::cout << "\n";*/
    }

    /* Do the calculations for edge vertex locations */
    //Calculate the number of edges
    unsigned int ui_edge = 0;

    /*
     * TODO: REFACTOR!!!!
     */
    for (unsigned int ui = 0; ui < labels.size(); ui++) {
        Adjacency &a = labels.at(ui);

        if (a.a1 == ui_edge) {
            Point Q;
            Triangle &t = triangles->at(ui);
            Triangle &tAdj = triangles->at(adjacencies.at(ui).a1);

            //Add the edge points
            Q = points->at(t.a2) + points->at(t.a3);
            Q = Q * this->loop_edge_adjacent_weight;
            
            //Add the opposite points
            Q = Q + (points->at(t.a1) * this->loop_edge_opposite_weight);
            
            switch (tAdj.indexOf(t.a2, t.a3)) {
                case 1:
                    Q = Q + (points->at(tAdj.a1) * this->loop_edge_opposite_weight);
                    break;
                case 2:
                    Q = Q + (points->at(tAdj.a2) * this->loop_edge_opposite_weight);
                    break;
                case 3:
                    Q = Q + (points->at(tAdj.a3) * this->loop_edge_opposite_weight);
                    break;
                default:
                    assert(0 != 0);
                    break;
            }

            newPoints->at(ui_edge + points->size()) = Q;
            ui_edge++;

        }

        if (a.a2 == ui_edge) {
            Point Q;
            Triangle &t = triangles->at(ui);
            Triangle &tAdj = triangles->at(adjacencies.at(ui).a2);

            //Add the edge points
            Q = points->at(t.a1) + points->at(t.a3);
            Q = Q * this->loop_edge_adjacent_weight;
            
            //Add the opposite points
            Q = Q + (points->at(t.a2) * this->loop_edge_opposite_weight);
            
            switch (tAdj.indexOf(t.a1, t.a3)) {
                case 1:
                    Q = Q + (points->at(tAdj.a1) * this->loop_edge_opposite_weight);
                    break;
                case 2:
                    Q = Q + (points->at(tAdj.a2) * this->loop_edge_opposite_weight);
                    break;
                case 3:
                    Q = Q + (points->at(tAdj.a3) * this->loop_edge_opposite_weight);
                    break;
                default:
                    assert(0 != 0);
                    break;
            }

            newPoints->at(ui_edge + points->size()) = Q;
            ui_edge++;

        }

        if (a.a3 == ui_edge) {
            Point Q;
            Triangle &t = triangles->at(ui);
            Triangle &tAdj = triangles->at(adjacencies.at(ui).a3);

            //Add the edge points
            Q = points->at(t.a1) + points->at(t.a2);
            Q = Q * this->loop_edge_adjacent_weight;
            
            //Add the opposite points
            Q = Q + (points->at(t.a3) * this->loop_edge_opposite_weight);
            
            switch (tAdj.indexOf(t.a1, t.a2)) {
                case 1:
                    Q = Q + (points->at(tAdj.a1) * this->loop_edge_opposite_weight);
                    break;
                case 2:
                    Q = Q + (points->at(tAdj.a2) * this->loop_edge_opposite_weight);
                    break;
                case 3:
                    Q = Q + (points->at(tAdj.a3) * this->loop_edge_opposite_weight);
                    break;
                default:
                    assert(0 != 0);
                    break;
            }

            newPoints->at(ui_edge + points->size()) = Q;
            ui_edge++;

        }
    }
}

void Subdivision::splitTriangles() {
    std::vector<Triangle> *triangles = Renderer::getInstance()->triangles;
    std::vector<Point> *points = Renderer::getInstance()->points;

    int numVertices = (int) Renderer::getInstance()->points->size();
    Point* p;
    Triangle tCurr;

    for (int tri_index = 0; tri_index < (int)triangles->size(); tri_index++) {
        tCurr = triangles->at(tri_index);

        /*std::cout << labels.at(tri_index).a1+numVertices << std::endl;
        std::cout << (int) newPoints->size() << std::endl;*/

        //a2a3
        p = &(newPoints->at(labels.at(tri_index).a1+numVertices));
        p->x = (points->at(tCurr.a2).x + points->at(tCurr.a3).x) / 2.;
        p->y = (points->at(tCurr.a2).y + points->at(tCurr.a3).y) / 2.;
        p->z = (points->at(tCurr.a2).z + points->at(tCurr.a3).z) / 2.;

        //a1a3
        p = &(newPoints->at(labels.at(tri_index).a2+numVertices));
        p->x = (points->at(tCurr.a1).x + points->at(tCurr.a3).x) / 2.;
        p->y = (points->at(tCurr.a1).y + points->at(tCurr.a3).y) / 2.;
        p->z = (points->at(tCurr.a1).z + points->at(tCurr.a3).z) / 2.;

        //a1a2
        p = &(newPoints->at(labels.at(tri_index).a3+numVertices));
        p->x = (points->at(tCurr.a2).x + points->at(tCurr.a1).x) / 2.;
        p->y = (points->at(tCurr.a2).y + points->at(tCurr.a1).y) / 2.;
        p->z = (points->at(tCurr.a2).z + points->at(tCurr.a1).z) / 2.;
    }
}