using UnityEngine;
using System.Collections;

public class MotorManager : MonoBehaviour {

    GameObject[] motorPositions;
    public float[] angles;
    MonsterSpawner spawner;

    public float[] motorVals = new float[5];

    public string[] motorNames = new string[5];

    ArduinoHandler arduino;

    private int[] motorOuts;

	// Use this for initialization
	void Start () {
        motorPositions = new GameObject[] { GameObject.Find("ForwardCenter"), GameObject.Find("ForwardRight"),
            GameObject.Find("RearRight"), GameObject.Find("RearLeft"), GameObject.Find("ForwardLeft")};

        //motorPositions = GameObject.FindGameObjectsWithTag("MotorPosition");
        spawner = GameObject.FindGameObjectWithTag("MonsterSpawner").GetComponent<MonsterSpawner>();

        angles = new float[motorPositions.Length];

        for (int i = 0; i < motorPositions.Length; i++)
        {
            motorNames[i] = motorPositions[i].name;

            angles[i] = Vector3.Angle(-transform.up, (transform.position - motorPositions[i].transform.position));

            if (motorPositions[i].transform.localPosition.x < 0)
                angles[i] = -angles[i] + 360;
        }

        motorOuts = new int[5];
        arduino = GameObject.FindGameObjectWithTag("ArduinoHandler").GetComponent<ArduinoHandler>();

	}

    // Update is called once per frame
    void Update()
    {
        for (int i = 0; i < motorVals.Length; i++)
        {
            motorVals[i] = 0;
        }

        foreach (Monster monster in spawner.monsters)
        {
            for (int i = 0; i < motorPositions.Length; i++)
            //foreach (GameObject motorPosition in motorPositions)
            {
                float angleToMonster = Vector3.Angle(transform.up, monster.transform.position - transform.position);

                if (transform.InverseTransformPoint(monster.transform.position).x < 0)
                {
                    angleToMonster = -angleToMonster + 360;
                }

//                Debug.Log("AngleToMonster: " + angleToMonster);
                

                int motornum = 0;

                while (motornum < motorPositions.Length && angles[motornum] < angleToMonster)
                {
                    motornum++;
                }

                if (motornum > 4)
                    motornum = 0;

                int rightMotor = motornum;
                int leftMotor = motornum - 1;

                if (leftMotor < 0)
                {
                    leftMotor = 4;
                }

                float angleToRight = angles[rightMotor] - angleToMonster;

                if (angleToRight < 0)
                    angleToRight += 360;

                float angleToLeft = angleToMonster - angles[leftMotor];

                float totalAngle = angleToLeft + angleToRight;

                float rightPercent = angleToLeft / totalAngle;
                float leftPercent = angleToRight / totalAngle;

                float inverseDist = 1 / (transform.position - monster.transform.position).magnitude;

                motorVals[leftMotor] += leftPercent * inverseDist;
                motorVals[rightMotor] += rightPercent * inverseDist;

                //Vector3 motorOffset = (motorPositions[i].transform.position - transform.position).normalized;

                //float val = Vector3.Dot(motorOffset, monster.transform.position);

                //if (val < 0)
                //    val = 0;
                //else
                //    val = 1 / Mathf.Pow(val, 2);

                //motorVals[i] = val;
            }
        }

        for (int i = 0; i < motorVals.Length; i++)
        {
            motorOuts[i] = (int)Mathf.Clamp(map(motorVals[i], 0, 1, 30, 255), 0, 255);
        }

        arduino.motorvals = motorOuts;
	}

    float map(float x, float in_min, float in_max, float out_min, float out_max)
    {
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
    }

}
