#version 330 core
precision highp float;

#extension EXT_gpu_shader4 : require

layout( location = 0 )  in vec4 position_mass;
layout( location = 1 )  in vec4 prev_position;
 
uniform mat4 MVP;
uniform samplerBuffer tex_position_mass;
uniform samplerBuffer tex_prev_position_mass;
uniform vec2  inv_cloth_size;			//rozmiar pojedynczej łaty w przestrzeni świata
uniform vec2  step;						//przyrost rozmiaru tekstury
uniform int texsize_x;					//rozmiar tekstury położeń
uniform int texsize_y; 
uniform float dt, ksStr, ksShr, ksBnd, 
				 kdStr, kdShr, kdBnd, DEFAULT_DAMPING;
 
uniform mat4  ellipsoid_xform;		//transformacje elipsoidy
uniform mat4  inv_ellipsoid;		//odwrotne transformacje elipsoidy
uniform vec4  ellipsoid;			//(środek jako xyz, promień jako w) dla elipsoidy

uniform vec3 gravity;
 
out vec4 out_position_mass;
out vec4 out_prev_position;
 

const vec3 center = vec3(0,0,0);
const float radius = 1;


ivec2 getNextNeighbor(int n, out float ks, out float kd) { 
   //strukturalne siły sprężystości (od najbliższych sąsiadów)
   //        o
   //        |
   //     o--m--o
   //        |
   //        o
   if(n<4) {
       ks = ksStr;
       kd = kdStr;
   }
	if (n == 0) return ivec2( 1, 0);
	if (n == 1) return ivec2( 0, -1);
	if (n == 2) return ivec2(-1, 0);
	if (n == 3) return ivec2( 0, 1);
	
	//siły ścinające (od sąsiadów diagonalnych)
	//     o  o  o
	//      \   /
	//     o  m  o
	//      /   \
	//     o  o  o
	if(n<8) {
       ks = ksShr;
       kd = kdShr;
   }
	if (n == 4) return ivec2( 1,  -1);
	if (n == 5) return ivec2( -1, -1);	
	if (n == 6) return ivec2(-1,  1);
	if (n == 7) return ivec2( 1,  1);
	
	//siły gnące (od sąsiadów oddalonych o 1 węzeł)
	//
	//o   o   o   o   o
	//        | 
	//o   o   |   o   o
	//        |   
	//o-------m-------o
	//        |  
	//o   o   |   o   o
	//        |
	//o   o   o   o   o 
	if(n<12) {
       ks = ksBnd;
       kd = kdBnd;
   }
	if (n == 8) return ivec2( 2, 0);
	if (n == 9) return ivec2( 0, -2);
	if (n ==10) return ivec2(-2, 0);
	if (n ==11) return ivec2( 0, 2);
}

