// T-Code Serial Controller v3.2
// TempestVR 11-7-22
//
// This plugin automatically tracks character movement in the scene and outputs T-Code
// commands to a selected serial port
// A big thanks to VAMRobot for his work using colliders to track character movement!
//
// 24-09-2021, v3.1 - Tracking ramp in/out bug fix. T-valve control (A1) added.
// 11-07-2022, v3.2 - Serial function performance improvement. Thanks Bob!


using System.Linq;
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using SimpleJSON;
using System;
using System.Diagnostics;
using System.IO.Ports;
using System.Threading;
using System.Text;

using MeshVR;
using MVR.FileManagementSecure;

using UnityEngine.UI;
using UnityEngine.Events;
using Request = MeshVR.AssetLoader.AssetBundleFromFileRequest;
using AssetBundles;

namespace Tempest {	
    public class TCodeCtrl : MVRScript {	

		public static class JsonHelper {
			public static T[] FromJson<T>(string json)
			{
				Wrapper<T> wrapper = JsonUtility.FromJson<Wrapper<T>>(json);
				return wrapper.Items;
			}

			public static string ToJson<T>(T[] array)
			{
				Wrapper<T> wrapper = new Wrapper<T>();
				wrapper.Items = array;
				return JsonUtility.ToJson(wrapper);
			}

			public static string ToJson<T>(T[] array, bool prettyPrint)
			{
				Wrapper<T> wrapper = new Wrapper<T>();
				wrapper.Items = array;
				return JsonUtility.ToJson(wrapper, prettyPrint);
			}

			[Serializable]
			private class Wrapper<T>
			{
				public T[] Items;
			}
		}

        // Name of plugin
        public static string pluginName = "Serial Controller";
		public static readonly string SCREENSHOT_DIRECTORY = "Saves/VR_Renders/";
		// Hardcoded, not sure if this matters
		public static readonly int FRAMES_PER_SECOND = 60;
		public static readonly float FRAME_TO_TIME = 1000.0f / (float)FRAMES_PER_SECOND;

        // Serial Variables
        protected JSONStorableStringChooser portChooser;
        private string[] portNames;
        private string portSelected = "None";
        private int baudRate;
        private int sendThisChannelAnyway = 0;
        protected UIDynamicButton startButton;
        protected UIDynamicButton stopButton;
        protected JSONStorableBool autoLoadToggle;
        protected UIDynamicButton loadButton;
        protected UIDynamicButton saveButton;
        protected JSONStorableString serialReport;
        protected UIDynamicTextField serialOutWindow;
        protected JSONStorableBool suppressRepeats;
        protected JSONStorableBool logSerialIn;
        
        // TCode axis variables
        private List<TCodeAxis> axesList;

		// Recorded data to be saved later
		private List<RecordData> dataL0 = new List<RecordData>();
		private List<RecordData> dataL1 = new List<RecordData>();
		private List<RecordData> dataL2 = new List<RecordData>();
		private List<RecordData> dataR0 = new List<RecordData>();
		private List<RecordData> dataR1 = new List<RecordData>();
		private List<RecordData> dataR2 = new List<RecordData>();
		private List<RecordData> dataA1 = new List<RecordData>();
		
		// Used to keep track how many frames have passed.
		// Using real passed time would be better, but when recording, actual
		// time passed is off as the game FPS is incredibly low, so ingame one frame
		// may take several seconds, which messes up the timestamps. If we can find
		// the true passed simulation time, that would be ideal.
		private int frameCount = 0;
        
        // Tracking Interface
        protected JSONStorableStringChooser femaleChooser;
        protected JSONStorableStringChooser maleChooser;
        protected JSONStorableString positionReport;
        protected UIDynamicTextField positionWindow;
        protected JSONStorableFloat suckMouth;
        protected JSONStorableBool suckMouthPop;
        protected JSONStorableFloat suckVagina;
        
        // Atom tracking variables
        private List<string> playerList;
        private List<Female> females;
        private List<Male> males;
        
        // Robot position tracking variables
        private float setTargetX;
        private float setTargetY;
        private float setTargetZ;
        private float setTargetRY;
        private float setTargetRZ;
        private float setPositionX;
        private float setPositionY;
        private float setPositionZ;
        private float setPositionRY;
        private float setPositionRZ;
        private float setSuck;
        private int trackingCountIn;
        private int neutralCountIn;
        
        private object serialLock = new object();

        private bool serialIsStarted = false;

        // Output sliders
        
