void EnemyShip::changeVelocity(const float& dt, Engine::ParticleSystem& system)
{
	if (target != nullptr)
	{
		Vector2 toTarget = target->getPosition() - shipPosition;
		
		if (LengthSquared(toTarget) != 0)
		{
			Vector2 normalizedtoTarget = PerpCCW(Normalized(toTarget));

			shipRotation = atan2f(normalizedtoTarget.getY(), normalizedtoTarget.getX());

			if (LengthSquared(toTarget) >= 0)
			{
				velocity = velocity + (Engine::Matrix2::rotation(shipRotation) * Engine::Vector2(0, -(acceleration * dt) * dt));
				system.AddParticle(new THRUSTPARTICLEUP);
			}
			else
			{
				velocity = velocity + (Engine::Matrix2::rotation(shipRotation) * Engine::Vector2(0, (acceleration * dt) * dt));
				system.AddParticle(new THRUSTPARTICLEDOWN);
			}
		}
	}
}

SVector2 ArriveBehavior::Update(float deltaTime)
{
    if (mActive)
    {
        SVector2 positionToDestination = mpAgent->GetDestination() - mpAgent->GetPosition();
        SVector2 posToDestNorm = Normalize(positionToDestination);

        float maxSpeed = mpAgent->GetMaxSpeed();

        SVector2 desiredVelocity = posToDestNorm * maxSpeed;

        float distSq = LengthSquared(positionToDestination);
        float velocitySq = LengthSquared(mpAgent->GetVelocity());
        float slowingRadiusSq = 160000.0f;
        float stopRadiusSq = 25.0f;

        if (distSq < velocitySq)
        {
            if (distSq < slowingRadiusSq && distSq > stopRadiusSq )
            {
                //desiredVelocity = -velocitySq/(2*sqrt(distSq));
                desiredVelocity = posToDestNorm * maxSpeed * distSq/(slowingRadiusSq*slowingRadiusSq*slowingRadiusSq*slowingRadiusSq);// * maxSpeed/(distSq*distSq);
            }
            else if (distSq <= stopRadiusSq)
            {
                mpAgent->SetPosition(mpAgent->GetDestination());
                desiredVelocity = SVector2(0.0f, 0.0f);
            }
        }
        return desiredVelocity - mpAgent->GetVelocity();
    }

    return SVector2(0.0f, 0.0f);

}

void ColourNormalFit::Permute3()
{
  const Vec3 scale  = Vec3( 1.0f / 0.5f);
  const Vec3 offset = Vec3(-1.0f * 0.5f);
  const Vec3 scalei = Vec3( 1.0f * 0.5f);
  
  // cache some values
  int const count = m_colours->GetCount();
  Vec3 const* values = m_colours->GetPoints();
  Scr3 const* freq = m_colours->GetWeights();
  
  cQuantizer3<5,6,5> q = cQuantizer3<5,6,5>();
  Scr3 berror = Scr3(DEVIANCE_MAXSUM);
  
  Vec3 c_start = m_start;
  Vec3 c_end   = m_end;
  Scr3 l_start = LengthSquared(Normalize(scale * (offset + m_start)));
  Scr3 l_end   = LengthSquared(Normalize(scale * (offset + m_end)));
  Vec3 q_start = Reciprocal(q.grid + Vec3(1.0f));
  Vec3 q_end   = q_start;

  // adjust offset towards sphere-boundary
  if (!(l_start < Scr3(1.0f)))
    q_start = Vec3(0.0f) - q_start;
  if (!(l_end   < Scr3(1.0f)))
    q_end   = Vec3(0.0f) - q_end;
  
  int trie = 0x3F;
  do {
    // permute end-points +-1 towards sphere-boundary
    Vec3 p_start = q_start & Vec3(!(trie & 0x01), !(trie & 0x02), !(trie & 0x04));
    Vec3 p_end   = q_end   & Vec3(!(trie & 0x08), !(trie & 0x10), !(trie & 0x20));
    
    p_start = q.SnapToLattice(c_start + p_start);
    p_end   = q.SnapToLattice(c_end   + p_end);

    // create a codebook
    // resolve "metric * (value - code)" to "metric * value - metric * code"
    Vec3 codes[3]; Codebook3n(codes, p_start, p_end);

    Scr3 merror = Scr3(DEVIANCE_BASE);
    for (int i = 0; i < count; ++i) {
      // find the closest code
      Vec3 value = Normalize(scale * (offset + values[i]));
      Scr3 dist; MinDeviance3<false>(dist, i, value, codes);
      
      // accumulate the error
      AddDeviance(dist, merror, freq[i]);
    }
    
    if (berror > merror) {
      berror = merror;

      m_start = p_start;
      m_end   = p_end;
    }

  } while(--trie);
}

