#include <iostream>
#include "mesh.h"
#include <stdio.h>
#include <math.h>
#include <stdlib.h>


Mesh::Mesh()
{
  verts=0;
  colors=0;
  tris=0;
  uvs=0;

  nbrVerts=0;
  nbrTris=0;
}

Mesh::~Mesh()
{
  delete [] verts;
  delete [] uvs;
  delete [] colors;
  delete [] tris;
}

void Mesh::initVerts(int p_nbrVerts)
{
  if (verts) return;
  nbrVerts=p_nbrVerts;
  verts =new Vertex[nbrVerts];
  uvs   =new     UV[nbrVerts];
  colors=new  Color[nbrVerts];
  
  vertNorm=new Normal[nbrVerts];
  vertNeigh=new Neighbors[nbrVerts];
}

void Mesh::initTris(int p_nbrTris)
{
  nbrTris=p_nbrTris;
  tris=new Triangle[nbrTris];
  
  triNorm=new Normal[nbrTris];
}

void Mesh::initNeighbors() //carrega a estrutura de dados de vizinhanca da malha
{
  for (int t=0;t<nbrTris;t++)
  {
    for (int v=0;v<3;v++)
    {
      vertNeigh[tris[t].vert[v]].addNeighbor(t);
    }
  }
}

void Mesh::calcNormals() // calcula todas as normais da malha, carregando a estrutura de dados
{
  // First calculate the triangle normals
  for (int t=0;t<nbrTris;t++)
  {
    calcNormal(verts[tris[t].vert[0]],
               verts[tris[t].vert[1]],
               verts[tris[t].vert[2]],
               triNorm[t]);
  }
  // Now calculate the vertex normals
  for (int v=0;v<nbrVerts;v++)
  {
    vertNorm[v].x=vertNorm[v].y=vertNorm[v].z=0;
    for (int t=0;t<vertNeigh[v].nbr;t++)
    {
      vertNorm[v].x+=triNorm[vertNeigh[v].tri[t]].x;
      vertNorm[v].y+=triNorm[vertNeigh[v].tri[t]].y;
      vertNorm[v].z+=triNorm[vertNeigh[v].tri[t]].z;
    }
    normalize(vertNorm[v]);
  }
}

void Mesh::criaPlano (int n, int m, float offset){
     
     initVerts( (n+1) * (m+1) );
     initTris ( n*m*2 );
     
     //aloca matriz intermidiária
     int **mTemp = new int*[(n+1)];
     for (int i=0; i <n+1; i++)
         mTemp[i] = new int [m+1];
     
     //cria vértices
     for (int i=0; i < n+1; i++){
         for (int j=0; j < m+1; j++){
            // float ui = (i/(MAXx-MINx)) + (MAXx/(MAXx-MINx));
            // float vi = -(j/(MAXy-MINy)) + (MAXy/(MAXy-MINy));
             
             float Rr = matrix[i*255/n][j*255/m].r;
            // float Rg = matrix[(int)(ui*255)][(int)(vi*255)].g;
            // float Rb = matrix[(int)(ui*255)][(int)(vi*255)].b;
             
             //coordenadas do vértice
             verts[i+j*(n+1)].x = i*offset;
             verts[i+j*(n+1)].y = Rr/5.0; //50
             
             verts[i+j*(n+1)].z = j*offset;
             
             
             //coordenadas da textura
             uvs[i+j*(n+1)].u = ((float)i/(float)n);
             uvs[i+j*(n+1)].v = ((float)j/(float)m);
             //printf( "RGB  %f  \n", Rr );
             
             //carrega o índice da matriz
             mTemp[i][j] = i+j*(n+1);
         }
     }
     //cria triangulos ligando os vertices
     int vv=0;
     for (int i=0; i < n; i++){
         for (int j=0; j < m; j++){
             tris[vv].vert[0] = mTemp[i][j];
             tris[vv].vert[1] = mTemp[i][j+1];
             tris[vv].vert[2] = mTemp[i+1][j+1];
             vv++;
             tris[vv].vert[0] = mTemp[i][j];
             tris[vv].vert[1] = mTemp[i+1][j+1];
             tris[vv].vert[2] = mTemp[i+1][j];
             vv++;
         }
     }
}

void MassaMola::initMassas(int p_nbrMassas){
  if (vMassa) return;
  nbrMassas=p_nbrMassas;
  vMassa = new Massa[nbrMassas];
}

void MassaMola::initMolas(int p_nbrMolas){
  nbrMolas=p_nbrMolas;
  vMola=new Mola[nbrMolas];
}


