/*
Copyright © 2011 Brad Murry
All rights reserved.

BSD License:

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 
OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.



KM_Axis.cs 
*/
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Diagnostics;
using MachineUtil;

namespace KMotion_dotNet
{
    /// <summary>
    /// Axis class for KM_Controller
    /// Use to control a single axis channel
    ///  
    /// The axis channel can be configured via a c program and the KM_Controller
    /// 
    /// or
    /// 
    /// Use the configuration methods to set the desired parameters
    /// </summary>
    public partial class KM_Axis
    {
        public class KM_Axis_TuningParams
        {
            #region Fields
            /// <summary>
            /// KM_Controller Object for used for device access
            /// </summary>
            protected KM_Controller _Controller;
            /// <summary>
            /// Axis Channel Number
            /// Valid range = ( 0 - 7 )
            /// </summary>
            protected int _ID = -1; 



               protected bool _LimitSwitchWatchPos = true;
        protected bool _LimitSwitchStopWhenLowPos = true;
        protected int _LimitSwitchBitPos = 0;
        protected bool _LimitSwitchWatchNeg = true;
        protected bool _LimitSwitchStopWhenLowNeg = true;
        protected int _LimitSwitchBitNeg =0;
        protected LimitSwitchAction _LimitSwitchAction = LimitSwitchAction.DisallowDriveIntoLimit;
            #endregion

            #region Properties
            /// <summary>
            /// Axis Channel Number
            /// Valid range = ( 0 - 7 )
            /// </summary>
            public int ID
            {
                get
                {
                    return _ID;
                }
                set
                {
                    _ID = value;
                }
            }

            public double Jerk
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("Jerk{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("Jerk{0}={1}", _ID, value));
                }
            }

            public double Pgain
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("P{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("P{0}={1}", _ID, value));
                }
            }