        // Serial Stuff
        SerialPort serial;
		
        
        // Function to initiate plugin
        public override void Init() {
            pluginLabelJSON.val = "T-Code Serial Controller";
        
        
            // Left Column of UI - Serial stuff
            
            // Select COM Port
            portNames = System.IO.Ports.SerialPort.GetPortNames();
            List<string> comPorts = new List<string>(portNames);
            portChooser = new JSONStorableStringChooser("COM Port Chooser", comPorts, "None", "Select COM port", PortChooserCallback);
            UIDynamicPopup portChooserPopup = CreatePopup(portChooser, false);
            portChooserPopup.labelWidth = 300f;
            
            // Set Baud Rate
            baudRate = 115200;
            
            // Serial buttons
            startButton = CreateButton("Start Serial", false);
            if (startButton != null) {
                startButton.button.onClick.AddListener(StartButtonCallback);
            }
            stopButton = CreateButton("Stop Serial", false);
            if (stopButton != null) {
                stopButton.button.onClick.AddListener(StopButtonCallback);
            }
            
            // Window to show what's being sent over serial
            serialReport = new JSONStorableString("Serial Output", "");
            serialOutWindow = CreateTextField(serialReport, false);
    
            // Load/Save axis range limits controls
            autoLoadToggle = new JSONStorableBool("Auto load range limits from device", true);
            CreateToggle(autoLoadToggle, false);
            loadButton = CreateButton("'D2' - Load range limits from device", false);
            if (loadButton != null) {
                loadButton.button.onClick.AddListener(LoadButtonCallback);
            }
            saveButton = CreateButton("'$' - Save range limits to device", false);
            if (saveButton != null) {
                saveButton.button.onClick.AddListener(SaveButtonCallback);
            }
        
            axesList = new List<TCodeAxis>();
            axesList.Add(new TCodeAxis("L0","Up",false));
            axesList.Add(new TCodeAxis("L1","Forward",false));
            axesList.Add(new TCodeAxis("L2","Left",true));
            axesList.Add(new TCodeAxis("R0","Twist",true));
            axesList.Add(new TCodeAxis("R1","Roll",true));
            axesList.Add(new TCodeAxis("R2","Pitch",false));
            axesList.Add(new TCodeAxis("A1","Suck",false));
            
            // Draw axis range sliders
            for (int n = 0; n < axesList.Count; n++) {
                CreateSlider(axesList[n].rangeSliderA, false);
                CreateSlider(axesList[n].rangeSliderB, false);
            }
            // Draw axis inverter controls
            for (int n = 0; n < axesList.Count; n++) {
                CreateToggle(axesList[n].invert, false);
            }
            // Draw axis override controls
            for (int n = 0; n < axesList.Count; n++) {
                CreateToggle(axesList[n].manualBool, false);
                CreateSlider(axesList[n].manual, false);
            }
            
            // Toggle system to suppress repeating TCode commands
            suppressRepeats = new JSONStorableBool("Suppress repeat commands", true);
            CreateToggle(suppressRepeats, false);
            // Toggle system to suppress repeating TCode commands
            logSerialIn = new JSONStorableBool("Print incoming serial to log", true);
            CreateToggle(logSerialIn, false);
            
            // Robot axis controls
            setTargetX = 0.5f;
            setTargetY = 0.5f;
            setTargetZ = 0.5f;
            setTargetRY = 0.5f;
            setTargetRZ = 0.5f;
            trackingCountIn = 0;
            neutralCountIn = 0;
        
            
            // Right Column of UI - Scene tracking
            
            // Find all 'Person' Atoms currently in the scene
            Atom tempAtom;
            playerList = new List<string>();
            females = new List<Female>();
            males = new List<Male>();
            foreach (string atomUID in SuperController.singleton.GetAtomUIDs()) {
                tempAtom = SuperController.singleton.GetAtomByUid(atomUID);
                if (tempAtom.type == "Person") {
                    // Add new Player/'Person' Atom to playerList
                    playerList.Add(atomUID);

                    string sex = "female";

                    // Check if the penis tip collider exists to determine if male or female
                    try {
                        foreach (Collider c in tempAtom.gameObject.GetComponentsInChildren<Collider>()) {
                            // Check if the penis tip collider exists to determine if male or female
                            if (c.gameObject.name == "AutoColliderGen3bHard") {
                                sex = "male";
                            }
                        }
                    } catch { sex = "female"; }

                    if (sex == "female") {
                        females.Add(new Female(atomUID));
                    } else {
                        males.Add(new Male(atomUID));
                    }
                }
            }
            
            // Setup female selector
            List<string> femalesList = new List<string>();
            for (int f = 0; f < females.Count(); f++) {
                femalesList.Add(females[f].name);
            }
            femaleChooser = new JSONStorableStringChooser("Female Chooser", femalesList, femalesList[0], "Select Female", FemaleChooserCallback);
            RegisterStringChooser(femaleChooser);
            UIDynamicPopup femaleChooserPopup = CreatePopup(femaleChooser, true);
            femaleChooserPopup.labelWidth = 300f;

            // Setup male selector
            List<string> malesList = new List<string>();
            for (int m = 0; m < males.Count(); m++) {
                malesList.Add(males[m].name);
            }
            maleChooser = new JSONStorableStringChooser("Male Chooser", malesList, malesList[0], "Select Male", MaleChooserCallback);
            RegisterStringChooser(maleChooser);
            UIDynamicPopup maleChooserPopup = CreatePopup(maleChooser, true);
            maleChooserPopup.labelWidth = 300f;
            
            // Window to show target position
            positionReport = new JSONStorableString("Target Position", "");
            positionWindow = CreateTextField(positionReport, true);
            
            // T-Valve controls
            suckMouth = new JSONStorableFloat("T-Valve Suction - Mouth", 1f, 0f, 1f, true, true);
            CreateSlider(suckMouth,true);
            suckMouthPop = new JSONStorableBool("T-Valve Mouth 'Pop' effect", true);
            CreateToggle(suckMouthPop, true);
            suckVagina = new JSONStorableFloat("T-Valve Suction - Vagina", 0.5f, 0f, 1f, true, true);
            CreateSlider(suckVagina,true);
        
        }

