﻿// Game4Automation (R) Framework for Automation Concept Design, Virtual Commissioning and 3D-HMI
// (c) 2019 in2Sight GmbH - Usage of this source code only allowed based on License conditions see https://game4automation.com/lizenz  

using game4automation;
using UnityEngine;

namespace game4automation
{
    [RequireComponent(typeof(Drive))]
    //! Behavior model of a cylinder movement which can be connected to a Drive.
    //! The cylinder is defined by a maximum (*MaxPos*) and minimum (*MinPos*) position in millimeter
    [HelpURL("https://game4automation.com/documentation/current/drivebehaviour.html")]
    public class Drive_Simple : BehaviorInterface
    {
       
        [Header("Settings")] public float ScaleSpeed = 1;  //!< Scale factor for the input and output speed and acceleration
        public float ManualSpeed = 1;  //!< Speed setting for usage without IO
        public float ManualAccelaration = 1;  //!< Accelaration setting for usage without IO

        [Header("PLC IOs")] public PLCOutputFloat Speed; //!< PLCOutput for the speed of the drive in millimeter / second, can be scaled by Scale factor.
        public PLCOutputFloat Accelaration; //!< PLCOutput for the speed of the drive in millimeter / second, can be scaled by Scale factor.
        public PLCOutputBool Forward; //!< Signal to move the drive forward
        public PLCOutputBool Backward; //!< Signal to move the drive backward
        public PLCInputFloat IsAtPosition; //!< Signal for current position of the drive (in millimeter).
        public PLCInputFloat IsAtSpeed; //!< Signal for current speed of the drive (in millimeter/s).
        public PLCInputBool IsDriving; //!< Signal is true if Drive is driving.
        public PLCInputBool IsAtLowerLimit; //!< Signal is true if Drive is at lower limit.
        public PLCInputBool IsAtUpperLimit; //!< Signal is true if Drive is at upper limit.

        private Drive Drive;
        private bool _isSpeedNotNull;
        private bool _isIsAtPositionNotNull;
        private bool _isForwardNotNull;
        private bool _isBackwardNotNull;
        private bool _isIsDrivingNotNull;
        private bool _isIsAtSpeedNotNull;
        private bool _isAccelerationNotNull;
        private bool _isUpperLimitNotNull;
        private bool _isLowerLimitNotNull;

        // Use this for initialization
        void Start()
        {
            _isIsDrivingNotNull = IsDriving != null;
            _isBackwardNotNull = Backward != null;
            _isForwardNotNull = Forward != null;
            _isIsAtPositionNotNull = IsAtPosition != null;
            _isIsAtSpeedNotNull = IsAtSpeed != null;
            _isSpeedNotNull = Speed != null;
            _isAccelerationNotNull = Accelaration != null;
            _isUpperLimitNotNull = IsAtUpperLimit != null;
            _isLowerLimitNotNull = IsAtLowerLimit != null;
            Drive = GetComponent<Drive>();
        }

        // Update is called once per frame
        void FixedUpdate()
        {
            // Get external PLC Outputs
            if (_isSpeedNotNull)
            {
                Drive.TargetSpeed = Speed.Value * ScaleSpeed;
            }
            else
            {
                Drive.TargetSpeed = ManualSpeed;
            }
                
            if (_isForwardNotNull)
                Drive.JogForward = Forward.Value;
            if (_isBackwardNotNull)
                Drive.JogBackward = Backward.Value;
            if (_isAccelerationNotNull)
            {
                Drive.Acceleration = Accelaration.Value * ScaleSpeed;
            }
            else
            {
                Drive.Acceleration = ManualAccelaration;
            }
                
        
            // Set external PLC Outpits
            if (_isIsAtPositionNotNull)
                IsAtPosition.Value = Drive.CurrentPosition/ ScaleSpeed;
            if (_isIsAtSpeedNotNull)
                IsAtSpeed.Value = Drive.CurrentSpeed/ ScaleSpeed ;
            if (_isIsDrivingNotNull)
                IsDriving.Value = Drive.IsRunning;
            if (_isUpperLimitNotNull)
                IsAtUpperLimit.Value = Drive.CurrentPosition == Drive.UpperLimit;
            if (_isLowerLimitNotNull)
                IsAtLowerLimit.Value = Drive.CurrentPosition == Drive.LowerLimit;
        }
    }
}