/*
    Most of this code hasn't been made by me (maybe partially tweaked to fit) and just collected those snippets from many sources 
    across the internet. I haven't saved some of the original author names and all the credits 
    should go to them. I'm pretty some of you may find optimizations to them, feel free to leave a comment.
    
    HPlass (hermann.plass@gmail.com) - 2020
*/


/*
  Yaw Pitch Roll to Vector and viceversa conversions.
*/
VEC3  yawToVector(float yaw) {
  return VEC3(sinf(yaw), 0.0f, cosf(yaw));
}
float vectorToYaw(VEC3 front) {
  return atan2f(front.x, front.z);
}
VEC3  yawPitchToVector(float yaw, float pitch) {
  return VEC3(
      sinf(yaw) * cosf(-pitch)
    ,             sinf(-pitch)
    , cosf(yaw) * cosf(-pitch)
  );
}
void  vectorToYawPitch(VEC3 front, float* yaw, float* pitch) {
  *yaw = vectorToYaw(front);
  // Projection of front in the plane XZ
  float mod_xz = sqrtf(front.x*front.x + front.z*front.z);
  *pitch = -atan2f(front.y, mod_xz);
}

/*
    Rotation matrix, rotation quat, angle between vectors.
    Source:
    //http://number-none.com/product/IK%20with%20Quaternion%20Joint%20Limits/
    //https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
*/
QUAT rotateToFrom ( VEC3 v1, VEC3 v2 )
{
  QUAT out = QUAT::Identity;
  VEC3 axis = v1.Cross(v2);// vec3.cross(vec3.create(), v1, v2);
  float dot = v1.Dot(v2);// vec3.dot(v1, v2);

  if (dot < -1.f + 0.01f) {
    out.x = 0.f;
    out.y = 1.f;
    out.z = 0.f;
    out.w = 0.f;
    return out;
  }

  out.x = axis.x * 0.5f;
  out.y = axis.y * 0.5f;
  out.z = axis.z * 0.5f;
  out.w = (1 + dot) * 0.5f;
  
  out.Normalize();//  quat.normalize(out, out);

  return out;
}


MAT44 rotateToFrom( VEC3 A, VEC3 B ) {
	A.Normalize();
	B.Normalize();
	float cos_angle = A.Dot(B);
	VEC3 crossProd = A.Cross(B);

	MAT44 skew(
		0, -crossProd.z, crossProd.y, 0,
		crossProd.z, 0, -crossProd.x, 0,
		-crossProd.y, crossProd.x, 0, 0,
		0, 0, 0, 0
	);

	MAT44 skewPow2 = skew * skew;
	float q = 1.f / (1.f + cos_angle);
	MAT44 rotation = MAT44::Identity + skew + skewPow2 * q;
	return rotation;
}


float angle(VEC3 v1, VEC3 v2) const {
    VEC3 v3 = v1.Cross( v2 );
    float dot_front = v1.Dot(v2);
    float dot_left =  v3.Dot(v2);
    
    return atan2f(dot_left, dot_front);
}

/*
 Get a Quaterion out of vectors
 Source: http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors
*/
QUAT fromaxisangle(float angle, VEC3 axis)
{
    float half_sin = sinf(0.5f * angle);
    float half_cos = cosf(0.5f * angle);
    return QUAT(half_cos,
                half_sin * axis.x,
                half_sin * axis.y,
                half_sin * axis.z);
}

QUAT fromtwovectors(VEC3 u, VEC3 v)
{
    u.Normalize();
    v.Normalize();
    float cos_theta = u.Dot(v);
    float angle = acos(cos_theta);
    VEC3 w = u.Cross(v);
    w.Normalize();
    return fromaxisangle(angle, w);
}



/*
  Usually we use the delta time from frame to frame to make consistent operations framerate-independant, 
  but particularly on lerps doesn't work properly. I found this solution that works like a charm.
  
  Source: //http://www.rorydriscoll.com/2016/03/07/frame-rate-independent-damping-using-lerp/
  Dependencies: https://github.com/Microsoft/DirectXTK/wiki/SimpleMath
*/
float damp(float a, float b, float lambda, float dt) {
  return Lerp(a, b, 1.f - exp(-lambda * dt));
}

VEC2 damp(VEC2 a, VEC2 b, float lambda, float dt) {
  return VEC2::Lerp(a, b, 1.f - exp(-lambda * dt));
}

VEC3 damp(VEC3 a, VEC3 b, float lambda, float dt) {
  return VEC3::Lerp(a, b, 1.f - exp(-lambda * dt));
}

VEC4 damp(VEC4 a, VEC4 b, float lambda, float dt) {
  return VEC4::Lerp(a, b, 1.f - exp(-lambda * dt));
}

QUAT damp(QUAT a, QUAT b, float lambda, float dt) {
  return QUAT::Lerp(a, b, 1.f - exp(-lambda * dt));
}

/*
  Given a position, returns the closest point in the path to that point.
  
  This one I made it for a partrolling entity to return to the patrolling route 
  without having to return to the point where engaged, instead to thec closest 
  point in the route. 
  
  Dependencies: https://github.com/Microsoft/DirectXTK/wiki/SimpleMath
*/
VEC3 closestPoint2Line(VEC3 p, VEC3 a, VEC3 b) {
  VEC3 ab = b - a;
  VEC3 ap = p - a;
  float ab2 = ab.x * ab.x + ab.y * ab.y;
  float APdotAB = ap.Dot(ab);
  float t = APdotAB / ab2;

  if (t > ab.Length()) return b;
  if (t <= 0) return a;
  return a + ab * t;
}

VEC3 closestPoint2Path(VEC3 pos, std::vector<VEC3> path){
  VEC3 point,p;
  size_t size = path.size();
  float d,dist = 9999999999999999.9f;

  for (int i = 0; i < size; ++i){
    p = closestPoint2Line(pos, path[i], path[(i + 1) % size]);
    d = VEC3::Distance(pos, p);
    if (d < dist) {
      dist = d;
      point = p;
    }
  }
  return point;
}