        // Update is called with each rendered frame by Unity
        void Update() {
            // Track atoms and set robot position 
            TrackAndSet();
            SendPulse();
        }
        
        
        // Function to track if the selected male's penis is in a mouth or vagina
        // (Credit to VAMRobot for a lot of this code!)
        // Function then sets the robot position
        public void TrackAndSet() {
            
            // Update female(s) data
            for (int f = 0; f < females.Count(); f++) {
                females[f].update();
            }
            // Update male(s) data
            for (int m = 0; m < males.Count(); m++) {
                males[m].update();
            }
            
            // Initial assumption is that robot isn't tracking
            bool tracking = false;
            
            // Cycle through any male to identify the male (if more than one male is in the scene) that was selected through
            // the VAM UI selector and who's penis is the basis for driving the robot
            for (int m = 0; m < males.Count(); m++) {
                if (males[m].name == maleChooser.val) {
                    // Once the male is found, cycle through any females in the scene
                    for (int f = 0; f < females.Count(); f++) {
                        // Loop through the vagina triggers and look for any collisions between them and any other colliders in the scene
                        for (int i = 0; i < females[f].vaginaTriggers.Length; i++) {
                            Dictionary<Collider, bool> vaginaDictionary = females[f].vaginaRigidbodies[i].GetComponent<CollisionTriggerEventHandler>().collidingWithDictionary;
                            // Check for any collisions, zero if no collisions currently occurring
                            if (vaginaDictionary.Count > 0) {
                                foreach (KeyValuePair<Collider, bool> entry in vaginaDictionary) {
                                    // Loop through penis colliders
                                    for (int c = 0; c < males[m].penisNames.Length; c++) {
                                        // Check if one of the penis colliders is having a collision with the current female's vagina trigger
                                        if (entry.Key.gameObject.name == males[m].penisNames[c]) {
                                            // Set vagina insertion flag for consistency checking
                                            females[f].vaginaInsertionFlags[i] = true;

                                            if (c == (males[m].penisNames.Length - 1)) {
                                                females[f].vaginaTipInsertionFlags[i] = true;
                                            }
                                        }
                                    }
                                }
                            }
                        }

                        // Consistency checking to ensure the penis is actually properly into the vagina
                        // Sometimes in the VAM environment the various rigidbodies and colliders can move through each other
                        // And this consistency checking avoids such spurious collisions from driving the robot in an uncontrolled manner
                        bool penisInserted = false;
                        int penisTipInsertionIndex = 0;
                        for (int i = 0; i < females[f].vaginaTriggers.Length; i++) {
                            if (females[f].vaginaInsertionFlags[i]) {
                                for (int j = i; j >= 0; j--) {
                                    if (females[f].vaginaInsertionFlags[j]) {
                                        penisInserted = true;
                                        penisTipInsertionIndex = i;
                                    }
                                }
                            } else {
                                break;
                            }
                        }


                        // Reset the vagina insertion and tip insertion flags
                        for (int i = 0; i < females[f].vaginaTriggers.Length; i++) {
                            females[f].vaginaInsertionFlags[i] = false;
                            females[f].vaginaTipInsertionFlags[i] = false;
                        }

                        // If the penis is inserted into the vagina
                        if (penisInserted) {
                            // Setup T-code reference coordinate system
                            // X(L0) is up/down in reference to the selected male's penis vector and is positive up
                            // Y(L1) is toward/away orthogonal to the selected male's penis vector and is positive away
                            // Z(L2) is left/right orthogonal to the selected male's penis vector and is positive left

                            // Vector from the selected male's penis's base to tip colliders
                            Vector3 refAxisX = males[m].penisVector;

                            // Use the vector from male's abdomenControl Rigidbody to the male's penis's base collider to establish the Z reference axis
                            Vector3 refAxisZ = Vector3.Cross(refAxisX, males[m].abdomen - males[m].penis[0]);
                            refAxisZ = (refAxisX.magnitude / refAxisZ.magnitude) * refAxisZ;

                            // Use the reference X and Z axes to establish the orthogonal Y axis
                            Vector3 refAxisY = Vector3.Cross(refAxisX, refAxisZ);
                            refAxisY = (refAxisX.magnitude / refAxisY.magnitude) * refAxisY;

                            // Vector from the female's vagina's labia trigger to the male's penis's base collider
                            Vector3 vaginaLabiaToPenisBase = females[f].vagina[0] - males[m].penis[0];

                            // Calculate X(L0) for robot based on the reference X axis and the vector from the female's vagina's labia trigger to the male's penis's base collider
                            float robotX = Vector3.Dot(refAxisX, vaginaLabiaToPenisBase) / (refAxisX.magnitude * refAxisX.magnitude);
                            robotX = (robotX+0.1f)/1.2f;

                            // Calculate Y(L1) for robot based on the reference Y axis and the vector from the female's vagina's labia trigger to the male's penis's base collider
                            float robotY = 0.5f + (2f + 2f*robotX)*Vector3.Dot(refAxisY, vaginaLabiaToPenisBase) / (refAxisX.magnitude * refAxisX.magnitude);

                            // Calculate Z(L2) for robot based on the reference Z axis and the vector from the female's vagina's labia trigger to the male's penis's base collider
                            float robotZ = 0.5f + (2f + 2f*robotX)*Vector3.Dot(refAxisZ, vaginaLabiaToPenisBase) / (refAxisX.magnitude * refAxisX.magnitude);

                            // Vector from the female's vagina's labia to vagina triggers
                            Vector3 vaginaLabiaToVaginaTrigger = females[f].vagina[0] - females[f].vagina[1];

                            // Calculate RY(R1) for robot based on the reference Z axis and the vector from the female's vagina's labia to vagina triggers
                            float robotRYAngle = 90.0f - Vector3.Angle(refAxisZ, vaginaLabiaToVaginaTrigger);
                            float robotRY = 0.5f + robotRYAngle / 45.0f;

                            // Calculate RZ(R2) for robot based on the reference Y axis and the vector from the female's vagina's labia to vagina triggers
                            float robotRZAngle = -(90.0f - Vector3.Angle(refAxisY, vaginaLabiaToVaginaTrigger));
                            float robotRZ = 0.5f + robotRZAngle / 45.0f;
                            
                            // Report tracking information
                            positionReport.val = "Inserted: Vagina\n";
                            positionReport.val += "Robot X (L0): " + robotX + "\n";
                            positionReport.val += "Robot Y (L1): " + robotY + "\n";
                            positionReport.val += "Robot Z (L2): " + robotZ + "\n";
                            positionReport.val += "Robot RY (R1): " + robotRY + "\n";
                            positionReport.val += "Robot RZ (R2): " + robotRZ;
                            //positionReport.val += "\n" + "Robot RY(R1) Angle: " + robotRYAngle;
                            //positionReport.val += "\n" + "Robot RZ(R2) Angle: " + robotRZAngle;
                            
                            // Output tracking data
                            tracking = true;
                            neutralCountIn = 0;
                            setTargetX = robotX;
                            setTargetY = robotY;
                            setTargetZ = robotZ;
                            setTargetRY = robotRY;
                            setTargetRZ = robotRZ;
                            
                            // T-Valve setting
                            setSuck = suckVagina.val;
                            
                        }

                        // Loop through the mouth triggers and look for any collisions between them and any other colliders in the scene
                        for (int i = 0; i < females[f].mouthTriggers.Length; i++) {
                            IDictionary<Collider, bool> mouthDictionary = females[f].mouthRigidbodies[i].GetComponent<CollisionTriggerEventHandler>().collidingWithDictionary;

                            // Check for any collisions, zero if no collisions currently occurring
                            if (mouthDictionary.Count > 0) {
                                foreach (KeyValuePair<Collider, bool> entry in mouthDictionary) {
                                    // Loop through penis colliders
                                    for (int c = 0; c < males[m].penisNames.Length; c++) {
                                        // Check if one of the penis colliders is having a collision with the current female's mouth trigger
                                        if (entry.Key.gameObject.name == males[m].penisNames[c]) {
                                            // Set mouth insertion flag for consistency checking
                                            females[f].mouthInsertionFlags[i] = true;

                                            if (c == (males[m].penisNames.Length - 1)) {
                                                females[f].mouthTipInsertionFlags[i] = true;
                                            }
                                        }
                                    }
                                }
                            }
                        }

                        // Consistency checking to ensure the penis is actually properly into the mouth
                        // Sometimes in the VAM environment the various rigidbodies and colliders can move through each other
                        // And this consistency checking avoids such spurious collisions from driving the robot in an uncontrolled manner
                        penisInserted = false;
                        penisTipInsertionIndex = 0;

                        for (int i = 0; i < females[f].mouthTriggers.Length; i++) {
                            if (females[f].mouthInsertionFlags[i]) {
                                for (int j = i; j >= 0; j--) {
                                    if (females[f].mouthInsertionFlags[j]) {
                                        penisInserted = true;

                                        penisTipInsertionIndex = i;
                                    }
                                }
                            } else { 
                                break;
                            }
                        }


                        // Reset the mouth insertion  and tip insertion flags
                        for (int i = 0; i < females[f].mouthTriggers.Length; i++) {
                            females[f].mouthInsertionFlags[i] = false;
                            females[f].mouthTipInsertionFlags[i] = false;
                        }

                        // If the penis is inserted into the mouth
                        if (penisInserted) {
                            // Setup T-code reference coordinate system
                            // X(L0) is up/down in reference to the selected male's penis vector and is positive up
                            // Y(L1) is toward/away orthogonal to the selected male's penis vector and is positive away
                            // Z(L2) is left/right orthogonal to the selected male's penis vector and is positive left

                            // Vector from the selected male's penis's base to tip colliders
                            Vector3 refAxisX = males[m].penisVector;

                            // Use the vector from male's abdomenControl Rigidbody to the male's penis's base collider to establish the Z reference axis
                            Vector3 refAxisZ = Vector3.Cross(refAxisX, males[m].abdomen - males[m].penis[0]);
                            refAxisZ = (refAxisX.magnitude / refAxisZ.magnitude) * refAxisZ;

                            // Use the reference X and Z axes to establish the orthogonal Y axis
                            Vector3 refAxisY = Vector3.Cross(refAxisX, refAxisZ);
                            refAxisY = (refAxisX.magnitude / refAxisY.magnitude) * refAxisY;

                            // Vector from the female's mouth's lip trigger to the male's penis's base collider
                            Vector3 mouthLipToPenisBase = females[f].mouth[0] - males[m].penis[0];

                            // Calculate X(L0) for robot based on the reference X axis and the vector from the female's mouth's lip trigger to the male's penis's base collider
                            float robotX = Vector3.Dot(refAxisX, mouthLipToPenisBase) / (refAxisX.magnitude * refAxisX.magnitude);
                            robotX = (robotX+0.1f)/1.3f;

                            // Calculate Y(L1) for robot based on the reference Y axis and the vector from the female's mouth's lip trigger to the male's penis's base collider
                            float robotY = 0.5f + (1f + 2f*robotX)*Vector3.Dot(refAxisY, mouthLipToPenisBase) / (refAxisX.magnitude * refAxisX.magnitude);

                            // Calculate Z(L2) for robot based on the reference Z axis and the vector from the female's mouth's lip trigger to the male's penis's base collider
                            float robotZ = 0.5f + (1f + 2f*robotX)*Vector3.Dot(refAxisZ, mouthLipToPenisBase) / (refAxisX.magnitude * refAxisX.magnitude);

                            // Vector from the female's mouth's lip to mouth triggers
                            Vector3 mouthLipToMouthTrigger = females[f].mouth[0] - females[f].mouth[1];

                            // Calculate RY(R1) for robot based on the reference Z axis and the vector from the female's mouth's lip to mouth triggers
                            float robotRYAngle = 90.0f - Vector3.Angle(refAxisZ, mouthLipToMouthTrigger);
                            float robotRY = 0.5f + robotRYAngle / 45.0f;

                            // Calculate RZ(R2) for robot based on the reference Y axis and the vector from the female's mouth's lip to mouth triggers
                            float robotRZAngle = -(90.0f - Vector3.Angle(refAxisY, mouthLipToMouthTrigger));
                            float robotRZ = 0.5f + robotRZAngle / 45.0f;

                            // Report tracking data
                            positionReport.val = "Inserted: Mouth\n";
                            positionReport.val += "Robot X (L0): " + robotX + "\n";
                            positionReport.val += "Robot Y (L1): " + robotY + "\n";
                            positionReport.val += "Robot Z (L2): " + robotZ + "\n";
                            positionReport.val += "Robot RY (R1): " + robotRY + "\n";
                            positionReport.val += "Robot RZ (R2): " + robotRZ;
                            //positionReport.val += "\n" + "Robot RY(R1) Angle: " + robotRYAngle;
                            //positionReport.val += "\n" + "Robot RZ(R2) Angle: " + robotRZAngle;

                            // Output tracking data
                            tracking = true;
                            neutralCountIn = 0;
                            setTargetX = robotX;
                            setTargetY = robotY;
                            setTargetZ = robotZ;
                            setTargetRY = robotRY;
                            setTargetRZ = robotRZ;
                            
                            // T-Valve setting
                            setSuck = suckMouth.val;
                            if (suckMouthPop.val && (robotX > 0.9f)) {
                                setSuck = 0f;
                            }

                        }
                    }
                }
            }
            
            // Code to set robot position
            if (tracking) {
                // Check whether connect ramp applies
                 int rampLength = 10;  // Number of frames for robot to adopt tracked position
                trackingCountIn++;
                if (trackingCountIn > rampLength) {
                    // If ramp-in complete, use tracked position
                    trackingCountIn = rampLength;
                    setPositionX = setTargetX;
                    setPositionY = setTargetY;
                    setPositionZ = setTargetZ;
                    setPositionRY = setTargetRY;
                    setPositionRZ = setTargetRZ;
                } else {
                    // ramp from neutral position to tracked position
                    float rampFraction = (float)trackingCountIn;
                    rampFraction /=rampLength;
                    setPositionX = setPositionX + rampFraction*(setTargetX-setPositionX);
                    setPositionY = setPositionY + rampFraction*(setTargetY-setPositionY);
                    setPositionZ = setPositionZ + rampFraction*(setTargetZ-setPositionZ);
                    setPositionRY = setPositionRY + rampFraction*(setTargetRY-setPositionRY);
                    setPositionRZ = setPositionRZ + rampFraction*(setTargetRZ-setPositionRZ);
                }
            } else {
                // If not tracking, use neutral position
                positionReport.val = "Not inserted\n";
                
                // Check whether disconnect ramp applies
                 int rampLength = 60;  // Number of frames for robot to return to neutral position
                neutralCountIn++;
                if (neutralCountIn > rampLength) {
                    // If ramp complete, adopt neutral position
                    neutralCountIn = rampLength;
                    setPositionX = 1f;
                    setPositionY = 0.5f;
                    setPositionZ = 0.5f;
                    setPositionRY = 0.5f;
                    setPositionRZ = 0.5f;
                } else {
                    // Ramp
                    float rampFraction = (float)neutralCountIn;
                    rampFraction /=rampLength;
                    setPositionX = setTargetX + rampFraction*(1f-setTargetX);
                    setPositionY = setTargetY + rampFraction*(0.5f-setTargetY);
                    setPositionZ = setTargetZ + rampFraction*(0.5f-setTargetZ);
                    setPositionRY = setTargetRY + rampFraction*(0.5f-setTargetRY);
                    setPositionRZ = setTargetRZ + rampFraction*(0.5f-setTargetRZ);
                }
                // Not tracking, so reset tracking count in
                trackingCountIn = 0;
                // Set T-valve open
                setSuck = 0f;
            }
            //Send position data to axes
            axesList.First(x => x.channel == "L0").position = setPositionX;
            axesList.First(x => x.channel == "L1").position = setPositionY;
            axesList.First(x => x.channel == "L2").position = setPositionZ;
            axesList.First(x => x.channel == "R1").position = setPositionRY;
            axesList.First(x => x.channel == "R2").position = setPositionRZ;
            axesList.First(x => x.channel == "A1").position = setSuck;
            
        }
        
        
        // Function to interpret incoming serial data
        public void InterpretInput(string inString) {
            // 
            switch(inString.Substring(0,1)) {
              case "L":
              case "R":
              case "V":
              case "A":
                string channel = inString.Substring(0,2);
                for (int n = 0; n < axesList.Count; n++) {
                    if (channel == axesList[n].channel) {
                        axesList[n].SetLimitsFromString(inString);
                    }
                }
                break;
              default:
                
                break;
            }
            
            if (inString == "Ready!" && autoLoadToggle.val) {
                SuperController.LogMessage("[Out]: D2");
                lock (serialLock) {
                    serial.WriteLine("D2 \n");
                }
            }
            
        }
        
