using DG.Tweening;
using System;
using System.Collections.Generic;
using UnityEngine;

/// <summary>
/// 机器人对象
/// </summary>
public class RobotObject : MonoBehaviour
{
    public RectTransform rect_transform;
    /*
     bmp_x = origin.x
     */
    /// <summary>
    /// 机器人信息
    /// </summary>
    public RobotClass robotClass = null;
    /// <summary>
    /// 机器人地图数据
    /// </summary>
    public RobotMap RobotMap = null;
    /// <summary>
    /// 机器人位置状态信息
    /// </summary>
    public Queue<RobotPos> RobotPosQueue = new Queue<RobotPos>();
    /// <summary>
    /// 机器人任务队列
    /// </summary>
    public RobotTask RobotTask = null;

    /// <summary>
    /// 当前执行的机器人位置信息
    /// 【map_x，map_y】
    /// </summary>
    private RobotPos current_robot_pos = null;

    //固定值
    public int origin_x;//=info.origin.x
    public int origin_y;//=info.origin.y
    public int pixel_width;//info.width
    public int pixel_height;//info.height
    public double resolution;//=info.resolution
    //public double bmp_x;//=origin.x/resolution
    //public double bmp_y;//=info.height-(origin.y/resolution)
    //public float bmp_h;//=info.height
    //public float bmp_w;//=info.width

    //实时值
    //public double map_x;//=current_pos.x
    //public double map_y;//=current_pos.y
    public double display_x;//=(current_pos.x-origin.x)/resolution
    public double display_y;//=(current_pos.y-origin.y)/resolution

    private bool get;
    // Start is called before the first frame update
    void Start()
    {
        rect_transform = GetComponent<RectTransform>();
        //robotClass = null;
        //RobotMap = null;
    }

    // Update is called once per frame
    void Update()
    {
        if (robotClass == null || RobotMap == null)
            return;
        if (!get && RobotMap.info != null)
        {
            get = true;
            origin_x = RobotMap.info.origin.x;
            origin_y = RobotMap.info.origin.y;
            pixel_width = RobotMap.info.width;
            pixel_height = RobotMap.info.height;
            resolution = RobotMap.info.resolution;
            Debug.Log($"origin_x：{origin_x}，origin_y：{origin_y}，pixel_width：{pixel_width}，pixel_height：{pixel_height}，resolution：{resolution}");
        }

        if (RobotPosQueue.Count > 0 && current_robot_pos == null)
        {
            try
            {
                current_robot_pos = RobotPosQueue.Peek();
                //Debug.Log(Newtonsoft.Json.JsonConvert.SerializeObject(current_robot_pos));

                /*display_x = (current_robot_pos.current_pose.x - origin_x) / resolution;
                display_y = (current_robot_pos.current_pose.x - origin_y) / resolution;

                display_x *= (rect_transform.sizeDelta.x / pixel_width);
                display_y *= (rect_transform.sizeDelta.y / pixel_height);*/


                // display_x = - bmp_x + map_x/resolution
                display_x = -origin_x + current_robot_pos.current_pose.x / resolution;
                // display _y= -（bmp_h - bmp_y + map_y/resolution）
                display_y = -(pixel_height - origin_y + current_robot_pos.current_pose.y / resolution);

                var _display_y = (display_x / pixel_width) * (296) + 50;
                //var _display_x = MathF.Abs(((float)display_y / (float)pixel_height) * 750) - 790;
                var _display_x = MathF.Abs(790 - MathF.Abs(((float)display_y / (float)pixel_height) * 785));

                //rect_transform.DOAnchorPos(new Vector2((float)display_x, (float)display_y), 0.5f).OnComplete(() =>
                rect_transform.DOAnchorPos(new Vector2((float)_display_x + 24, (float)_display_y + 24), 0.5f).OnComplete(() =>
                {
                    //Debug.Log($"robot位置轨迹：{new Vector2((float)display_x, (float)display_y)}");
                    Debug.Log($"robot位置轨迹：{new Vector2((float)_display_x + 24, (float)_display_y + 24)}");
                    RobotPosQueue.Dequeue();
                    current_robot_pos = null;
                });
            }
            catch (Exception e)
            {
                current_robot_pos = null;
                Debug.Log($"robot位置轨迹错误：{e.Message}，行次：{e.StackTrace}");
            }

        }
    }
}