// Use this for initialization
 void Start()
 {
     //forward = new Quaternion(0, 0, 0, 0);
     // ResetFocus();
     curOffset = Vector3.zero;
     curLBI    = null;
 }
    // Update is called once per frame
    void FixedUpdate()
    {
        if (!stopUpdate && !disabled)
        {
            refreshTimer  -= Time.fixedDeltaTime;
            refreshTimer2 -= Time.fixedDeltaTime;
            if (vehicleScript != null)
            {
                if (vehicleScript.player)
                {
                    stopUpdate = true; //temp solution
                }
                else if (!vehicleScript.broken)
                {
                    {
                        Vector3 pos = transform.position + transform.up;

                        RaycastHit hit;

                        if (refreshTimer <= 0 && Physics.Raycast(pos, -transform.up, out hit, 1000f))//, 1 << LayerMask.NameToLayer("Road")))
                        {
                            if (hit.transform.GetComponent <LevelBlockInfo>() && hit.transform.GetComponent <LevelBlockInfo>() != curLBI)
                            {
                                curLBI      = hit.transform.GetComponent <LevelBlockInfo>();
                                curWaypoint = 0;
                                if (curLBI.Lanes != null)
                                {
                                    curLBIQueue = curLBI.Lanes[laneID];
                                }
                            }
                            else if (hit.transform.parent != null && hit.transform.parent.transform.GetComponent <LevelBlockInfo>() && hit.transform.parent.transform.GetComponent <LevelBlockInfo>() != curLBI)
                            {
                                curLBI      = hit.transform.parent.transform.GetComponent <LevelBlockInfo>();
                                curWaypoint = 0;
                                if (curLBI.Lanes != null)
                                {
                                    curLBIQueue = curLBI.Lanes[laneID];
                                }
                            }
                            else if (hit.transform.parent != null && hit.transform.parent.transform.parent != null && hit.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>() && hit.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>() != curLBI)
                            {
                                curLBI      = hit.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>();
                                curWaypoint = 0;
                                if (curLBI.Lanes != null)
                                {
                                    curLBIQueue = curLBI.Lanes[laneID];
                                }
                            }
                            else if (hit.transform.parent != null && hit.transform.parent.transform.parent != null && hit.transform.parent.transform.parent.transform.parent != null && hit.transform.parent.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>() && hit.transform.parent.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>() != curLBI)
                            {
                                curLBI      = hit.transform.parent.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>();
                                curWaypoint = 0;
                                if (curLBI.Lanes != null)
                                {
                                    curLBIQueue = curLBI.Lanes[laneID];
                                }
                            }
                        }


                        if (refreshTimer2 <= 0 && Physics.Raycast(pos, transform.forward, out hit, dist, layerMask))
                        {
                            float distance = hit.distance;

                            if (distance <= 5)
                            {
                                // Mash dat break
                                breakForce = 10000f;
                                motor      = -1f;
                            }
                            else if (prevHitDistance >= distance)
                            {
                                // Getting close to something need to slow down
                                breakForce = 1850f;
                                motor      = 0f;
                            }
                            else
                            {
                                // Object in front of me is moving I can move
                                breakForce = 0f;
                                motor      = 1f;
                            }

                            prevHitDistance = distance;
                        }
                        else
                        {
                            // Open road just drive
                            motor      = 1f;
                            breakForce = 0f;
                        }

                        //input motor movement
                        vehicleScript.inputAccel(motor);

                        //lr movement based on waypoints
                        Vector3 target = Vector3.zero;
                        if (curLBIQueue != null && curLBIQueue.Count > curWaypoint && curLBIQueue[curWaypoint] != null)
                        {
                            target = curLBIQueue[curWaypoint].position;
                            Vector3 targetDir = (target - transform.position);


                            //input horizontal movement
                            vehicleScript.inputHorz(AngleDir(transform.forward, targetDir, transform.up));

                            if (DebugThis)
                            {
                                Debug.Log(" left or right " + AngleDir(transform.forward, targetDir, transform.up));
                            }

                            //update waypoints
                            if ((transform.position - target).magnitude < 4f)
                            {
                                curWaypoint++;
                            }
                        }
                        else
                        {
                            vehicleScript.inputHorz(0);
                        }

                        if (refreshTimer <= 0)
                        {
                            refreshTimer = refreshTimeSet;
                        }
                        if (refreshTimer2 <= 0)
                        {
                            refreshTimer2 = refreshTimeSet;
                        }
                    }
                }
            }
        }
    }
    // Update is called once per frame
    void FixedUpdate()
    {
        if (focus != null)
        {
            float      dist      = 100f;
            int        layerMask = 1 << LayerMask.NameToLayer("Vehicle");
            Vector3    pos       = focus.position + focus.up;
            RaycastHit hit;


            //update waypoint queues
            if (Physics.Raycast(pos, -focus.up, out hit, 1000f))   //, 1 << LayerMask.NameToLayer("Road")))
            {
                if (hit.transform.GetComponent <LevelBlockInfo>()) // && hit.transform.GetComponent<LevelBlockInfo>() != curLBI)
                {
                    curLBI = hit.transform.GetComponent <LevelBlockInfo>();
                }
                else if (hit.transform.parent != null && hit.transform.parent.transform.GetComponent <LevelBlockInfo>())// && hit.transform.parent.transform.GetComponent<LevelBlockInfo>() != curLBI)
                {
                    curLBI = hit.transform.parent.transform.GetComponent <LevelBlockInfo>();
                }
                else if (hit.transform.parent != null && hit.transform.parent.transform.parent != null && hit.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>())// && hit.transform.parent.transform.parent.transform.GetComponent<LevelBlockInfo>() != curLBI)
                {
                    curLBI = hit.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>();
                }
                else if (hit.transform.parent != null && hit.transform.parent.transform.parent != null && hit.transform.parent.transform.parent.transform.parent != null && hit.transform.parent.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>())// && hit.transform.parent.transform.parent.transform.parent.transform.GetComponent<LevelBlockInfo>() != curLBI)
                {
                    curLBI = hit.transform.parent.transform.parent.transform.parent.transform.GetComponent <LevelBlockInfo>();
                }
            }

            switch (curState)
            {
            case CameraState.Vehicle:
                if (curLBI)
                {
                    if (b_snapSpawnCam)
                    {
                        Snap();
                    }

                    if (curOffset == Vector3.zero)
                    {
                        curOffset = curLBI.VehicleCameraOffset;
                        //transform.eulerAngles = curLBI.VehicleCameraEulerAngles + new Vector3(0, curLBI.transform.eulerAngles.y, 0);
                    }
                    else
                    {
                        //Vector3 modifiedFocus = focus.position + curLBI.transform.rotation * curLBI.RiderCameraOffset;
                        Vector3 modifiedFocus = focus.position + curLBI.VehicleCameraOffset;
                        Vector3 dir           = (modifiedFocus - transform.position).normalized * Mathf.Min((modifiedFocus - transform.position).magnitude, 1);

                        curOffset += dir * curLBI.VehicleCameraFollowSpeed * Time.deltaTime;

                        //Debug.Log(curLBI.VehicleCameraTrackSpeed + "from " + curLBI);
                        Quaternion rot = transform.rotation;
                        rot.eulerAngles    = curLBI.VehicleCameraEulerAngles;  // + new Vector3(0, curLBI.transform.eulerAngles.y, 0);
                        transform.rotation = Quaternion.RotateTowards(transform.rotation, rot, curLBI.VehicleCameraTrackSpeed * Time.deltaTime);
                    }
                }

                break;

            case CameraState.Rider:
                if (curLBI)
                {
                    if (curOffset == Vector3.zero)
                    {
                        curOffset = curLBI.RiderCameraOffset;
                        //transform.eulerAngles = curLBI.RiderCameraEulerAngles + new Vector3(0, curLBI.transform.eulerAngles.y, 0);
                    }
                    else
                    {
                        //Vector3 modifiedFocus = focus.position + curLBI.transform.rotation * curLBI.RiderCameraOffset;
                        Vector3 modifiedFocus = focus.position + curLBI.RiderCameraOffset;

                        Vector3 dir = (modifiedFocus - transform.position).normalized * Mathf.Min((modifiedFocus - transform.position).magnitude, 1);

                        curOffset += dir * curLBI.RiderCameraFollowSpeed * Time.deltaTime;


                        Quaternion rot = transform.rotation;
                        rot.eulerAngles    = curLBI.RiderCameraEulerAngles; // + new Vector3(0, curLBI.transform.eulerAngles.y, 0);
                        transform.rotation = Quaternion.RotateTowards(transform.rotation, rot, curLBI.RiderCameraTrackSpeed * Time.deltaTime);
                    }
                }

                break;

            case CameraState.Dead:
                if (curLBI)
                {
                    deathZoom += Time.deltaTime;
                    deathZoom  = Mathf.Min(deathZoom, deathZoomMin);

                    Vector3 modifiedFocus = focus.position + Vector3.up * deathZoom;

                    Vector3 dir = (modifiedFocus - transform.position).normalized * Mathf.Min((modifiedFocus - transform.position).magnitude, 1);

                    curOffset += dir * deathZoomFollow * Time.deltaTime;


                    Quaternion rot = transform.rotation;
                    rot.eulerAngles    = new Vector3(90, curLBI.transform.eulerAngles.y, 0);
                    transform.rotation = Quaternion.RotateTowards(transform.rotation, rot, deathZoomTrack * Time.deltaTime);
                }
                break;

            case CameraState.Win:

                transform.LookAt(focus.position + focus.transform.up * 1.1f);
                break;

            case CameraState.Spawning:
                transform.LookAt(focus);
                break;

            default:

                break;
            }

            if (curState != CameraState.Spawning && curState != CameraState.Win)
            {
                transform.position = focus.position + curOffset;
            }
        }
    }
 public void ResetFocus()
 {
     focus     = null;
     curOffset = Vector3.zero;
     curLBI    = null;
 }