        // Button callbacks
        protected void PortChooserCallback(string s) {
            portSelected = s;
        }

        protected void StartButtonCallback() {
			frameCount = 0;
			dataL0.Clear();
			dataL1.Clear();
			dataL2.Clear();
			dataR0.Clear();
			dataR1.Clear();
			dataR2.Clear();
			dataA1.Clear();
			
            StartSerial();
            SuperController.LogMessage("Serial connection started: " + portSelected + ", baud " + baudRate.ToString() );
            if (autoLoadToggle.val) {
                //SuperController.LogMessage("[Out]: D2");
                lock (serialLock) {
                    serial.WriteLine("D2 \n");
                }
            }
        }
		
		public string GetFileTimestamp()
        {
            return DateTime.Now.Year.ToString("D4") + DateTime.Now.Month.ToString("D2") + DateTime.Now.Day.ToString("D2") + "-"
                + DateTime.Now.Hour.ToString("D2") + DateTime.Now.Minute.ToString("D2") + DateTime.Now.Second.ToString("D2");
        }
		
		protected void writeFunscriptAxis(List<RecordData> data, string filepath) {
			
			// I have no idea how serializing an array of JSON objects is supposed to work with whatever
			// C# / unity version VAM uses...
			string actions = "[";
			for (int n = 0; n < data.Count; n++) {
				string action = "{\"at\":" + data[n].at + ", \"pos\":" + data[n].pos + "}";
				if (n < data.Count-1) {
					action += ",";
				}
				actions += action;
			}
			actions += "]";
			
			string outJson = "{\"version\":\"1.0\",\"inverted\":false,\"range\":100,\"actions\":" + actions + "}";
			
			SuperController.LogMessage("writing to: " + filepath + " with data count: " + data.Count);
			FileManagerSecure.WriteAllText(filepath, outJson);
		}
		
