using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using FlyADBase;

namespace FLY.Simulation.Flyad7
{
    //public interface ISimDrive 
    //{
    //    MOTORTYPE MotorType{get;set;}
    //    bool IsMaxLimit{get;}
    //    bool IsMinLimit { get; }
    //    bool IsOrg { get; }
    //    int MaxPos { get; }
    //    int MinPos { get; }
    //    double Mmpp { get; }
    //    UInt32 AccTime { get; set; }
    //    UInt32 DecTime { get; set; }
    //    double Velocity { get; set; }
    //    int Pos { get; set; }
    //    int Speed { get; }
    //    SimDrive_STATE State { get; }

    //    void QuickRunto(int pos);
    //    void Forw();
    //    void Backw();
    //    void Stop();
    //    void OnPoll(DateTime now);
    //}


    public enum SimDrive_STATE 
    {
        STOPPING,
        STOP_BY_LIMIT,
        RUNNING
    }

    public class SimDrive 
    {
        public MOTORTYPE MotorType { get; set; }
        /// <summary>
        /// 当前正向限位
        /// </summary>
        public bool IsMaxLimit
        {
            get
            {
                if (m_currpos >= MaxPos)
                {
                    return true;
                }
                return false;
            }

        }
        /// <summary>
        /// 当前反向限位
        /// </summary>
        public bool IsMinLimit
        {
            get
            {
                if (m_currpos <= MinPos)
                {
                    return true;
                }
                return false;
            }
        }
        /// <summary>
        /// 归0信号灯亮
        /// </summary>
        public bool IsOrg
        {
            get
            {
                if (m_currpos <= 0)
                    return true;
                else
                    return false;
            }
        }

        
        const double mmpp = 0.1133;//mm/脉冲
        public double Mmpp 
        { 
            get { return mmpp; } 
        }

        const double maxmm = 1970;//机架最大位置
        const double minmm = -50;//机架最小位置

        
        /// <summary>
        /// 最大脉冲
        /// </summary>
        public int MaxPos
        {
            get{
                return (int)(maxmm / mmpp);
            }
        }
        /// <summary>
        /// 最小脉冲
        /// </summary>
        public int MinPos
        {
            get
            {
                return (int)(minmm / mmpp);
            }
        }


        /// <summary>
        /// 目标速度 12m/min , 单位 脉冲/s
        /// </summary>
        const double m_targetspeed = (12 * 1000) / mmpp / 60;

        /// <summary>
        /// 加速度，启动的加速度, 单位 脉冲/s²  0->12m/min 只需要0.5s
        /// </summary>
        double m_acc;
        UInt32 acctime;
        /// <summary>
        /// 加速时间 ms
        /// </summary>
        public UInt32 AccTime 
        {
            get { return acctime; }
            set {
                if (acctime != value) 
                {
                    acctime = value;
                    m_acc = m_targetspeed / (acctime / 1000);
                }
            }
        }

        /// <summary>
        /// 减速度 刹车时的加速度 单位 脉冲/s²
        /// </summary>
        double m_dec;

        UInt32 dectime;
        /// <summary> 
        /// 减速时间 ms
        /// </summary>
        public UInt32 DecTime 
        {
            get { return dectime; }
            set {
                if (dectime != value) 
                {
                    dectime = value;
                    m_dec = m_targetspeed / (dectime / 1000);
                }
            }
        }



        double velocity;
        /// <summary>
        /// 速度 脉冲/s
        /// </summary>
        public double Velocity 
        {
            get { return velocity; }
            set {
                if (velocity != value) 
                {
                    velocity = value;
                }
            }
        }



        /// <summary>
        ///  最大速度 25m/min , 单位 脉冲/s
        /// </summary>
        const double m_maxspeed = (int)((25 * 1000) / mmpp / 60);

        /// <summary>
        /// 最大加速度，启动的加速度, 单位 脉冲/s²  0->25m/min 只需要0.1s
        /// </summary>
        const double m_maxacc = m_maxspeed / 0.1;



        
        /// <summary>
        /// 当前脉冲
        /// </summary>
        double m_currpos = 990;
        /// <summary>
        /// 归0信号偏移
        /// </summary>
        int m_offset = 0;
        public int Pos
        {
            get
            {
                return (int)(m_currpos) + m_offset;
            }
            set
            {
                m_offset = value - (int)(m_currpos);
            }
        }
        /// <summary>
        /// 脉冲/s 
        /// </summary>
        double m_speed = 0;
        public int Speed 
        {
            get {
                return (int)m_speed;
            }
        }