bool Collisions::sphereToBox(RigidBody *sphere, RigidBody *box, CollisionInfo &info)
{
    //translation
    vec2 nearest;
    vec2 clamp_box = box->collider.dims/2;
    vec2 sphere_in_box_world = sphere->position-box->position;
    nearest.x = clamp(sphere_in_box_world.x,-clamp_box.x,clamp_box.x);
    nearest.y = clamp(sphere_in_box_world.y,-clamp_box.y,clamp_box.y);

    float dist = LengthSquared(sphere_in_box_world-nearest);
    float radius = sphere->collider.radius*sphere->collider.radius;

    if(dist < radius)
    {
        float realDist = sqrt(dist);
        info.intersection = (box->position + nearest) + (sphere_in_box_world*(realDist-sphere->collider.radius)/realDist)/2;
        info.normal = Normalize(nearest - sphere_in_box_world);
        info.type = SPHERE_TO_BOX;

        // Side of box collision
        if(abs(nearest.x) < abs(nearest.y))
            info.boxSideCol = SIDE_EDGE;
        else
            info.boxSideCol = UPPER_EGDE;

        return true;
    }
    else
        return false;
}

float Vector::Length()
{
	float r = (float)sqrt(LengthSquared());
	if ( r < 0.0f )
		r = -r;
	return r;
}

float4 float4::normalize() const
{
	f32 lsqr = LengthSquared();
	if(NearZero(lsqr)) { return ZERO; };
	f32 recip = InvSqrt(lsqr);
	
	return float4(vec[0]*recip, vec[1]*recip, vec[2]*recip, vec[3]*recip);
};

        bool Collides(const CollisionRadius& other) const
        {
            T radiusSquared = RadiusSquared() + other.RadiusSquared();

            auto rawDistance = _position - other._position;

            return rawDistance.LengthSquared() <= radiusSquared;
        }

/*------------------------------------------------------------------------------
normalize this quaternion
------------------------------------------------------------------------------*/
Quaternion& Quaternion::Normalize() {
	float lengthSquared = LengthSquared();
	float invLength = 0;
	if( lengthSquared ) {
		invLength = 1.0f / sqrtf( lengthSquared );
	}
	*this *= invLength;
	return *this;
}

Vector2 Vector2::NormalizeFast() const
{
    float len = LengthSquared();

    if (len > 0)
        return Mul(invsqrt(len));

    return Vector2();
}

void Vector4::FastNormalize()
{
	// No error checking, plus fast inverse square root
	float recLen = FastInvSqRt( LengthSquared() );
	x *= recLen;
	y *= recLen;
	z *= recLen;
	w *= recLen;
}