        protected void StopButtonCallback() {

            StopSerial();
            SuperController.LogMessage("Serial connection stopped");
			
			string timestamp = GetFileTimestamp();
			string baseFilename = SCREENSHOT_DIRECTORY + timestamp;
			string ext = ".funscript";
			
			writeFunscriptAxis(dataL0, baseFilename + ext);
			writeFunscriptAxis(dataL1, baseFilename + ".surge" + ext);
			writeFunscriptAxis(dataL2, baseFilename + ".sway" + ext);
			writeFunscriptAxis(dataR0, baseFilename + ".twist" + ext);
			writeFunscriptAxis(dataR1, baseFilename + ".roll" + ext);
			writeFunscriptAxis(dataR2, baseFilename + ".pitch" + ext);
			writeFunscriptAxis(dataA1, baseFilename + ".suck" + ext);
        }
        protected void SaveButtonCallback() {
            string sendString;
            if(serial != null && serial.IsOpen) {
                SuperController.LogMessage("Saving axis ranges to T-Code Device:");
                for (int n = 0; n < axesList.Count; n++) {
                    sendString = axesList[n].GetTCodeSaveLimits();
                    SuperController.LogMessage("[Out]: " + sendString);
                    lock (serialLock) {
                        serial.WriteLine(sendString + "\n");
                    }
                }
            }
        }
        protected void LoadButtonCallback() {
            string sendString;
            if(serial != null && serial.IsOpen) {
                SuperController.LogMessage("Identify available T-Code axes:");
                SuperController.LogMessage("[Out]: D2");
                lock (serialLock) {
                    serial.WriteLine("D2 \n");
                }
            }
        }
        protected void FemaleChooserCallback(string female) {
            SuperController.LogMessage(female + " selected.");
        }
        protected void MaleChooserCallback(string male) {
            SuperController.LogMessage(male + " selected.");
        }
        
        
        