        public enum STATE
        {
            STOPPING,
            STOP_BY_LIMIT,
            RUNNING
        }
        public STATE state = STATE.STOPPING;

        public enum ORDER
        {
            IDLE,
            STOP,
            FORW,
            BACKW,
            /// <summary>
            /// 超快速,无视加速时间
            /// </summary>
            QUICK_RUNTO,
            /// <summary>
            /// 运行到
            /// </summary>
            RUNTO,
        }
        public ORDER order = ORDER.IDLE;
        ORDER runto_order_sub;
        public SimDrive() 
        {
            MotorType = MOTORTYPE.VF0;
            Velocity = (12 * 1000) / mmpp / 60;//12m/min
            AccTime = 500;//0.5s
            DecTime = 500;//0.5s
        }


        int runto_target_pos;

        public void QuickRunto(int pos) 
        {
            order = ORDER.QUICK_RUNTO;
            state = STATE.RUNNING;
            runto_target_pos = pos;
        }
        public void Runto(int pos)
        {
            order = ORDER.RUNTO;
            state = STATE.RUNNING;
            runto_target_pos = pos;

            if (runto_target_pos > Pos)
            {
                runto_order_sub = ORDER.FORW;
            }
            else if (runto_target_pos < Pos)
            {
                runto_order_sub = ORDER.BACKW;
            }
            else
            {
                runto_order_sub = ORDER.STOP;
            }
        }
        public void Forw()
        {
            order = ORDER.FORW;
            state = STATE.RUNNING;
        }
        public void Backw() 
        {
            order = ORDER.BACKW;
            state = STATE.RUNNING;
        }
        public void Stop() 
        {
            order = ORDER.STOP;
            state = STATE.RUNNING;
        }
        void OnPoll_Order(TimeSpan ts)
        {
            switch (order)
            {
                case ORDER.QUICK_RUNTO:
                    {
                        //不允许走过了
                        double target_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds;
                        double acc = (target_speed - m_speed) / ts.TotalSeconds;
                        if (Math.Abs(acc) > m_maxacc) 
                        {
                            if (acc > 0)
                                acc = m_maxacc;
                            else
                                acc = -m_maxacc;
                            target_speed = acc * ts.TotalSeconds + m_speed;
                        }
                        if (Math.Abs(target_speed) > m_maxspeed)
                        {
                            if (target_speed > 0)
                                target_speed = m_maxspeed;
                            else
                                target_speed = -m_maxspeed;
                        }
                        double currpos = m_currpos + target_speed * ts.TotalSeconds;

                        if (runto_target_pos > m_currpos)
                        {
                            if (currpos > runto_target_pos)//走过了
                            {
                                target_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds;
                            }
                        }
                        else if (runto_target_pos < m_currpos)
                        {
                            if (currpos < runto_target_pos)//走过了
                            {
                                target_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds;
                            }
                        }
                        m_speed = target_speed;
                        
                    }break;
                case ORDER.RUNTO:
                    {
                        switch (runto_order_sub) 
                        {
                            case ORDER.FORW:
                                {
                                    m_speed += m_acc * ts.TotalSeconds;
                                    if (m_speed > Velocity)
                                        m_speed = Velocity;

                                    double currpos = m_currpos + m_speed * ts.TotalSeconds;
                                    if (currpos >= runto_target_pos)
                                    {
                                        runto_order_sub = ORDER.STOP;
                                        if (MotorType == MOTORTYPE.SERVO)
                                            m_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds;
                                    }
                                } break;
                            case ORDER.BACKW:
                                {
                                    m_speed -= m_acc * ts.TotalSeconds;
                                    if (m_speed < -Velocity)
                                        m_speed = -Velocity;

                                    double currpos = m_currpos + m_speed * ts.TotalSeconds;
                                    if (currpos <= runto_target_pos)
                                    {
                                        runto_order_sub = ORDER.STOP;
                                        if (MotorType == MOTORTYPE.SERVO)
                                            m_speed = (runto_target_pos - m_currpos) / ts.TotalSeconds;
                                    }
                                } break;
                            case ORDER.STOP:

                                if (MotorType == MOTORTYPE.SERVO)
                                {
                                    m_speed = 0;
                                }
                                else
                                {
                                    if (m_speed > 0)
                                    {
                                        m_speed -= m_dec * ts.TotalSeconds;
                                        if (m_speed < 0)
                                            m_speed = 0;
                                    }
                                    else
                                    {
                                        m_speed += m_dec * ts.TotalSeconds;
                                        if (m_speed > 0)
                                            m_speed = 0;
                                    }
                                }
                                break;
                        }
                    }break;

                case ORDER.STOP:

                    if (MotorType == MOTORTYPE.SERVO)
                    {
                        m_speed = 0;
                    }
                    else
                    {
                        if (m_speed > 0)
                        {
                            m_speed -= m_dec * ts.TotalSeconds;
                            if (m_speed < 0)
                                m_speed = 0;
                        }
                        else
                        {
                            m_speed += m_dec * ts.TotalSeconds;
                            if (m_speed > 0)
                                m_speed = 0;
                        }
                    }
                    break;
                case ORDER.FORW:
                    m_speed += m_acc * ts.TotalSeconds;
                    if (m_speed > Velocity)
                        m_speed = Velocity;
                    break;
                case ORDER.BACKW:
                    m_speed -= m_acc * ts.TotalSeconds;
                    if (m_speed < -Velocity)
                        m_speed = -Velocity;
                    break;
            }

            if(m_speed!=0)
            {
                state = STATE.RUNNING;
                double currpos = m_currpos + m_speed * ts.TotalSeconds;
                
                if ((m_speed > 0) && (currpos >= MaxPos))
                {
                    //限位了
                    m_currpos = MaxPos;
                    m_speed = 0;
                    state = STATE.STOP_BY_LIMIT;
                    order = ORDER.IDLE;
                }
                else if ((m_speed < 0) && (currpos <= MinPos))
                {
                    m_speed = 0;
                    m_currpos = MinPos;
                    state = STATE.STOP_BY_LIMIT;
                    order = ORDER.IDLE;
                }
                else
                {
                    m_currpos = currpos;
                }
            }
            else
            {
                state = STATE.STOPPING;
                order = ORDER.IDLE;
            }
        }