void sphereCollision(inout vec3 x, vec4 sphere)
{
	vec3 delta = x - sphere.xyz;
	float dist = length(delta);
	if (dist < sphere.w) {
		x = sphere.xyz + delta*(sphere.w / dist);
	}
}  
void planeCollision(inout vec3 x,  vec4 plane) {
	 float dist = dot(plane.xyz,x)+ plane.w;
	 if(dist<0) {
		 x += plane.xyz*-dist;
	 }
}
/*
void boxCollision(inout vec3 x, in vec3 x0, vec3 mn, vec3 mx) { 
	vec3 dir = normalize(x-x0);
    vec3 invR = 1.0 / dir;
    vec3 tbot = invR * (mn - x0);
    vec3 ttop = invR * (mx - x0);

    //uporządkuj przecięcia, aby znaleźć najmniejsze i największe na każdej osi
    vec3 tmin = min(ttop, tbot);
    vec3 tmax = max(ttop, tbot);

    //znajdź największe tmin i najmniejsze tmax
    float largest_tmin = max(max(tmin.x, tmin.y), max(tmin.x, tmin.z));
    float smallest_tmax = min(min(tmax.x, tmax.y), min(tmax.x, tmax.z));

    if(  smallest_tmax > largest_tmin) { 
        vec3 center = (mx+mn) * 0.5;
        vec3 hit_point = x0 + largest_tmin*dir;
        vec3 normal = normalize( hit_point-center);
        vec3 absN = abs(normal);
        float mx = max(absN.x,max(absN.y,absN.z));
        if(mx == absN.x)
	       normal=(vec3(1,0,0)*(normal));
        else if(mx == absN.y)
	       normal=(vec3(0,1,0)*(normal));
        else if(mx == absN.z)
	       normal=(vec3(0,0,1)*(normal));

		float d = length(hit_point-x);
		x  = x0;

   } 
}
*/
void main(void) 
{  
	float m = position_mass.w;
	vec3 pos = position_mass.xyz;
    vec3 pos_old = prev_position.xyz;	
	vec3 vel = (pos - pos_old) / dt;
	float ks=0, kd=0;
	int index = gl_VertexID;
	int ix = index % texsize_x;
	int iy = index / texsize_x;
	if(index ==0 || index == (texsize_x-1))
		m = 0;
    vec3 F = gravity*m + (DEFAULT_DAMPING*vel);
	
	for(int k=0;k<12;k++) {
		ivec2 coord = getNextNeighbor(k, ks, kd);
		int j = coord.x;
		int i = coord.y;		
		if (((iy + i) < 0) || ((iy + i) > (texsize_y-1)))
			continue;

		if (((ix + j) < 0) || ((ix + j) > (texsize_x-1)))
			continue;

		int index_neigh = (iy + i) * texsize_x + ix + j;

		vec3 p2 = texelFetchBuffer(tex_position_mass, index_neigh).xyz;
		vec3 p2_last = texelFetchBuffer(tex_prev_position_mass, index_neigh).xyz;
		
		vec2 coord_neigh = vec2(ix + j, iy + i)*step;
		float rest_length = length(coord*inv_cloth_size);

		vec3 v2 = (p2- p2_last)/dt;
		vec3 deltaP = pos - p2;	
		vec3 deltaV = vel - v2;	 
		float dist = length(deltaP);
				
		float   leftTerm = -ks * (dist-rest_length);
		float  rightTerm = kd * (dot(deltaV, deltaP)/dist);		
		vec3 springForce = (leftTerm + rightTerm)* normalize(deltaP);
		F +=  springForce;	
	}
    vec3 acc = vec3(0);
	if(m!=0)
	   acc = F/m; 

	
	vec3 tmp = pos; 
	pos = pos * 2.0 - pos_old + acc* dt * dt;
	pos_old = tmp;

	//kolizję z podłożem zastąpiła kolizja z płaszczyzną 
	//pos.y=max(0, pos.y);

	//kolizja z płaszczyzną 
	//planeCollision(pos, vec4(0,1,0,0));

	//kolizja ze sferą
	//sphereCollision(pos, vec4(0,2,0,1)) ;

	//kolizja z prostopadłościanem
	///boxCollision(pos, pos_old, vec3(0,0,0), vec3(1,1,1));

	
	//kolizja z elipsoidą	
	vec4 x0 = inv_ellipsoid*vec4(pos,1); 
	vec3 delta0 = x0.xyz-ellipsoid.xyz;
	float dist2 = dot(delta0, delta0);
	if(dist2<1) {  
		delta0 = (ellipsoid.w - dist2) * delta0 / dist2;
		//przetransformuj delta do pierwotnej przestrzeni
		vec3 delta;
		vec3 transformInv = vec3(ellipsoid_xform[0].x, ellipsoid_xform[1].x, ellipsoid_xform[2].x);
		transformInv /= dot(transformInv, transformInv);

		delta.x = dot(delta0, transformInv);
		transformInv = vec3(ellipsoid_xform[0].y, ellipsoid_xform[1].y, ellipsoid_xform[2].y);
		transformInv /= dot(transformInv, transformInv);

		delta.y = dot(delta0, transformInv);
		transformInv = vec3(ellipsoid_xform[0].z, ellipsoid_xform[1].z, ellipsoid_xform[2].z);
		transformInv /= dot(transformInv, transformInv);

		delta.z = dot(delta0, transformInv); 
		pos +=  delta ; 
		pos_old = pos; //to zostało dodane, aby wypadkowa prędkość w następnej iteracji była równa zero
					   //usunięcie tego spowoduje, że kolidujące punkty będą ciągle podskakiwały
	}
	
	out_position_mass = vec4(pos, m);	
	out_prev_position = vec4(pos_old,m);			  
	gl_Position = MVP*vec4(pos, 1);
	

	/*
	//Debug
	out_position_mass = vec4(ix,iy,0,1);
	out_prev_position = out_position_mass ;
	gl_Position = MVP*vec4(pos,1.0);
	*/
}