        // Serial functions
        // Function to start serial
        void StartSerial() {
            if (portSelected != "None") {
                // Add extra characters for COM ports > 9
                if (portSelected.Substring(0,3) == "COM") {
                    if (portSelected.Length != 4) {
                        portSelected = "\\\\.\\" + portSelected;
                    }
                }
                
                // Open the serial connection
                serial = new SerialPort(portSelected, baudRate);
                serial.Open();
                serial.DtrEnable = true;
                serial.ReadTimeout = 10;
                serialIsStarted = true;

				// Start serial thread
                ThreadPool.QueueUserWorkItem(delegate {
                    while (serialIsStarted) {
                        Thread.Sleep(100);
                        if (serial == null || !serial.IsOpen) {
                            return;
                        }

                        // Read incoming serial one line at a time
                        string inString = null;
                        try {
                            lock (serialLock) {
                                inString = serial.ReadLine();
                            }
                        } catch (TimeoutException) {
                            continue;
                        }

                        // Print it to the log if this is active
                        if (logSerialIn.val && inString != null) { SuperController.LogMessage("[In]: " + inString); }
                        // Send each line for interpretation
                        InterpretInput(inString);
                    }
                });
            }
        }
        // Function to stop serial
        void StopSerial() {
            serialIsStarted = false;
            if(serial != null && serial.IsOpen) {
                lock (serialLock) {
                    serial.Close();
                }
            }
        }
		
		// Take a TCode string and convert to funscript-compatible value
		// The string is pretty easy to parse + already has been through the limit filtering
		void convertToFunscript(string channel, string tcodeOut) {
			int converted = 0;

			try {
				// remove channel data from TCode input
				string sub = tcodeOut.Substring(2);
				// Parse as integer and convert from 0...1 to 0...100
				converted = (int)Math.Round(float.Parse(sub) / 10);
			} catch (FormatException) {
				// This will happen for things like magnitudes and speed, ie. "L020S10"
				SuperController.LogMessage("Failed converting: " + tcodeOut);
				return;
			}

			if (converted < 0) {
				converted = 0;
			} else if (converted > 99) {
				converted = 99;
			}
			RecordData d = new RecordData(converted, (int)(frameCount * FRAME_TO_TIME));

			switch (channel) {
				case "L0":
					dataL0.Add(d);
					break;
				case "L1":
					dataL1.Add(d);
					break;
				case "L2":
					dataL2.Add(d);
					break;
				case "R0":
					dataR0.Add(d);
					break;
				case "R1":
					dataR1.Add(d);
					break;
				case "R2":
					dataR2.Add(d);
					break;
				case "A1":
					dataA1.Add(d);
					break;
			}
		}
		