        DateTime dtLast = DateTime.MinValue;
        public void OnPoll(DateTime now)
        {
            DateTime dt = now;
            if (dtLast != DateTime.MinValue)
            {
                TimeSpan ts = dt - dtLast;
                OnPoll_Order(ts);
            }
            dtLast = dt;
        }

    }
    public class OrderMan 
    {
        public delegate void FinishEventHandler(bool isNormalStop);
        public event FinishEventHandler FinishEvent;

        public Order_Runto order_runto = new Order_Runto();
        public Order_Stop order_stop = new Order_Stop();
        public Order_Org order_org = new Order_Org();
        public Order_Forward order_forw = new Order_Forward();
        public Order_Backward order_backw = new Order_Backward();
        public Order_Sync order_sync = new Order_Sync();
        public Order order;
        public void Init(FLYAD7 flyad, SimDrive simDrive) 
        {
            order_runto.Init(flyad, simDrive);
            order_stop.Init(flyad, simDrive);
            order_org.Init(flyad, simDrive);
            order_forw.Init(flyad, simDrive);
            order_backw.Init(flyad, simDrive);
            order_sync.Init(flyad, simDrive);
        }
        public void Stop()
        {
            order = order_stop;
            order.Start();
        }
        public void Org()
        {
            order = order_org;
            order.Start();
        }
        public void Forw()
        {
            order = order_forw;
            order.Start();
        }
        public void Backw()
        {
            order = order_backw;
            order.Start();
        }
        public void Runto(int dest)
        {
            order_runto.destpos = dest;
            order = order_runto;
            order.Start();
        }
        public void Sync() 
        {
            order = order_sync;
            order.Start();
        }
        public void OnPoll() 
        {
            if (order != null) 
            {
                Order.STATE state = order.OnPoll();
                if (state != Order.STATE.RUNNING) 
                {
                    if (FinishEvent != null)
                        FinishEvent((state == Order.STATE.STOP));
                    order = null;
                }
            }
        }
    }
    public abstract class Order 
    {
        public enum STATE
        {
            INIT,
            RUNNING,
            STOP,
            STOP_BY_LIMIT
        }
        public STATE State = STATE.INIT;
        public virtual void Start()
        {
            State = STATE.RUNNING;
        }
        public virtual STATE OnPoll()
        {
            State = STATE.STOP;
            return State;
        }

