using UnityEngine;
using System.Collections;

public class Exercise_1_Solution
{
    // Hints:
    // You will need to use dot and cross products.

    // "PointToTest" is already lying on a plane with a triangle.
    public static bool IsPointInsideTriangle(Vector3 A, Vector3 B, Vector3 C, Vector3 TriangleNormal, Vector3 PointToTest)
    {
        Vector3 edge0 = B - A;
        Vector3 cross0 = Vector3.Cross(edge0, PointToTest - A);
        if (Vector3.Dot(cross0, TriangleNormal) < 0.0f)
            return false;
        
        Vector3 edge1 = C - B;
        Vector3 cross1 = Vector3.Cross(edge1, PointToTest - B);
        if (Vector3.Dot(cross1, TriangleNormal) < 0.0f)
            return false;
        
        Vector3 edge2 = A - C;
        Vector3 cross2 = Vector3.Cross(edge2, PointToTest - C);
        if (Vector3.Dot(cross2, TriangleNormal) < 0.0f)
            return false;
        
        return true;
    }
}

public class Exercise_2_Solution
{
    // Hints:
    // You will need to use dot product and vector operations (like normalization, distance comparison).

    public static Vector3 CalculateNearestPointOnCurve(Vector3[] CurvePoints, Vector3 PointToTest)
    {
        Vector3 Result = Vector3.zero;
        float nearestDistanceSqr = float.MaxValue;
        
        for (int i=0 ; i<(CurvePoints.Length-1) ; ++i)
        {
            Vector3 CurveDir = CurvePoints[i+1] - CurvePoints[i];
            Vector3 CurveDirNormalized = CurveDir.normalized;
            
            Vector3 vP = PointToTest - CurvePoints[i];
            float dotValue = Vector3.Dot(CurveDirNormalized, vP);
            
            Vector3 PointOnCurve = dotValue * CurveDirNormalized;
            PointOnCurve += CurvePoints[i];
            
            if (dotValue < 0.0f)
            {                                  
                PointOnCurve = CurvePoints[i];
            }
            else if (dotValue > CurveDir.magnitude)
            {
                PointOnCurve = CurvePoints[i+1];
            }
            
            float currentDistanceSqr = (PointToTest - PointOnCurve).sqrMagnitude;
            if (currentDistanceSqr < nearestDistanceSqr)
            {
                Result = PointOnCurve;
                nearestDistanceSqr = currentDistanceSqr;
            }
        }
        
        return Result;
    }
}

public class Exercise_3_Solution
{
    // Hints:
    // 
    // Rotation matrix around x-axis:
    // [ 1, 0,    0,     0 ]
    // [ 0, cosA, -sinA, 0 ]
    // [ 0, sinA, cosA,  0 ]
    // [ 0, 0,    0,     1 ]
    //
    // Rotation matrix around y-axis:
    // [ cosA,  0, sinA, 0 ]
    // [ 0,     1, 0,    0 ]
    // [ -sinA, 0, cosA, 0 ]
    // [ 0,     0, 0,    1 ]

    private static Matrix4x4 RotationMatrixAroundX(float AngleX)
    {
        Matrix4x4 m = Matrix4x4.identity;
        m.m11 = Mathf.Cos(AngleX);
        m.m12 = -Mathf.Sin(AngleX);
        m.m21 = Mathf.Sin(AngleX);
        m.m22 = Mathf.Cos(AngleX);
        return m;
    }
    
    private static Matrix4x4 RotationMatrixAroundY(float AngleY)
    {
        Matrix4x4 m = Matrix4x4.identity;
        m.m00 = Mathf.Cos(AngleY);
        m.m02 = Mathf.Sin(AngleY);
        m.m20 = -Mathf.Sin(AngleY);
        m.m22 = Mathf.Cos(AngleY);
        return m;
    }
    
    public static Matrix4x4 AddRotationsFromAngles(Matrix4x4 BaseTransform, float AngleX, float AngleY)
    {
        return RotationMatrixAroundX(AngleX) * RotationMatrixAroundY(AngleY) * BaseTransform;
    }
}

public class Exercise_4_Solution
{
    // Hints:
    //
    // Position and velocity integration:
    //    NewPosition = PreviousPosition + Velocity * dt
    //    NewVelocity = PreviousVelocity + Acceleration * dt
    //
    // Simulation is running at 50Hz.
    // Coefficient of restitution is 0.5.

    public static Vector3 PredictPosition(Vector3 InitialPosition, Vector3 InitialLinearVelocity, float TimeToPredict)
    {
        Vector3 position = InitialPosition;
        Vector3 velocity = InitialLinearVelocity;
        Vector3 acceleration = new Vector3(0.0f, -9.8f, 0.0f);
        
        float t = 0.0f;
        float dt = 0.02f;
        while (t < TimeToPredict)
        {
            velocity += acceleration * dt;
            position += velocity * dt;
            
            if (position.y <= 0.0f)
            {
                position.y = 0.0f;
                
                float restitution = 0.5f;
                
                Vector3 contactNormal = new Vector3(0.0f, 1.0f, 0.0f);
                float projectedVelocity = Vector3.Dot(contactNormal, velocity);
                velocity -= contactNormal * projectedVelocity;
                velocity -= restitution * contactNormal * projectedVelocity;
            }
            
            t += dt;
        }
        
        return position;
    }
}