using UnityEngine;
using System.Collections;

public class WaveAnimation : MonoBehaviour {

    public float posX;
    public float negX;
    public float posZ;
    public float negZ;

    public Vector3 Origin;
    public Vector3 Triangle;


    public float phase;
    public float amp;
    public float speed;
    public float bouyouncyoffset;


    public Vector4 wave1;
    public Vector2 wave1dir;
    public Vector4 wave2;
    public Vector2 wave2dir;
    public Vector4 wave3;
    public Vector2 wave3dir;


    private Vector3 pOne;
    private Vector3 pTwo;
    private Vector3 pThree;
    private Vector3 pFour;
    private Vector3 currentpos;
    Quaternion currentROT;
    private GameObject sphere1;
    private GameObject sphere2;
    private GameObject sphere3;
    private GameObject sphere4;



    // Use this for initialization
    void Start () {

        speed = 1.0f;

        Vector3 currentpos = transform.localPosition;
        //Quaternion currentROT = transform.localRotation;



        Vector3 scale = new Vector3(0.2f, 0.2f, 0.2f);


        sphere1 = GameObject.CreatePrimitive(PrimitiveType.Sphere);
        sphere1.transform.localScale = scale;
        sphere2 = GameObject.CreatePrimitive(PrimitiveType.Sphere);
        sphere2.transform.localScale = scale;
        sphere3 = GameObject.CreatePrimitive(PrimitiveType.Sphere);
        sphere3.transform.localScale = scale;



        //currentROT = transform.localRotation;


    }

	// Update is called once per frame
	void Update ()
    {

        Vector3 currentpos = transform.position;

        //Get Parent Rotation
        Matrix4x4 parentxform = transform.parent.transform.localToWorldMatrix;

        Vector3 localright = parentxform.GetColumn(0);
        Vector3 localforward = parentxform.GetColumn(1);



        Vector3 pOne = currentpos + Origin;
        Vector3 pTwo = pOne + ((localright * Triangle.x));
        Vector3 pThree = pOne + ((localforward * Triangle.z));




        float time = Time.time;

        // Gerstner Positions for Y position
        Vector3 WaveA = GerstnerWave(wave1.x, wave1.y, wave1.z, wave1dir, currentpos, wave1.w, time);
        Vector3 WaveB = GerstnerWave(wave2.x, wave2.y, wave2.z, wave2dir, currentpos, wave2.w, time);
        Vector3 WaveC = GerstnerWave(wave3.x, wave3.y, wave3.z, wave3dir, currentpos, wave3.w, time);

        Vector3 CombinedWave = (WaveA + WaveB + WaveC) / 3.0f;

        //Boat Rotation
        //Put points through gerstner waves
        //POne
        Vector3 pOneWaveA = GerstnerWave(wave1.x, wave1.y, wave1.z, wave1dir, pOne, wave1.w, time);
        Vector3 pOneWaveB = GerstnerWave(wave2.x, wave2.y, wave2.z, wave2dir, pOne, wave2.w, time);
        Vector3 pOneWaveC = GerstnerWave(wave3.x, wave3.y, wave3.z, wave3dir, pOne, wave3.w, time);
        //pTwo
        Vector3 pTwoWaveA = GerstnerWave(wave1.x, wave1.y, wave1.z, wave1dir, pTwo, wave1.w, time);
        Vector3 pTwoWaveB = GerstnerWave(wave2.x, wave2.y, wave2.z, wave2dir, pTwo, wave2.w, time);
        Vector3 pTwoWaveC = GerstnerWave(wave3.x, wave3.y, wave3.z, wave3dir, pTwo, wave3.w, time);
        //POne
        Vector3 pThreeWaveA = GerstnerWave(wave1.x, wave1.y, wave1.z, wave1dir, pThree, wave1.w, time);
        Vector3 pThreeWaveB = GerstnerWave(wave2.x, wave2.y, wave2.z, wave2dir, pThree, wave2.w, time);
        Vector3 pThreeWaveC = GerstnerWave(wave3.x, wave3.y, wave3.z, wave3dir, pThree, wave3.w, time);

        pOne = (pOneWaveA + pOneWaveB + pOneWaveC) / 3.0f;
        pTwo = (pTwoWaveA + pTwoWaveB + pTwoWaveC) / 3.0f;
        pThree = (pThreeWaveA + pThreeWaveB + pThreeWaveC) / 3.0f;

        var right = (pTwo - pOne).normalized;
        var front = (pTwo - pThree).normalized;
        var up = Vector3.Cross(right, front);

        var rotation = Quaternion.LookRotation(right, up);

        float yPos = CombinedWave.y + bouyouncyoffset;
        yPos = (pOne.y + pTwo.y + pThree.y) / 3f;

       transform.position = new Vector3(currentpos.x, yPos, currentpos.z);
       transform.localRotation = Quaternion.RotateTowards(transform.localRotation, rotation, float.MaxValue);

        //Debug Spheres
       sphere1.transform.localPosition = pOne;
       sphere2.transform.position = pTwo;
       sphere3.transform.position = pThree;




    }

    float wave(float wspos,float t)
    {
           return Mathf.Sin(t + wspos);
    }


    Vector3 GerstnerWave(float A, float L, float Q, Vector2 D, Vector3 P0, float Sp, float T)
    {
        float w = 2f * 3.1416f / L;
        Vector2 P0Swizzle;
        P0Swizzle.x = P0.x;
        P0Swizzle.y = P0.z;
        float dotD = Vector2.Dot(P0Swizzle, D);
        float C = Mathf.Cos(w * dotD + (T / 1) * Sp);
        float S = Mathf.Sin(w * dotD + (T / 1) * Sp);

        float x = P0.x + Q * A * C * D.x;
        float y = A * S;
        float z = P0.z + Q * A * C * D.y;

        Vector3 gwave;
        gwave.x = x;
        gwave.y = y;
        gwave.z = z;


        return gwave;

    }


}