        public virtual void Init(FLYAD7 flyad, SimDrive simDrive) 
        {
            mflyad7 = flyad;
            mSimDrive = simDrive;
        }
        protected FLYAD7 mflyad7;
        protected SimDrive mSimDrive;
    }
    public class Order_Runto : Order 
    {
        public int destpos=0;
        public int Velocity;
        public override void Start() 
        {
            mflyad7.mPosGrid.Start();
            base.Start();
            mSimDrive.Velocity = Velocity;
            mSimDrive.Runto(destpos);
        }
        public override STATE OnPoll() 
        {
            switch (mSimDrive.state)
            {
                case SimDrive.STATE.STOPPING:
                    {
                        mflyad7.mPosGrid.End();
                        State = STATE.STOP;
                    } break;
                case SimDrive.STATE.STOP_BY_LIMIT:
                    {
                        State = STATE.STOP_BY_LIMIT;
                    } break;
            }
            return State;
        }
    }
    public class Order_Forward : Order 
    {
        public int Velocity;
        public override void Start()
        {
            State = STATE.RUNNING;
            mSimDrive.Velocity = Velocity;
            mSimDrive.Forw();
        }
        public override STATE OnPoll()
        {
            switch (mSimDrive.state)
            { 
                case SimDrive.STATE.STOPPING:
                    State = STATE.STOP;
                    break;
                case SimDrive.STATE.STOP_BY_LIMIT:
                    State = STATE.STOP_BY_LIMIT;
                    break;
            }
            
            return State;
        }
    }
    public class Order_Backward : Order_Forward 
    {

        public override void Start()
        {
            State = STATE.RUNNING;
            mSimDrive.Velocity = Velocity;
            mSimDrive.Backw();
        }
    }
    public class Order_Org : Order
    {
        private int order_adv_state = 0;
        public int Velocity1;
        public int Velocity2;
        public override void Start()
        {
            State = STATE.RUNNING;
            order_adv_state = 0;
        }
        public override STATE OnPoll()
        {
            switch (order_adv_state)
            {
                case 0:
                    if (!mSimDrive.IsOrg)
                    {
                        mSimDrive.Velocity = Velocity1;
                        mSimDrive.Backw();
                    }
                        order_adv_state = 1;
                    break;
                case 1:
                    if (mSimDrive.IsOrg)
                    {
                        mSimDrive.Stop(); 
                        order_adv_state = 2;
                    } break;
                case 2:
                    if (mSimDrive.state != SimDrive.STATE.RUNNING)
                    {
                        mSimDrive.Velocity = Velocity2;
                        mSimDrive.Forw();
                        order_adv_state = 3;
                    } break;
                case 3:
                    if (!mSimDrive.IsOrg)
                    {
                        mSimDrive.Stop();
                        order_adv_state = 4;
                    } break;
                case 4:
                    if (mSimDrive.state != SimDrive.STATE.RUNNING)
                    {
                        mSimDrive.Pos = 0;
                        //归0完成
                        State = STATE.STOP;
                    } break;
            }
            return State;
        }
    }
    public class Order_Stop : Order_Forward
    {
        public override void Start()
        {
            State = STATE.RUNNING;
            mSimDrive.Stop();
        }
    }
    public class Order_Sync : Order
    {
        public List<ASyncOrder> mSyncOrderList;

        public override STATE OnPoll()
        {
            if (mSyncOrderList.Count() > 0)
            {
                ASyncOrder  o = mSyncOrderList.First();
                if (o.State == STATE.INIT)
                {

                    o.Init(mflyad7, mSimDrive);
                    o.Start();

                    if (CurrSyncOrderChangedEvent != null)
                    {
                        CurrSyncOrderChangedEvent(this);
                    }
                }
                o.OnPoll();
                if (mflyad7.Position <= -500)
                { 
                
                }
                if (o.State != STATE.RUNNING) 
                {
                    //完成一个
                    mSyncOrderList.RemoveAt(0);

                }
            }
            return STATE.RUNNING;
        }
        public override string ToString()
        {
            if (mSyncOrderList.Count > 0)
                return "Sync (" + mSyncOrderList.Count.ToString()+") " + mSyncOrderList.First().ToString();
            else
                return "Sync (0)";
        }
        public event Action<Order_Sync> CurrSyncOrderChangedEvent;
    }

    public abstract class ASyncOrder : Order
    {
        public Int32 marker;
        public override void Start()
        {
            base.Start();
            mflyad7.Marker = marker;
        }

    }
    public class SyncOrder_RunAtLC : ASyncOrder
    {
        public int pos2_begin;
        public int pos2_end;
        public int pos1lc_end;
        public bool hasDataGrid;