            public double Igain
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("I{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("I{0}={1}", _ID, value));
                }
            }

            public double Dgain
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("D{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("D{0}={1}", _ID, value));
                }
            }

            public double FFAccel
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("FFAccel{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("FFAccel{0}={1}", _ID, value));
                }
            }

            public double FFVel
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("FFVel{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("FFVel{0}={1}", _ID, value));
                }
            }

            public double BacklashAmount
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("BacklashAmount{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("BacklashAmount{0}={1}", _ID, value));
                }
            }

            public double BacklashRate
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("BacklashRate{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("BacklashRate{0}={1}", _ID, value));
                }
            }

            public int BacklashMode
            {
                get
                {
                    return GetConsoleIntegerValue(String.Format("BacklashMode{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("BacklashMode{0}={1}", _ID, value));
                }
            }



            public double CommutationOffset
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("CommutationOffset{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("CommutationOffset{0}={1}", _ID, value));
                }
            }
            public double DeadBandGain
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("DeadBandGain{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("DeadBandGain{0}={1}", _ID, value));
                }
            }
            public double DeadBandRange
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("DeadBandRange{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("DeadBandRange{0}={1}", _ID, value));
                }
            }



            public int InputMode
            {
                get
                {
                    return GetConsoleIntegerValue(String.Format("InputMode{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("InputMode{0}={1}", _ID, value));
                }
            }

            public double InputOffset
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("InputOffset{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("InputOffset{0}={1}", _ID, value));
                }
            }

            public double InputChan0
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("InputChan0{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("InputChan0{0}={1}", _ID, value));
                }
            }

            public double InputChan1
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("InputChan1{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("InputChan1{0}={1}", _ID, value));
                }
            }

            public double InputGain
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("InputGain{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("InputGain{0}={1}", _ID, value));
                }
            }



            public double InvDistPerCycle
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("InvDistPerCycle{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("InvDistPerCycle{0}={1}", _ID, value));
                }
            }
            public double Lead
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("Lead{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("Lead{0}={1}", _ID, value));
                }
            }


            public bool LimitSwitchWatchNeg
            {
                get
                {
                    return _LimitSwitchWatchNeg;
                }
                set
                {
                    _LimitSwitchWatchNeg = value;
                }
            }
            public bool LimitSwitchStopWhenLowNeg
            {
                get
                {
                    return _LimitSwitchStopWhenLowNeg;
                }
                set
                {
                    _LimitSwitchStopWhenLowNeg = value;
                }
            }
            public int LimitSwitchBitNeg
            {
                get
                {
                    return _LimitSwitchBitNeg;
                }
                set
                {
                    _LimitSwitchBitNeg = value;
                }
            }

            public bool LimitSwitchWatchPos
            {
                get
                {
                    return _LimitSwitchWatchPos;
                }
                set
                {
                    _LimitSwitchWatchPos = value;
                }
            }
            public bool LimitSwitchStopWhenLowPos
            {
                get
                {
                    return _LimitSwitchStopWhenLowPos;
                }
                set
                {
                    _LimitSwitchStopWhenLowPos = value;
                }
            }
            public int LimitSwitchBitPos
            {
                get
                {
                    return _LimitSwitchBitPos;
                }
                set
                {
                    _LimitSwitchBitPos = value;
                }
            }
            public LimitSwitchAction LimitSwitchAction
            {
                get
                {
                    return _LimitSwitchAction;
                }
                set
                {
                    _LimitSwitchAction = value;
                }
            }
            public string LimitSwitch
            {
                get
                {
                    return _Controller.WriteLineReadLine(String.Format("LimitSwitch{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("LimitSwitch{0}={1}", _ID, value));
                }
            }


            public int MasterAxis
            {
                get
                {
                    return GetConsoleIntegerValue(String.Format("MasterAxis{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("MasterAxis{0}={1}", _ID, value));
                }
            }
            public double MaxErr
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("MaxErr{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("MaxErr{0}={1}", _ID, value));
                }
            }
            public double MaxFollowingError
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("MaxFollowingError{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("MaxFollowingError{0}={1}", _ID, value));
                }
            }
            public double MaxI
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("MaxI{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("MaxI{0}={1}", _ID, value));
                }
            }
            public double MaxOutput
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("MaxOutput{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("MaxOutput{0}={1}", _ID, value));
                }
            }


            public int OutputMode
            {
                get
                {
                    return GetConsoleIntegerValue(String.Format("OutputMode{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("OutputMode{0}={1}", _ID, value));
                }
            }
            public double OutputOffset
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("OutputOffset{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("OutputOffset{0}={1}", _ID, value));
                }
            }
            public double OutputChan0
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("OutputChan0{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("OutputChan0{0}={1}", _ID, value));
                }
            }
            public double OutputChan1
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("OutputChan1{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("OutputChan1{0}={1}", _ID, value));
                }
            }
            public double OutputGain
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("OutputGain{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("OutputGain{0}={1}", _ID, value));
                }
            }


            public double SlaveGain
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("SlaveGain{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("SlaveGain{0}={1}", _ID, value));
                }
            }
            public double StepperAmplitude
            {
                get
                {
                    return GetConsoleDoubleValue(String.Format("StepperAmplitude{0}", _ID));
                }
                set
                {
                    _Controller.WriteLine(String.Format("StepperAmplitude{0}={1}", _ID, value));
                }
            }


            #endregion

            public KM_Axis_TuningParams(KM_Controller controller, int id)
            {
                _Controller = controller;
                _ID = id;
            }


            public void SetIIR0(double p1, double p2, double p3, double p4, double p5)
            {
               _Controller.WriteLine(String.Format("IIR{0} 0={1} {2} {3} {4} {5}",
                   _ID, p1, p2, p3, p4, p5));
            }
            public void SetIIR1(double p1, double p2, double p3, double p4, double p5)
            {
                _Controller.WriteLine(String.Format("IIR{0} 1={1} {2} {3} {4} {5}",
                    _ID, p1, p2, p3, p4, p5));
            }
            public void SetIIR2(double p1, double p2, double p3, double p4, double p5)
            {
                _Controller.WriteLine(String.Format("IIR{0} 2={1} {2} {3} {4} {5}",
                    _ID, p1, p2, p3, p4, p5));
            }

            private double GetConsoleDoubleValue(string command)
            {
                var pos = _Controller.WriteLineReadLine(command);
                bool success = false;
                double val = 0.0;
                success = Double.TryParse(pos, out val);
                if (!success)
                {
                    //Maybe write to an error log...
                }
                return val;
            }
            private int GetConsoleIntegerValue(string command)
            {
                var pos = _Controller.WriteLineReadLine(command);
                bool success = false;
                int val = 0;
                success = Int32.TryParse(pos, out val);
                if (!success)
                {
                    //Maybe write to an error log...
                }
                return val;
            }

            public void UpdateLimitParams()
            {
                BitNode bn = new BitNode(32, true);
                bn.SetBit(0, _LimitSwitchWatchNeg);
                bn.SetBit(1, _LimitSwitchWatchPos);
                bn.SetBit(2, _LimitSwitchStopWhenLowNeg);
                bn.SetBit(3, _LimitSwitchStopWhenLowPos);
                bn.SetBits(4, (int)_LimitSwitchAction);
                bn.SetBits(16, _LimitSwitchBitNeg);
                bn.SetBits(24, _LimitSwitchBitPos);
                LimitSwitch = bn.ToHex();
            }
        }
    }
}