        // Function to send device position updates by serial
        void SendPulse() {
            var sendStringBuilder = new StringBuilder();
            var serialReportBuilder = new StringBuilder();
            serialReportBuilder.AppendLine("T-code:");
            // Send one channel sequentially regardless of no change
            sendThisChannelAnyway++;
			frameCount++;
            if (sendThisChannelAnyway >= axesList.Count) { sendThisChannelAnyway = 0; }
            // For each channel
            for (int n = 0; n < axesList.Count; n++) {
                // Send anyway if it's this channel's turn
                bool suppress = (sendThisChannelAnyway != n);
                // If suppress not set, send anyway
                if (!suppressRepeats.val) { suppress = false;}
                // Acquire T-Code command for this channel
				string tcodeOut = axesList[n].GetTCode(axesList[n].position,0,suppress);
                sendStringBuilder.Append(tcodeOut);
                serialReportBuilder.Append(axesList[n].lastTCode);
				
				// record if data changed or user selected to record repeats
				if (tcodeOut.Length > 0 || suppress == false) {
					convertToFunscript(axesList[n].channel, tcodeOut);
				}
            }

            serialReportBuilder.AppendLine("\n[Out]:");
            serialReportBuilder.Append(sendStringBuilder);
            sendStringBuilder.AppendLine("A90");
            
            serialReport.val = serialReportBuilder.ToString();
            var message = sendStringBuilder.ToString();
			
			if (serial != null && serial.IsOpen) {
				ThreadPool.QueueUserWorkItem(delegate {
					lock (serialLock) {
						serial.WriteLine(message);
					}
				});
			}
        }

        
        // Function to close plugin
        void OnDestroy() {
            StopSerial();
        }
        
    }
    
	[Serializable]
	public class RecordData {
		// Milliseconds since start of recording
		public int at;

		// Value of this axis, from 0 to 100
		public int pos;
		
		public RecordData(int d, int t) {
			pos = d;
			at = t;
		}
	}
    
    // TCode Axis Class
    public class TCodeAxis {
        // Properties
        public string channel;
        public string name;
        public bool symmetric;
        
        // Output parameters
        public float position;
        public string lastTCode;
        
        // Interface parameters
        public JSONStorableFloat rangeSliderA;
        public JSONStorableFloat rangeSliderB;
        public JSONStorableFloat adjust;
        public JSONStorableBool invert;
        public JSONStorableBool manualBool;
        public JSONStorableFloat manual;
        
        // Setup function
        public TCodeAxis(string setChannel, string setName, bool setSymmetric) {
            channel = setChannel.Substring(0,2);
            name = setName;
            symmetric = setSymmetric;
            
            // Set position to default
            position = 0.5f;
            if (channel.Substring(0,1) == "V") { position = 0f; };
            
            // Format output range sliders
            if (!symmetric) {
                rangeSliderA = new JSONStorableFloat("'" + channel + "' (" + name + ") Max", 1f, 0.5f, 1f, true, true);
                rangeSliderB = new JSONStorableFloat("'" + channel + "' (" + name + ") Min", 0f, 0.0f, 0.5f, true, true);
            } else {
                rangeSliderA = new JSONStorableFloat("'" + channel + "' (" + name + ") Max (+/-)", 0.5f, 0f, 0.5f, true, true);
                rangeSliderB = new JSONStorableFloat("'" + channel + "' (" + name + ") Centre", 0.50f, 0.0f, 1f, true, true);
            }
            
            // Format axis inverter
            invert = new JSONStorableBool("Invert '" + channel + "' (" + name + ")", false);
            
            // Format override controls
            manualBool = new JSONStorableBool("Override '" + channel + "' (" + name + ")", false);
            manual = new JSONStorableFloat("'" + channel + "' (" + name + ") Set", 0.5f, 0f, 1f, true, true);
        }
        
        // Function to create a T-Code string for this axis
        public string GetTCode(float input, int ramp, bool suppressRepeats) {
            float inputLimited, inputScaled;
            // If override, use the override value
            if (manualBool.val) {
                inputScaled = manual.val;
            } else {
                // Use input value; limit input to 0-1 range
                if (input >= 1f) { inputLimited = 1f; }
                else if (input <= 0f) { inputLimited = 0f; }
                else { inputLimited = input; }
                // Scale input according to range limits
                if (!symmetric) {
                    inputScaled = rangeSliderB.val + inputLimited*(rangeSliderA.val-rangeSliderB.val);
                } else {
                    inputScaled = rangeSliderB.val + (inputLimited-0.5f)*2*rangeSliderA.val;
                }
            }
            inputScaled *= 1000;
            string output;
            if (inputScaled >= 999f) {
                output = channel + "999";
            } else if (inputScaled >= 1f) {
                output = channel + inputScaled.ToString("000");
            } else {
                output = channel + "000";
            }
            
            // Live command?
            if (ramp == 0) {
                output += " ";
                // Option to suppress a live T-Code command if it's the same as the last one sent
                if (suppressRepeats && (output == lastTCode)) {
                    output = "";
                } else {
                    lastTCode = output;
                }
            } else {
                // Add ramp to output
                output += "S" + ramp.ToString() + " ";
                lastTCode = output;
            }
                
            return output;
        }
        