//--------------------------------------------------------------------
// Computes the average curvature per unit surface distance in the face
//--------------------------------------------------------------------
float ComputeFaceCurvature(Point3 *n, Point3 *v, Point3 bc)
{
	Point3 nc = (n[0]+n[1]+n[2])/3.0f;
	Point3 dn0 = n[0]-nc;
	Point3 dn1 = n[1]-nc;
	Point3 dn2 = n[2]-nc;
	Point3 c = (v[0] + v[1] + v[2]) /3.0f;
	Point3 v0 = v[0]-c;
	Point3 v1 = v[1]-c;
	Point3 v2 = v[2]-c;
	float d0 = DotProd(dn0,v0)/LengthSquared(v0);
	float d1 = DotProd(dn1,v1)/LengthSquared(v1);
	float d2 = DotProd(dn2,v2)/LengthSquared(v2);
	float ad0 = (float)fabs(d0);
	float ad1 = (float)fabs(d1);
	float ad2 = (float)fabs(d2);
	return (ad0>ad1)? (ad0>ad2?d0:d2): ad1>ad2?d1:d2;
}

void PlayerTurret::rotateTurret()
{
	Vector2 mousetoposition = Vector2((float)Input::GetMouseX(), (float)Input::GetMouseY()) - parentPosition;

	if (LengthSquared(mousetoposition) != 0)
	{
		rotationNormal = Engine::Normalized(mousetoposition);
	}
}

  float SSEVector3::Length() const
  {
    float result[ 4 ];
    float lengthSquared = LengthSquared();

    // Store in all floats, do not multiply fourth value: 0111 1111
    const int mask = 0x7F;
    _mm_store_ss( result, _mm_sqrt_ss( _mm_dp_ps( vec, vec, mask ) ) );
    return result[ 0 ];
  }

Vector3 Vector3::Normalize(const Vector3& v)
{
	float lengthSq = LengthSquared(v);
	if (lengthSq == 0.0f)
	{
		return v;
	}

	return v / static_cast<float>(Math::Sqrt(lengthSq));
}

	void magnet::update()
	{
		if(!is_activated())
		{
			return;
		}

		auto mag_pos = body_->GetWorldPoint(local_point_);

		auto ce = body_->GetContactList();
		//			std::set< b2Body* > processed;
		while(ce)
		{
			auto contact = ce->contact;
			if(contact->IsTouching())
			{
				auto fixA = contact->GetFixtureA();
				auto fixB = contact->GetFixtureB();

				//					b2Body* obj = nullptr;
				b2Fixture* fix = nullptr;
				if(fixA == sensor_)
				{
					//						obj = fixB->GetBody();
					fix = fixB;
				}
				else if(fixB == sensor_)
				{
					//						obj = fixA->GetBody();
					fix = fixA;
				}

				//					if(obj != nullptr && processed.find(obj) == processed.end())
				if(fix)
				{
					b2MassData md;
					fix->GetMassData(&md);
					auto metallic_mass = md.mass; // TODO: composition
					auto obj_pos = fix->GetBody()->GetWorldPoint(md.center);
					auto vec = mag_pos - obj_pos;
					auto sqr_dist = vec.LengthSquared();
					auto magnitude = strength_ * metallic_mass / sqr_dist;

					vec *= magnitude / std::sqrt(sqr_dist);
					fix->GetBody()->ApplyForce(vec, obj_pos, true);
					body_->ApplyForce(-vec, mag_pos, true);

					//						processed.insert(obj);
				}
			}

			ce = ce->next;
		}
	}

void
UniformGrid::ClosestPoint(Point3 p, float radius, int &pindex, float &d)
{
	xHitList.ClearAll();
	yHitList.ClearAll();
	zHitList.ClearAll();
	hitList.SetCount(0);

	//find the cell in the XGrid
	TagCells(p,radius, 0);
	//find the cell in the YGrid
	TagCells(p,radius, 1);
	//find the cell in the ZGrid
	TagCells(p,radius, 2);

	BitArray usedList;
	usedList.SetSize(pointBase.Count());
	usedList.ClearAll();

	int closest = -1;
	d = 0.0f;
	Box3 localBounds;
	localBounds.Init();
	localBounds += p;
	localBounds.EnlargeBy(radius);


	for (int i = 0; i < hitList.Count(); i++)
	{
		int index = hitList[i];
		if (!usedList[index])  //check to see if we have processed this one or not
		{
			if (xHitList[index] && yHitList[index] && zHitList[index])
			{
				usedList.Set(index);
				Point3 source = pointBase[index];
				if (localBounds.Contains(source))
				{
					float dist = LengthSquared(source-p);
					if ((dist < d) || (closest == -1))
					{
						d = dist;
						closest = index;
					}
				}
			}
		}
	}
	pindex = closest;
	d = sqrt(d);


}