void MassaMola::criaMassaMola (int n, int m, float offset){
     
     initMassas( (n+1) * (m+1) );
     initMolas ( n*m*6 );
     
     initVerts( (n+1) * (m+1) );
     initTris ( n*m*2 );
     
     
     //aloca matriz intermidiária
     int **mTemp = new int*[(n+1)];
     for (int i=0; i <n+1; i++)
         mTemp[i] = new int [m+1];
     
     //cria vértices
     for (int i=0; i < n+1; i++){
         for (int j=0; j < m+1; j++){
             
             //system ("pause");
             //coordenadas do vértice e oordenadas da massa
             vMassa[i+j*(n+1)].posicao[0] = verts[i+j*(n+1)].x = i*offset;
             vMassa[i+j*(n+1)].posicao[1] = verts[i+j*(n+1)].y = j*offset;
             vMassa[i+j*(n+1)].posicao[2] = 0;
             
             //define a massa
             vMassa[i+j*(n+1)].massa = 1.0;
             vMassa[i+j*(n+1)].fixa = false; // é false
             
             vMassa[i+j*(n+1)].velocidade[0] = 0;
             vMassa[i+j*(n+1)].velocidade[1] = 0;
             vMassa[i+j*(n+1)].velocidade[2] = 0;
             
             //carrega o índice da matriz
             mTemp[i][j] = i+j*(n+1);
             if (vMassa[i+j*(n+1)].posicao[0]==0){
                if (vMassa[i+j*(n+1)].posicao[1]==0){  
                   //system ("pause");       
                   vMassa[i+j*(n+1)].fixa = true;
                }
                else if (vMassa[i+j*(n+1)].posicao[1]==m*offset){
                     //system ("pause");
                     vMassa[i+j*(n+1)].fixa = true;
                }
             }
             if (vMassa[i+j*(n+1)].posicao[1]==0){
                if (vMassa[i+j*(n+1)].posicao[0]==n*offset){
                     //system ("pause");
                     vMassa[i+j*(n+1)].fixa = true;
                }
             }
             /*if ((vMassa[i+j*(n+1)].posicao[0]==n*offset)&&(vMassa[i+j*(n+1)].posicao[1]==m*offset)){
                //system ("pause");
                vMassa[i+j*(n+1)].fixa = true;
             }*/
             
         }
     }
     //cria triangulos ligando os vertices
     int vv=0;
     int aux = 0;
     for (int i=0; i < n; i++){
         for (int j=0; j < m; j++){
             tris[vv].vert[0] = mTemp[i][j];
             tris[vv].vert[1] = mTemp[i][j+1];
             tris[vv].vert[2] = mTemp[i+1][j+1];
                 vMola[aux].massas[0] = mTemp[i][j];
                 vMola[aux].massas[1] = mTemp[i][j+1];
                 vMola[aux].comp_nominal = 1; //??? offset
                 vMola[aux].k = 7;
                 aux++;
                 vMola[aux].massas[0] = mTemp[i][j+1];
                 vMola[aux].massas[1] = mTemp[i+1][j+1];
                 vMola[aux].comp_nominal = 1; //??? offset
                 vMola[aux].k = 7;
                 aux++;
                 vMola[aux].massas[0] = mTemp[i+1][j+1];
                 vMola[aux].massas[1] = mTemp[i][j];
                 vMola[aux].comp_nominal = sqrt(2); //??? offset
                 vMola[aux].k = 7;
                 aux++;
             vv++;
             tris[vv].vert[0] = mTemp[i][j];
             tris[vv].vert[1] = mTemp[i+1][j+1];
             tris[vv].vert[2] = mTemp[i+1][j];
                 vMola[aux].massas[0] = mTemp[i][j];
                 vMola[aux].massas[1] = mTemp[i+1][j+1];
                 vMola[aux].comp_nominal = sqrt(2); //??? offset
                 vMola[aux].k = 7;
                 aux++;
                 vMola[aux].massas[0] = mTemp[i+1][j+1];
                 vMola[aux].massas[1] = mTemp[i+1][j];
                 vMola[aux].comp_nominal = 1; //??? offset
                 vMola[aux].k = 7;
                 aux++;
                 vMola[aux].massas[0] = mTemp[i+1][j];
                 vMola[aux].massas[1] = mTemp[i][j];
                 vMola[aux].comp_nominal = 1; //??? offset
                 vMola[aux].k = 7;
                 aux++;
             vv++;
         }
     }
}