        // Function to create a '$' T-Code string
        // This stores the limit preferences to the T-Code device
        public string GetTCodeSaveLimits() {
            string output;
            float min,max;
            if (!symmetric) {
                max = 10000*rangeSliderA.val;
                min = 10000*rangeSliderB.val;
            } else {
                max = 10000*(rangeSliderB.val+rangeSliderA.val);
                min = 10000*(rangeSliderB.val-rangeSliderA.val);
            }
            // '$' to specify range, and identify channel
            output = "$" + channel + "-";
            // Minimum value
            if (min >= 9999f) {
                output += "9999-";
            } else if (min >= 1f) {
                output += min.ToString("0000") + "-";
            } else {
                output += "0000-";
            }
            // Maximum value
            if (max >= 9999f) {
                output += "9999";
            } else if (max >= 1f) {
                output += max.ToString("0000");
            } else {
                output += "0000";
            }
                
            return output;
        }

        // Function to set this axis' limits from incoming serial
        public void SetLimitsFromString(string inString) {
            bool valid = true;
            int min = 0;
            float minF = 0f;
            int max = 1000;
            float maxF = 1f;
            int marker = inString.IndexOf(" ");
            if (marker < 0) { valid = false; }
            if (valid) {
                inString = inString.Substring(marker+1);
                marker = inString.IndexOf(" ");
                if (marker < 0) { valid = false; }
            }
            if (valid) {
                min = int.Parse(inString.Substring(0,marker));
                inString = inString.Substring(marker+1);
                marker = inString.IndexOf(" ");
                if (marker < 0) { valid = false; }
            }
            if (valid) {
                max = int.Parse(inString.Substring(0,marker));
                minF = 0.0001f*(float)min;
                if (minF >= 1f) {minF = 1f;} else if (minF <= 0f) {minF = 0f;}
                maxF = 0.0001f*(float)max;
                if (maxF >= 1f) {maxF = 1f;} else if (maxF <= 0f) {maxF = 0f;}
                if (!symmetric) {
                    rangeSliderA.val = maxF;
                    rangeSliderB.val = minF;
                } else {
                    rangeSliderA.val = 0.5f*(maxF-minF);
                    rangeSliderB.val = 0.5f*(maxF+minF);
                } 
            }

        }
        
    }
    
    
    // Class for the females - Credit VAMRobot
    public class Female {
        public string name;
        public Vector3[] vagina;
        public Vector3[] mouth;
        public Vector3 vaginaVector;
        public Vector3 mouthVector;
        public string[] vaginaTriggers = { "LabiaTrigger", "VaginaTrigger", "DeepVaginaTrigger", "DeeperVaginaTrigger" };
        public string[] mouthTriggers = { "LipTrigger", "MouthTrigger", "ThroatTrigger" };
        public bool[] vaginaInsertionFlags;
        public bool[] mouthInsertionFlags;
        public bool[] vaginaTipInsertionFlags;
        public bool[] mouthTipInsertionFlags;
        public Rigidbody[] vaginaRigidbodies;
        public Rigidbody[] mouthRigidbodies;
        public List<Rigidbody[]> femaleRigidbodies;

        public Female(string female) {
            name = female;
            vagina = new Vector3[vaginaTriggers.Length];
            mouth = new Vector3[mouthTriggers.Length];
            vaginaRigidbodies = new Rigidbody[vaginaTriggers.Length];
            vaginaInsertionFlags = new bool[vaginaTriggers.Length];
            vaginaTipInsertionFlags = new bool[vaginaTriggers.Length];

            for (int i = 0; i < vaginaTriggers.Length; i++) {
                vaginaRigidbodies[i] = SuperController.singleton.GetAtomByUid(female).rigidbodies.First(rb => rb.name == vaginaTriggers[i]);
                vaginaInsertionFlags[i] = false;
            }

            mouthRigidbodies = new Rigidbody[mouthTriggers.Length];
            mouthInsertionFlags = new bool[mouthTriggers.Length];
            mouthTipInsertionFlags = new bool[mouthTriggers.Length];

            for (int i = 0; i < mouthTriggers.Length; i++) {
                mouthRigidbodies[i] = SuperController.singleton.GetAtomByUid(female).rigidbodies.First(rb => rb.name == mouthTriggers[i]);
                mouthInsertionFlags[i] = false;
            }
        }

        public void update() {
            for (int i = 0; i < vaginaTriggers.Length; i++) {
                vagina[i] = vaginaRigidbodies[i].position;
            }

            vaginaVector = vagina[0] - vagina[vaginaTriggers.Length - 1];
        
            for (int i = 0; i < mouthTriggers.Length; i++) {
                mouth[i] = mouthRigidbodies[i].position;
            }

            mouthVector = mouth[0] - mouth[mouthTriggers.Length - 1];
        }
    }

    // Class for the males - Credit VAMRobot
    public class Male {
        public string name;
        public Vector3[] penis;
        public Vector3 penisVector;
        public string[] penisNames = { "AutoColliderGen1Hard", "AutoColliderGen2Hard", "AutoColliderGen3aHard", "AutoColliderGen3bHard" };
        public GameObject[] penisColliders;
        public Rigidbody abdomenControl;
        public Vector3 abdomen;

        public Male(string male) {
            name = male;
            penis = new Vector3[penisNames.Length];
            penisColliders = new GameObject[penisNames.Length];

            for (int i = 0; i < penisNames.Length; i++) {
                foreach (Collider c in SuperController.singleton.GetAtomByUid(male).gameObject.GetComponentsInChildren<Collider>()) {
                    if (c.gameObject.name == penisNames[i]) {
                        penisColliders[i] = c.gameObject;
                    }
                }
            }

            abdomenControl = new Rigidbody();
            abdomenControl = SuperController.singleton.GetAtomByUid(male).rigidbodies.First(rb => rb.name == "abdomenControl");
        }

        public void update() {
            for (int i = 0; i < penisNames.Length; i++) {
                penis[i] = penisColliders[i].transform.position;
            }

            penisVector = penis[penisNames.Length - 1] - penis[0];
            abdomen = abdomenControl.transform.position;
        }
    }
}