﻿// 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 NaughtyAttributes;
using UnityEngine;
using System;

namespace game4automation
{
	[RequireComponent(typeof(Drive))]
	//! Behavior model of an intelligent drive which is getting a destination and moving to the destination.
	//! This component needs to have as a basis a standard Drive.
	[HelpURL("https://game4automation.com/documentation/current/drivebehaviour.html")]
	public class Drive_DestinationMotorWWA: BehaviorInterface {
		private Drive Drive;

		private new void Awake()
		{
			_isActPosNotNull = ActPos!=null;
		}

		[Header("PLC IOs")]
		public PLCOutputInt OperMode; //!< Current operating mode of the drive
		public PLCOutputInt ReqPos; //!< Requested position
		public PLCOutputInt MaxSpeed; //!< Target (maximum) speed of the drive in mm/ second
		public PLCOutputInt Acc; //!< Acceleration of the drive in millimeters / second
		public PLCOutputInt SpecCmd; //!< Drive special command
		public PLCInputBool ErrFollow; //!< Follower Error
	
		public PLCInputInt ActOperMode; //<! Current Operating mode
		public PLCInputInt ActSpeed; //<! CurrentSpeed
		public PLCInputInt ActPos;//<! CurrentPosition
		public PLCInputBool InPos; //!< Signal is true if Drive is at destination position

		public double GearRatio;
		public double PulsesPerRev = 409600;

		private double PositionOffs;

		private bool _isOperModeNotNull;
		private bool _isReqPosNotNull;
		private bool _isMaxSpeedNotNull;
		private bool _isAccNotNull;
		private bool _isSpecCmdNotNull;

		private bool _isActSpeedNotNull;
		private bool _isActOperModeNotNull;
		private bool _isActPosNotNull;
		private bool _isInPosNotNull;


		// Use this for initialization
		void Start()
		{
			_isOperModeNotNull = OperMode != null;
			_isReqPosNotNull = ReqPos != null;
			_isMaxSpeedNotNull = MaxSpeed != null;
			_isAccNotNull = Acc != null;
			_isSpecCmdNotNull = SpecCmd != null;

			_isActSpeedNotNull = ActSpeed!=null;
			_isActOperModeNotNull = ActOperMode != null;
			_isActPosNotNull = ActPos != null;
			_isInPosNotNull = InPos != null;

			Drive = GetComponent<Drive>();
		}

		// Update is called once per frame
		void FixedUpdate()
		{
			

			// PLC Outputs
			if (_isOperModeNotNull  && _isMaxSpeedNotNull && _isReqPosNotNull)
            {
				Drive.TargetStartMove = false;
				Drive.TargetSpeed = (float)MaxSpeed.Value / 100 * 6;

				switch (OperMode.Value)
				{
					case 0:
						//Drive.TargetPosition = 0;
						Drive.TargetStartMove = false;
						Drive.JogForward = false;
						Drive.JogBackward = false;
						break;
					case 1:
						//Drive.TargetPosition = 0;
						Drive.TargetStartMove = false;
						Drive.JogForward = Drive.TargetSpeed > 0;
						Drive.JogBackward = Drive.TargetSpeed < 0;
						break;
					case 2:
						Drive.JogForward = false;
						Drive.JogBackward = false;
						double nDegMotor = ((double)(ReqPos.Value) / PulsesPerRev) * 360.0;
						double nDegGearbox = nDegMotor / GearRatio;
						Drive.TargetPosition = (float)(nDegGearbox+PositionOffs);
						Drive.TargetStartMove = true;
						break;
				}

            }
			if (Drive.CurrentPosition == Drive.LowerLimit || Drive.CurrentPosition == Drive.UpperLimit)
			{
				ErrFollow.Value = true;
				//Drive.JogForward = false;
			}
			else
				ErrFollow.Value = false;

			if (_isAccNotNull)
				Drive.Acceleration = Acc.Value;
			if (_isSpecCmdNotNull)
            {
				if(SpecCmd.Value == 354 && Drive.CurrentSpeed == 0)
					PositionOffs = Drive.CurrentPosition;
			}

			// PLC Inputs
			if (_isActSpeedNotNull)
				ActSpeed.Value = (int)(Drive.CurrentSpeed*100 / 6);
			if (_isActOperModeNotNull)
				ActOperMode.Value = OperMode.Value;
			if (_isActPosNotNull)
            {
				double DrivePos = (double)((Drive.CurrentPosition-PositionOffs) / 360.0);
				double PulsePos = (DrivePos * PulsesPerRev);
				double MotorPos = (PulsePos * GearRatio);
				ActPos.Value = (int)MotorPos;
			}
			if (_isInPosNotNull)
				InPos.Value = Drive.IsAtTarget;
		}
	}
}
