using System;
using System.Drawing;
using System.Collections.Generic;
using System.Linq;

namespace AGVControl.Models
{
    public enum Direction
    {
        Forward = 0,
        Backward = 1,
        Stop = 2
    }

    public struct movehistorydata
    {
        public UInt16 rfid { get; set; }
        public Direction direction { get; set; }

        public override string ToString()
        {
            return $"RFID:{rfid},DIR:{direction}";
        }
    }

    public class AGV
    {
        public Point CurrentPosition { get; set; }
        public uint CurrentRFID { get; set; }
        public float BatteryLevel { get; set; } = 0f;
        /// <summary>
        /// AGV에서 방향값이 수산됩니다.
        /// </summary>
        public Direction CurrentDirection { get; set; }

        /// <summary>
        /// 현재위치가 수산되면 목적지까지의 방향값이 계산됩니다.
        /// </summary>
        public Direction TargetDirection { get; set; } = Direction.Stop;
        public bool IsMoving { get; set; }
        public List<Point> CurrentPath { get; set; } = new List<Point>();
        public List<Point> PlannedPath { get; set; }
        public List<string> PathRFIDs { get; set; }
        public Point TargetPosition { get; set; }
        public uint TargetRFID { get; set; }
        public float? BodyAngle { get; set; } = null;
        public float MotorAngle { get; set; } = 0f;
        // 이동 경로 기록을 위한 새로운 속성들
        public List<movehistorydata> MovementHistory { get; } = new List<movehistorydata>();
        public List<Point> PositionHistory { get; } = new List<Point>();
        public const int HISTORY_SIZE = 4; // 최근 4개 위치 기록

        public AGV()
        {
            CurrentPath = new List<Point>();
            PlannedPath = new List<Point>();
            PathRFIDs = new List<string>();
            CurrentDirection = Direction.Forward;
            TargetPosition = Point.Empty;
            TargetRFID = 0;
            TargetDirection = Direction.Forward;
            BodyAngle = null;
        }

        public void Move()
        {
            if (CurrentPath.Count > 0)
            {
                CurrentPosition = CurrentPath[0];
                CurrentPath.RemoveAt(0);
            }
        }

        // 이동 경로에 새로운 RFID 추가
        public void AddToMovementHistory(UInt16 rfidValue, Point position, Direction direction)
        {
            // 중복 RFID가 연속으로 들어오는 경우 무시
            if (MovementHistory.Count > 0 && MovementHistory.Last().rfid == rfidValue)
                return;

            MovementHistory.Add(new movehistorydata { rfid = rfidValue, direction = direction }) ;
            PositionHistory.Add(position);

            // 기록 크기 제한
            if (MovementHistory.Count > HISTORY_SIZE)
            {
                MovementHistory.RemoveAt(0);
                PositionHistory.RemoveAt(0);
            }

            //최초방향과 마지막 방향이 일치하지 않으면 그 이전의 데이터는 삭제한다.
            if(MovementHistory.Count > 2 && MovementHistory.First().direction != MovementHistory.Last().direction)
            {
                var lastTwo = MovementHistory.Skip(MovementHistory.Count - 2).Take(2).ToArray(); // [9, 10]
                MovementHistory.Clear();
                MovementHistory.AddRange(lastTwo);
            }
        }

        // 연결 정보 기반 실제 이동 방향 계산
        public Direction? CalculateActualDirectionByConnection(uint currentRFID, uint previousRFID, List<RFIDConnection> connections)
        {
            if (connections == null || connections.Count == 0)
                return null;

            // 이전 RFID에서 현재 RFID로의 연결 확인
            var connection = connections.FirstOrDefault(c =>
                (c.StartRFID == previousRFID && c.EndRFID == currentRFID) ||
                (c.IsBidirectional && c.StartRFID == currentRFID && c.EndRFID == previousRFID));

            if (connection == null)
                return null; // 연결되지 않은 경로

            // 연결 방향에 따라 실제 이동 방향 결정
            if (connection.StartRFID == previousRFID && connection.EndRFID == currentRFID)
            {
                return Direction.Forward; // Start -> End 방향으로 이동
            }
            else
            {
                return Direction.Backward; // End -> Start 방향으로 이동
            }
        }