        private int pos1lc_begin;
        private double p;
        public override void Start()
        {

            base.Start();
            pos1lc_begin = mflyad7.Position + mflyad7.Pos1LCShift;
            if (hasDataGrid)
                mflyad7.mPosGrid.Start();
            p = ((double)(pos1lc_end - pos1lc_begin)) / (pos2_end - pos2_begin);
        }
        public override STATE OnPoll()
        {
            if (mflyad7.Position2 < pos2_begin)
            { 
                
            }
            else if(mflyad7.Position2 <= pos2_end)
            {
                int pos = (int)((mflyad7.Position2 - pos2_begin) * p)+ (pos1lc_begin - mflyad7.Pos1LCShift);

                mSimDrive.QuickRunto(pos);
            }
            else 
            {
                State = STATE.STOP;
                if (hasDataGrid)
                    mflyad7.mPosGrid.End();
            }
            return State;
        }
        public override string ToString()
        {
            return "RunAtLC (" + pos2_begin.ToString() + "," + pos2_end.ToString() + ") pos1lc=" + pos1lc_end.ToString() + "|" + marker.ToString();
        }
    }
    public class SyncOrder_Origin : ASyncOrder
    {
        private int order_adv_state = 0;
        public override void Start()
        {
            base.Start();
            order_adv_state = 0;
        }
        public override STATE OnPoll()
        {
            switch (order_adv_state)
            {
                case 0:
                    if (!mSimDrive.IsOrg)
                        mSimDrive.Backw();
                    order_adv_state = 1;
                    break;
                case 1:
                    if (mSimDrive.IsOrg)
                    {
                        mSimDrive.Stop();
                        order_adv_state = 2;
                    } break;
                case 2:
                    if (mSimDrive.state != SimDrive.STATE.RUNNING)
                    {
                        mSimDrive.Forw();
                        order_adv_state = 3;
                    } break;
                case 3:
                    if (!mSimDrive.IsOrg)
                    {
                        mSimDrive.Stop();
                        order_adv_state = 4;
                    } break;
                case 4:
                    if (mSimDrive.state != SimDrive.STATE.RUNNING)
                    {
                        mSimDrive.Pos = 0;
                        //归0完成
                        State = STATE.STOP;
                    } break;
            }
            return State;
        }

        public override string ToString()
        {
            return "Origin |" + marker.ToString();
        }
    }
    public class SyncOrder_RunTo : ASyncOrder
    {
        public int pos1;
        public UInt32 velocity;
        public bool hasDataGrid;

        public override void Start()
        {
            mSimDrive.Velocity = velocity;
            mSimDrive.Runto(pos1);
            base.Start();
            if (hasDataGrid)
                mflyad7.mPosGrid.Start();
        }
        public override STATE OnPoll()
        {
            if (mSimDrive.state != SimDrive.STATE.RUNNING)
            {
                if (hasDataGrid)
                    mflyad7.mPosGrid.End();

                State = STATE.STOP;
            }
            return State;
        }
        public override string ToString()
        {
            return "RunTo pos1=" + pos1.ToString() + "|" + marker.ToString();
        }
    }
    public class SyncOrder_RunToLC : ASyncOrder
    {
        public int pos1lc;
        public UInt32 velocity;
        public bool hasDataGrid;

        public override void Start()
        {
            mSimDrive.Velocity = velocity;
            mSimDrive.Runto(pos1lc - mflyad7.Pos1LCShift);
            base.Start();
            if (hasDataGrid)
                mflyad7.mPosGrid.Start();
        }
        public override STATE OnPoll()
        {
            if (mSimDrive.state != SimDrive.STATE.RUNNING)
            {
                if (hasDataGrid)
                    mflyad7.mPosGrid.End();

                State = STATE.STOP;
            }
            return State;
        }
        public override string ToString()
        {
            return "RunToLC pos1lc=" + pos1lc.ToString() + " pos1lcshift="+ mflyad7.Pos1LCShift.ToString()+" |" + marker.ToString();
        }
    }
    public class SyncOrder_Wait : ASyncOrder
    {
        public int ms;
        DateTime dtstart;
        public override void Start()
        {
            dtstart = mflyad7.Now;
            base.Start();
        }
        public override STATE OnPoll()
        {
            if ((mflyad7.Now - dtstart).TotalMilliseconds >= ms)
            {
                State = STATE.STOP;
            }
            return State;
        }
        public override string ToString()
        {
            return "Wait ms=" + ms.ToString() + "|" + marker.ToString();
        }
    }
    

}