float MassaMola::calculaForca(int mola){
      float x1 = vMassa[vMola[mola].massas[0]].posicao[0];
      float y1 = vMassa[vMola[mola].massas[0]].posicao[1];
      float z1 = vMassa[vMola[mola].massas[0]].posicao[2];
      
      float x2 = vMassa[vMola[mola].massas[1]].posicao[0];
      float y2 = vMassa[vMola[mola].massas[1]].posicao[1];
      float z2 = vMassa[vMola[mola].massas[1]].posicao[2];
      
      float dist = sqrt( pow((x1-x2),2) + pow((y1-y2),2) + pow((z1-z2),2));
      
      float forca = vMola[mola].k * (dist - vMola[mola].comp_nominal);
      
      return forca;
}

void MassaMola::simulaFisica(float deltaT){
     
     float gravidade[3] = {0.5,0,0.5}; //vetor com direção da força aplicada na massa/mola
     
     for( int i = 0; i < nbrMolas; i++ ){

		// calcula direcao da forca pela orientacao da mola
		float direcao[3];
		direcao[0] = vMassa[vMola[i].massas[0]].posicao[0] - 
			         vMassa[vMola[i].massas[1]].posicao[0];
		direcao[1] = vMassa[vMola[i].massas[0]].posicao[1] - 
			         vMassa[vMola[i].massas[1]].posicao[1];
		direcao[2] = vMassa[vMola[i].massas[0]].posicao[2] - 
			         vMassa[vMola[i].massas[1]].posicao[2];

		// normaliza a direcao
		float modulo = sqrt( direcao[0] * direcao[0]
							+direcao[1] * direcao[1]
							+direcao[2] * direcao[2] );
		direcao[0] /= modulo; 
		direcao[1] /= modulo;
		direcao[2] /= modulo;

		// cria um vetor forca da mola
		float forca[3];
		float localF = calculaForca(i);
		forca[0] = direcao[0] * localF;
		forca[1] = direcao[1] * localF;
		forca[2] = direcao[2] * localF;
		//printf( "%f ", localF );

		// calcula aceleracao
		float a[3];
		a[0] = forca[0] / vMassa[vMola[i].massas[0]].massa;
		a[1] = forca[1] / vMassa[vMola[i].massas[0]].massa;
		a[2] = forca[2] / vMassa[vMola[i].massas[0]].massa;

		// truque pra regular as unidades (km ou mm?)
		a[0]/=1000.0;
		a[1]/=1000.0;
		a[2]/=1000.0;
		
		// calcula a velocidade
		float v[3];
		v[0] = vMassa[vMola[i].massas[0]].velocidade[0]*0.99 + a[0] + gravidade[0] * deltaT;
		v[1] = vMassa[vMola[i].massas[0]].velocidade[1]*0.99 + a[1] + gravidade[1] * deltaT;
		v[2] = vMassa[vMola[i].massas[0]].velocidade[2]*0.99 + a[2] + gravidade[2] * deltaT;

		// salva velocidade para a proxima iteracao
		vMassa[vMola[i].massas[0]].velocidade[0] = v[0];
		vMassa[vMola[i].massas[0]].velocidade[1] = v[1];
		vMassa[vMola[i].massas[0]].velocidade[2] = v[2];
		
		float deslocamento[3];
		deslocamento[0]= (v[0]);
		deslocamento[1]= (v[1]);
		deslocamento[2]= (v[2]);


		//aplica deslocamento 'a massa 0
		if( ! vMassa[vMola[i].massas[0]].fixa ){
			verts[vMola[i].massas[0]].x = (vMassa[vMola[i].massas[0]].posicao[0] -= deslocamento[0]);
			verts[vMola[i].massas[0]].y = (vMassa[vMola[i].massas[0]].posicao[1] -= deslocamento[1]);
			verts[vMola[i].massas[0]].z = (vMassa[vMola[i].massas[0]].posicao[2] -= deslocamento[2]);
		}

		//aplica deslocamento contrario 'a massa 1
		if( ! vMassa[vMola[i].massas[1]].fixa ){
			verts[vMola[i].massas[1]].x = (vMassa[vMola[i].massas[1]].posicao[0] += deslocamento[0]);
			verts[vMola[i].massas[1]].y = (vMassa[vMola[i].massas[1]].posicao[1] += deslocamento[1]);
			verts[vMola[i].massas[1]].z = (vMassa[vMola[i].massas[1]].posicao[2] += deslocamento[2]);
		}
	}
	
}
