PepApi.Core/PepLib.Core/Models/Program.cs

using PepLib.Codes;
using PepLib.Enums;
using PepLib.Geometry;
using PepLib.Interfaces;
using PepLib.IO;
using PepLib.Utilities;

namespace PepLib.Models
{
    public class Program : List<ICode>, IMovable
    {
        private ProgrammingMode mode;

        public Program(ProgrammingMode mode = ProgrammingMode.Absolute)
        {
            Mode = mode;
        }

        public ProgrammingMode Mode
        {
            get => mode;
            set
            {
                if (value == ProgrammingMode.Absolute)
                    SetProgrammingModeAbs();
                else
                    SetProgrammingModeInc();
            }
        }

        public double Rotation { get; protected set; }

        private void SetProgrammingModeInc()
        {
            if (mode == ProgrammingMode.Incremental)
                return;

            var pos = new Vector(0, 0);

            foreach (var code in this)
            {
                if (code is Motion motion)
                {
                    var pos2 = motion.EndPoint;
                    motion.Offset(-pos.X, -pos.Y);
                    pos = pos2;
                }
            }

            mode = ProgrammingMode.Incremental;
        }

        private void SetProgrammingModeAbs()
        {
            if (mode == ProgrammingMode.Absolute)
                return;

            var pos = new Vector(0, 0);

            foreach (var code in this)
            {
                if (code is Motion motion)
                {
                    motion.Offset(pos);
                    pos = motion.EndPoint;
                }
            }

            mode = ProgrammingMode.Absolute;
        }

        public virtual void Rotate(double angle) =>
            ApplyTransform(code => (code as IMovable)?.Rotate(angle), angle);

        public virtual void Rotate(double angle, Vector origin) =>
            ApplyTransform(code => (code as IMovable)?.Rotate(angle, origin), angle);

        public void Offset(double x, double y) =>
            ApplyTransform(code => (code as IMovable)?.Offset(x, y));

        public void Offset(Vector voffset) =>
            ApplyTransform(code => (code as IMovable)?.Offset(voffset));

        private void ApplyTransform(Action<ICode> transform, double? rotationAngle = null)
        {
            var previousMode = Mode;
            SetProgrammingModeAbs();

            foreach (var code in this)
            {
                if (rotationAngle.HasValue && code is SubProgramCall subpgm)
                {
                    subpgm.Loop?.Rotate(rotationAngle.Value);
                }

                transform(code);
            }

            if (previousMode == ProgrammingMode.Incremental)
                SetProgrammingModeInc();

            if (rotationAngle.HasValue)
                Rotation = MathHelper.NormalizeAngleRad(Rotation + rotationAngle.Value);
        }

        public Box GetBoundingBox()
        {
            var origin = new Vector(0, 0);
            return GetBoundingBox(ref origin);
        }

        private Box GetBoundingBox(ref Vector pos)
        {
            var bounds = new BoundsTracker();

            foreach (var code in this)
            {
                switch (code)
                {
                    case LinearMove line:
                        pos = ProcessLinearMotion(line.EndPoint, pos, bounds);
                        break;

                    case RapidMove rapid:
                        pos = ProcessLinearMotion(rapid.EndPoint, pos, bounds);
                        break;

                    case CircularMove arc:
                        pos = ProcessCircularMotion(arc, pos, bounds);
                        break;

                    case SubProgramCall subpgm:
                        var box = subpgm.Loop.GetBoundingBox(ref pos);
                        bounds.ExpandTo(box);
                        break;
                }
            }

            return bounds.ToBox();
        }

        private Vector ProcessLinearMotion(Vector endPoint, Vector pos, BoundsTracker bounds)
        {
            var pt = Mode == ProgrammingMode.Absolute ? endPoint : endPoint + pos;
            bounds.Include(pt);
            return pt;
        }

        private Vector ProcessCircularMotion(CircularMove arc, Vector pos, BoundsTracker bounds)
        {
            var radius = arc.CenterPoint.DistanceTo(arc.EndPoint);

            var (endpt, centerpt) = Mode == ProgrammingMode.Incremental
                ? (arc.EndPoint + pos, arc.CenterPoint + pos)
                : (arc.EndPoint, arc.CenterPoint);

            // Start with endpoint bounds
            var minX = Math.Min(pos.X, endpt.X);
            var maxX = Math.Max(pos.X, endpt.X);
            var minY = Math.Min(pos.Y, endpt.Y);
            var maxY = Math.Max(pos.Y, endpt.Y);

            // Check if arc crosses cardinal directions
            var startAngle = MathHelper.NormalizeAngleRad(pos.AngleFrom(centerpt));
            var endAngle = MathHelper.NormalizeAngleRad(endpt.AngleFrom(centerpt));

            // Switch angles for clockwise arcs
            if (arc.Rotation == RotationType.CW)
                Generic.Swap(ref startAngle, ref endAngle);

            // Expand bounds if arc crosses cardinal points
            if (MathHelper.IsAngleBetween(MathHelper.HalfPI, startAngle, endAngle))
                maxY = centerpt.Y + radius;

            if (MathHelper.IsAngleBetween(Math.PI, startAngle, endAngle))
                minX = centerpt.X - radius;

            if (MathHelper.IsAngleBetween(Math.PI * 1.5, startAngle, endAngle))
                minY = centerpt.Y - radius;

            if (MathHelper.IsAngleBetween(MathHelper.TwoPI, startAngle, endAngle))
                maxX = centerpt.X + radius;

            bounds.Expand(minX, minY, maxX, maxY);
            return endpt;
        }

        public static Program Load(Stream stream)
        {
            var reader = new ProgramReader();
            reader.Read(stream);
            return reader.Program;
        }

        private class BoundsTracker
        {
            private double minX, minY, maxX, maxY;

            public void Include(Vector pt)
            {
                if (pt.X < minX) minX = pt.X;
                if (pt.X > maxX) maxX = pt.X;
                if (pt.Y < minY) minY = pt.Y;
                if (pt.Y > maxY) maxY = pt.Y;
            }

            public void Expand(double left, double bottom, double right, double top)
            {
                if (left < minX) minX = left;
                if (right > maxX) maxX = right;
                if (bottom < minY) minY = bottom;
                if (top > maxY) maxY = top;
            }

            public void ExpandTo(Box box)
            {
                if (box.Left < minX) minX = box.Left;
                if (box.Right > maxX) maxX = box.Right;
                if (box.Bottom < minY) minY = box.Bottom;
                if (box.Top > maxY) maxY = box.Top;
            }

            public Box ToBox() => new(minX, minY, maxX - minX, maxY - minY);
        }
    }
}