static void FindVertexAngles(PatchMesh &pm, float *vang) {
	int i;
	for (i=0; i<pm.numVerts + pm.numVecs; i++) vang[i] = 0.0f;
	for (i=0; i<pm.numPatches; i++) {
		Patch &p = pm.patches[i];
		for (int j=0; j<p.type; j++) {
			Point3 d1 = pm.vecs[p.vec[j*2]].p - pm.verts[p.v[j]].p;
			Point3 d2 = pm.vecs[p.vec[((j+p.type-1)%p.type)*2+1]].p - pm.verts[p.v[j]].p;
			float len = LengthSquared(d1);
			if (len == 0) continue;
			d1 /= Sqrt(len);
			len = LengthSquared (d2);
			if (len==0) continue;
			d2 /= Sqrt(len);
			float cs = DotProd (d1, d2);
			if (cs>=1) continue;	// angle of 0
			if (cs<=-1) vang[p.v[j]] += PI;
			else vang[p.v[j]] += (float) acos (cs);
		}
	}
}

void RangeFit::Compress4( void* block )
{
    // cache some values
    int const count = m_colours->GetCount();
    Vec3 const* values = m_colours->GetPoints();

    // create a codebook
    Vec3 codes[4];
    codes[0] = m_start;
    codes[1] = m_end;
    codes[2] = ( 2.0f/3.0f )*m_start + ( 1.0f/3.0f )*m_end;
    codes[3] = ( 1.0f/3.0f )*m_start + ( 2.0f/3.0f )*m_end;

    // match each point to the closest code
    u8 closest[16];
    float error = 0.0f;
    for( int i = 0; i < count; ++i )
    {
        // find the closest code
        float dist = FLT_MAX;
        int idx = 0;
        for( int j = 0; j < 4; ++j )
        {
            float d = LengthSquared( m_metric*( values[i] - codes[j] ) );
            if( d < dist )
            {
                dist = d;
                idx = j;
            }
        }

        // save the index
        closest[i] = ( u8 )idx;

        // accumulate the error
        error += dist;
    }

    // save this scheme if it wins
    if( error < m_besterror )
    {
        // remap the indices
        u8 indices[16];
        m_colours->RemapIndices( closest, indices );

        // save the block
        WriteColourBlock4( m_start, m_end, indices, block );

        // save the error
        m_besterror = error;
    }
}

 static void Normalize(const gmVector3& a_vec, gmVector3& a_result)
 {
   float len2 = LengthSquared(a_vec);
   if(len2 != 0.0f)
   {
     float ooLen = 1.0f / (float)sqrt(len2);
     MulScalar(a_vec, ooLen, a_result);
   }
   else
   {
     a_result.m_x = 0.0f;
     a_result.m_y = 0.0f;
     a_result.m_z = 0.0f;
   }
 }

bool Collisions::sphereToSphere(RigidBody *sphere1, RigidBody *sphere2, CollisionInfo &info)
{
    float dist = sphere1->collider.radius + sphere2->collider.radius;
    if(LengthSquared(sphere1->position - sphere2->position) < dist*dist)
    {
        vec2 pos1 = sphere1->position;
        vec2 pos2 = sphere2->position;

        info.intersection = vec2 {(pos1.x-pos2.x / 2), (pos1.y-pos2.y / 2)};
        info.normal = Normalize(pos1 - pos2);
        info.type = SPHERE_TO_SPHERE;

        return true;
    }
    else
        return false;
}