using System;
using System.IO;
using System.Linq;
using ln.ethercat.controller;
using ln.ethercat.controller.drives;
using ln.ethercat.controller.remote;
using ln.json;
using ln.json.mapping;
using ln.logging;

namespace ln.ethercat.service
{

    public enum GTOperationMode { NONE, INDEPENDENT, SCREW, PRESS }
    
    public class GalaTechControllerLogic : ControllerLogic
    {
        public MyParameters Parameters { get; set; }

        SDOValue svRelais;
        SDOValue svEnable;
        private SDOValue svMode0, svMode1, svMode2;

        public GalaTechControllerLogic()
        {
            Parameters = new MyParameters();
            if (File.Exists("gtcl.json"))
            {
                JSONValue configValue = JSONParser.ParseFile("gtcl.json");
                JSONMapper.DefaultMapper.Apply(configValue as JSONObject, Parameters);
            }
        }

        public void Save()
        {
            if (JSONMapper.DefaultMapper.Serialize(Parameters, out JSONValue configObject))
            {
                using (StreamWriter sw = new StreamWriter("gtcl.json"))
                {
                    sw.Write(configObject.ToString());
                    sw.Flush();
                }
            }
        }

        public override void Initialize(Controller controller)
        {
            if (!(
                controller.ECMaster.GetSDOValue(1, 0x2012, 31, out svRelais) &&
                controller.ECMaster.GetSDOValue(1, 0x2012, 32, out svEnable) &&
                controller.ECMaster.GetSDOValue(2, 0x2008, 1, out svMode0) &&
                controller.ECMaster.GetSDOValue(2, 0x2008, 2, out svMode1) &&
                controller.ECMaster.GetSDOValue(2, 0x2008, 3, out svMode2)
            ))
                throw new Exception("could not retrieve needed SDOValues");
        }

        public override void Cycle(Controller controller)
        {
            GTOperationMode operationMode = GTOperationMode.NONE;

            object mm0 = svMode0.GetValue();
            Logging.Log(LogLevel.DEBUG,"MT: {0}", mm0);
            
            sbyte m0 = svMode0.GetValue<sbyte>();
            sbyte m1 = svMode1.GetValue<sbyte>();
            sbyte m2 = svMode2.GetValue<sbyte>();
            Logging.Log(LogLevel.DEBUG, "Mode Bytes: {0} {1} {2}", m0,m1,m2);
            
            if ((m0 == 1) && (m1 == 0) && (m2 == 0))
                operationMode = GTOperationMode.INDEPENDENT;
            else if ((m0 == 0) && (m1 == 1) && (m2 == 0))
                operationMode = GTOperationMode.SCREW;
            else if ((m0 == 0) && (m1 == 0) && (m2 == 1))
                operationMode = GTOperationMode.PRESS;
            else
                operationMode = GTOperationMode.NONE;
            

            switch (controller.ControllerState)
            {
                case ControllerStates.FAULT:
                    svEnable.SetValue((byte)0x00);
                    controller.ECMaster.DriveControllers[0].TargetTorque = 0;
                    controller.ECMaster.DriveControllers[0].TargetSpeed = 0;
                    controller.ECMaster.DriveControllers[1].TargetTorque = 0;
                    controller.ECMaster.DriveControllers[1].TargetSpeed = 0;
                    break;
                case ControllerStates.NOTREADY:
                    svEnable.SetValue((byte)0x01);
                    break;
            }

            if (controller.ControllerState == ControllerStates.OPERATIONAL)
            {
                svRelais.SetValue((byte)0x01);

                switch (operationMode)
                {
                    case GTOperationMode.NONE:
                        controller.ECMaster.DriveControllers[0].TargetTorque = 0;
                        controller.ECMaster.DriveControllers[0].TargetSpeed = 0;
                        controller.ECMaster.DriveControllers[1].TargetTorque = 0;
                        controller.ECMaster.DriveControllers[1].TargetSpeed = 0;
                        break;
                    case GTOperationMode.INDEPENDENT:
                        controller.ECMaster.DriveControllers[0].DriveMode = DriveMode.SPEED;
                        controller.ECMaster.DriveControllers[0].TargetTorque = 0;
                        controller.ECMaster.DriveControllers[0].TargetSpeed = controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
                        controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.SPEED;
                        controller.ECMaster.DriveControllers[1].TargetTorque = 0;

                        switch (controller.Remotes.FirstOrDefault()?.FeederOperation)
                        {
                            case FeederOperation.LEFT:
                                controller.ECMaster.DriveControllers[1].TargetSpeed = Parameters.FeedSpeedRatio * controller.Remotes.FirstOrDefault()?.Targets[1] ?? 0.0;
                                break;
                            case FeederOperation.RIGHT:
                                controller.ECMaster.DriveControllers[1].TargetSpeed = Parameters.FeedSpeedRatio * -controller.Remotes.FirstOrDefault()?.Targets[1] ?? 0.0;
                                break;
                            default:
                                controller.ECMaster.DriveControllers[1].TargetSpeed = 0.0;
                                break;

                        }
                            
                        break;
                    case GTOperationMode.SCREW:
                        controller.ECMaster.DriveControllers[0].DriveMode = DriveMode.SPEED;
                        controller.ECMaster.DriveControllers[0].TargetTorque = 0;
                        controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.SPEED;
                        controller.ECMaster.DriveControllers[1].TargetTorque = 0;

                        switch (controller.Remotes.FirstOrDefault()?.FeederOperation)
                        {
                            case FeederOperation.LEFT:
                                controller.ECMaster.DriveControllers[0].TargetSpeed = controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
                                controller.ECMaster.DriveControllers[1].TargetSpeed = Parameters.FeedScrewGearRatio * controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
                                break;
                            case FeederOperation.RIGHT:
                                controller.ECMaster.DriveControllers[0].TargetSpeed = -controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
                                controller.ECMaster.DriveControllers[1].TargetSpeed = -Parameters.FeedScrewGearRatio * controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
                                break;
                            default:
                                controller.ECMaster.DriveControllers[0].TargetSpeed = 0.0;
                                controller.ECMaster.DriveControllers[1].TargetSpeed = 0.0;
                                break;

                        }
                            
                        break;
                    case GTOperationMode.PRESS:
                        controller.ECMaster.DriveControllers[0].DriveMode = DriveMode.SPEED;
                        controller.ECMaster.DriveControllers[0].TargetTorque = 0;
                        controller.ECMaster.DriveControllers[0].TargetSpeed = controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
                        controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.TORQUE;
                        controller.ECMaster.DriveControllers[1].TargetSpeed = 0;

                        switch (controller.Remotes.FirstOrDefault()?.FeederOperation)
                        {
                            case FeederOperation.LEFT:
                                controller.ECMaster.DriveControllers[1].TargetTorque = Parameters.FeedTorqueRatio * controller.Remotes.FirstOrDefault()?.Targets[1] ?? 0.0;
                                break;
                            case FeederOperation.RIGHT:
                                controller.ECMaster.DriveControllers[1].TargetTorque = -Parameters.FeedTorqueRatio * controller.Remotes.FirstOrDefault()?.Targets[1] ?? 0.0;
                                break;
                            default:
                                controller.ECMaster.DriveControllers[1].TargetTorque = 0.0;
                                break;

                        }
                            
                        break;
                }
                
            } else {
                svRelais.SetValue((byte)0x00);
            }
        }


        public class MyParameters
        {
            public double FeedSpeedRatio = -1.0;
            public double FeedTorqueRatio = 0.5;
            public double FeedScrewGearRatio = -0.015;
            
        }

    }
}