        // 연결 정보 기반 방향 불일치 검증 및 정정
        public bool ValidateAndCorrectDirectionByConnection(Direction expectedDirection, List<RFIDConnection> connections)
        {
            if (MovementHistory.Count < 2 || connections == null)
                return true; // 검증 불가능한 경우

            // 최근 두 RFID 값 가져오기
            var recentRFIDs = MovementHistory.Skip( MovementHistory.Count - 2).Take(2).ToList();
            if (recentRFIDs.Count < 2)
                return true;

            var previousRFID = recentRFIDs[0];
            var currentRFID = recentRFIDs[1];

            var actualDirection = CalculateActualDirectionByConnection(currentRFID.rfid, previousRFID.rfid, connections);
            if (!actualDirection.HasValue)
                return true; // 연결 정보로 방향 판단 불가

            // 방향이 일치하지 않는 경우
            if (actualDirection.Value != expectedDirection)
            {
                // AGV 모터 방향을 실제 이동 방향으로 정정
                CurrentDirection = actualDirection.Value;
                TargetDirection = actualDirection.Value;

                // 몸체 방향도 180도 회전 (결정된 경우에만)
                if (BodyAngle.HasValue)
                {
                    BodyAngle = (BodyAngle.Value + 180) % 360;
                }

                return false; // 정정됨을 알림
            }

            return true; // 방향 일치
        }

        // RFID 순서 기반 실제 이동 방향 계산 (기존 메서드 - 호환성 유지)
        public Direction? CalculateActualDirectionByRFID()
        {
            if (MovementHistory.Count < 2)
                return null;

            // 최근 두 RFID 값으로부터 실제 이동 방향 계산
            var recentRFIDs = MovementHistory.Skip(Math.Max(0, MovementHistory.Count - 2)).Take(2).ToList();
            if (recentRFIDs.Count < 2)
                return null;

            var prevRFID = recentRFIDs[0];
            var currentRFID = recentRFIDs[1];

            // RFID 값의 증가/감소로 방향 판단
            if (currentRFID.rfid > prevRFID.rfid)
            {
                return Direction.Forward; // RFID 값이 증가하면 전진
            }
            else if (currentRFID.rfid < prevRFID.rfid)
            {
                return Direction.Backward; // RFID 값이 감소하면 후진
            }
            else
            {
                return null; // 같은 RFID 값이면 방향 판단 불가
            }
        }

        // 경로상 RFID 순서 기반 예상 방향 계산
        public Direction? CalculateExpectedDirectionByRFID()
        {
            if (CurrentPath == null || CurrentPath.Count < 2)
                return null;

            // 현재 위치의 RFID 찾기
            var currentRFID = CurrentRFID;
            if (currentRFID == 0)
                return null;

            // 경로상 다음 RFID 찾기
            int currentIdx = CurrentPath.FindIndex(p => p == CurrentPosition);
            if (currentIdx < 0 || currentIdx >= CurrentPath.Count - 1)
                return null;

            // 다음 위치의 RFID 찾기 (MapControl에서 RFID 정보 필요)
            // 이 부분은 MapControl에서 처리하도록 수정 필요
            return null;
        }

        // 실제 이동 방향 계산 (기존 메서드 - 호환성 유지)
        public Direction? CalculateActualDirection()
        {
            if (MovementHistory.Count < 2)
                return null;

            // 최근 두 위치로부터 실제 이동 방향 계산
            var recentPositions = PositionHistory.Skip(Math.Max(0, PositionHistory.Count - 2)).Take(2).ToList();
            if (recentPositions.Count < 2)
                return null;

            var prevPos = recentPositions[0];
            var currentPos = recentPositions[1];

            // X축 이동이 더 큰 경우
            if (Math.Abs(currentPos.X - prevPos.X) > Math.Abs(currentPos.Y - prevPos.Y))
            {
                return currentPos.X > prevPos.X ? Direction.Forward : Direction.Backward;
            }
            // Y축 이동이 더 큰 경우
            else
            {
                return currentPos.Y > prevPos.Y ? Direction.Forward : Direction.Backward;
            }
        }

        // 경로상 예상 방향 계산
        public Direction? CalculateExpectedDirection()
        {
            if (CurrentPath == null || CurrentPath.Count < 2)
                return null;

            int currentIdx = CurrentPath.FindIndex(p => p == CurrentPosition);
            if (currentIdx < 0 || currentIdx >= CurrentPath.Count - 1)
                return null;

            var currentPos = CurrentPath[currentIdx];
            var nextPos = CurrentPath[currentIdx + 1];

            // X축 이동이 더 큰 경우
            if (Math.Abs(nextPos.X - currentPos.X) > Math.Abs(nextPos.Y - currentPos.Y))
            {
                return nextPos.X > currentPos.X ? Direction.Forward : Direction.Backward;
            }
            // Y축 이동이 더 큰 경우
            else
            {
                return nextPos.Y > currentPos.Y ? Direction.Forward : Direction.Backward;
            }
        }
    }

    public class PathNode
    {
        public Point Location { get; set; }
        public string RFID { get; set; }
        public double G { get; set; }  // 시작점에서 현재 노드까지의 비용
        public double H { get; set; }  // 현재 노드에서 목표점까지의 예상 비용
        public double F => G + H;      // 총 비용
        public PathNode Parent { get; set; }

        public PathNode(Point location, string rfid)
        {
            Location = location;
            RFID = rfid;
            G = 0;
            H = 0;
            Parent = null;
        }
    }
}