Repository: AlabamaASRL/asset_asrl (language: python)

Issue:
Merge master to AutoScaling



Discussion / hints:

Fix patch:
diff --git a/.github/workflows/pypi-wheels.yml b/.github/workflows/pypi-wheels.yml
index 846ec9a4..70e259b7 100644
--- a/.github/workflows/pypi-wheels.yml
+++ b/.github/workflows/pypi-wheels.yml
@@ -61,7 +61,7 @@ jobs:
          -DCMAKE_LINKER:FILEPATH="C:/Program Files/LLVM/bin/lld-link.exe"
 
       - name: "Build Project"
-        run: cmake --build build --target asset pypiwheel --config Release -- -j1
+        run: cmake --build build --target asset pypiwheel --config Release -- -j3
 
       - name: 'Upload Wheels'
         uses: actions/upload-artifact@v3
@@ -105,7 +105,7 @@ jobs:
             run: cmake -S . -B build -G "Unix Makefiles" -DCMAKE_BUILD_TYPE=Release -DPIP_INSTALL=True -DBUILD_ASSET_WHEEL=True
 
           - name: "Build Project"
-            run: cmake --build build --target asset pypiwheel --config Release -- -j1
+            run: cmake --build build --target asset pypiwheel --config Release -- -j2
 
           - name: Audit Wheel Test
             run:  auditwheel show build/pypiwheel/asset_asrl/dist/*.whl
@@ -154,7 +154,7 @@ jobs:
             run: cmake -S . -B build -G "Unix Makefiles" -DCMAKE_BUILD_TYPE=Release -DPIP_INSTALL=True -DBUILD_ASSET_WHEEL=True
 
           - name: "Build Project"
-            run: cmake --build build --target asset pypiwheel --config Release -- -j1
+            run: cmake --build build --target asset pypiwheel --config Release -- -j2
 
           - name: Audit Wheel Test
             run:  auditwheel show build/pypiwheel/asset_asrl/dist/*.whl
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 756526e7..63b0f6ed 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.16)
-project(ASSET VERSION 0.4.0 LANGUAGES CXX)
+project(ASSET VERSION 0.5.0 LANGUAGES CXX)
 set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 
 set( CMAKE_MESSAGE_LOG_LEVEL "STATUS" )
@@ -78,9 +78,9 @@ if(BUILD_ASSET_WHEEL)
     # Windows runs out of ram with LINK_TIME_OPT on gh-actions
     if(NOT WIN32)
         list(APPEND RELEASE_FLAGS "-mtune=skylake")
-        set(LINK_TIME_OPT TRUE)   ### DO LTO - Recommended for full release
-        set(CLANG_MAX_INLINE_DEPTH 300)
     endif()
+    set(LINK_TIME_OPT TRUE)   ### DO LTO - Recommended for full release
+    set(CLANG_MAX_INLINE_DEPTH 400)
     
 else()
     list(APPEND RELEASE_FLAGS "-march=native")
@@ -116,6 +116,7 @@ if(LINK_TIME_OPT)
         if(BUILD_ASSET_WHEEL)
             ## Do full LTO for CI builds since it is single threaded anyway
             list(APPEND RELEASE_FLAGS "-flto=full")
+            list(APPEND RELEASE_FLAGS "/opt:lldltojobs=1")
         else()
             list(APPEND RELEASE_FLAGS "-flto=thin")
             list(APPEND RELEASE_FLAGS "/opt:lldltojobs=8")
diff --git a/asset_asrl/OptimalControl/ODEBaseClass.py b/asset_asrl/OptimalControl/ODEBaseClass.py
index 2af6553a..1ad797e8 100644
--- a/asset_asrl/OptimalControl/ODEBaseClass.py
+++ b/asset_asrl/OptimalControl/ODEBaseClass.py
@@ -1,9 +1,12 @@
 import asset as _asset
 import inspect
+import numpy as np
+import numbers
+
 
 class ODEBase:
  
-    def __init__(self,odefunc,Xvars,Uvars = None,Pvars = None):
+    def __init__(self,odefunc,Xvars,Uvars = None,Pvars = None,Vgroups = None):
 
         mlist = inspect.getmembers(_asset.OptimalControl)
         
@@ -37,8 +40,169 @@ def __init__(self,odefunc,Xvars,Uvars = None,Pvars = None):
             else:
                 self.ode = _asset.OptimalControl.ode_x.ode(odefunc,Xvars)
                 
+        if(Vgroups!=None):
+            self.add_Vgroups(Vgroups)
+                
+    
+    def _make_index_set(self,idxs):
+        
+        if(hasattr(idxs, 'input_domain') and hasattr(idxs, 'vf') and hasattr(idxs, 'is_linear')):
+            ## Handle Vector Functions
+            input_domain = idxs.input_domain()
+            idxstmp = []
+            for i in range(0,input_domain.shape[1]):
+                start = input_domain[0,i]
+                size = input_domain[1,i]
+                idxstmp+= list(range(start,start+size))
+            return idxstmp
+        elif(isinstance(idxs, (int,np.int32,np.intc))):
+            return [idxs]
+        elif(hasattr(idxs, '__iter__') and not isinstance(idxs, str)):
+            if(len(idxs)==0):
+                raise Exception("Index list is empty")
+            idxtmp = []
+            for idx in idxs:
+                idxtmp+=self._make_index_set(idx)
+            return idxtmp
+        else:
+            raise Exception("Invalid index: {}".format(str(idxs)))
+        
+    def add_Vgroups(self,Vgroups):
+        for name in Vgroups:
+            idxs = self._make_index_set(Vgroups[name])
+            if(isinstance(name, str)):
+               self.ode.add_idx(name,idxs)            
+            elif(hasattr(name, '__iter__')):
+                for n in name:
+                    self.ode.add_idx(n,idxs)
+    
+    def make_units(self,**kwargs):
+        """
+        Makes a vector of non-dim units from variable group names and unit values
+
+        Parameters
+        ----------
+        **kwargs : float,list,ndarray
+            Keyword list of the names of the variable groups and corresponding units.
+        Returns
+        -------
+        units : np.ndarray
+            Vector of units.
+        """
+        units = np.ones((self.XtUPVars()))
+        
+        for key, value in kwargs.items():
+            idx = self.idx(key)
+            
+            if(isinstance(value, numbers.Number)):
+                units_t = np.ones((len(idx)))*value
+            elif(hasattr(value, '__iter__') and not isinstance(value, str)):
+                units_t = value
+            else:
+                raise Exception("Invalid unit: {}".format(str(value)))
+
+                
+            for i in range(0,len(idx)):
+                units[idx[i]] = units_t[i]
+        
+        return units
+    
+    def make_input(self,**kwargs):
+        """
+        Makes an ODE input vector. Assigns sub-components to variable group names
+        and assembles them into  single vector with proper ordering
+
+        Parameters
+        ----------
+        **kwargs : float,list,ndarray
+            Keyword list of the sub-vectors of the full input.
+
+        Returns
+        -------
+        state : np.ndarray
+            ODE input vector.
+
+        """
+        state = np.zeros((self.XtUPVars()))
+        for key, value in kwargs.items():
+            idx = self.idx(key)
+            
+            if(isinstance(value, numbers.Number)):
+                state_t = np.ones((len(idx)))*value
+            elif(hasattr(value, '__iter__') and not isinstance(value, str)):
+                state_t = value
+            else:
+                raise Exception("Invalid unit: {}".format(str(value)))
+
+            for i in range(0,len(idx)):
+                state[idx[i]] = state_t[i]
+        return state
+    
+    
+    
+    def get_vars(self,names,source,retscalar = False):
+        """
+        Retrieve variables from a ODE input vector or trajectory(2darray/listoflists)
         
+        Parameters
+        ----------
+        names : str,list
+            Name,list of names/indices of the variables or sub-vectors to retrieve.
+        source : np.ndarray,list
+            Source vector or 2d-array for retreiving variables.
+        retscalar : bool, optional
+            Return a scalar instead of vector if result has only one element. 
+            The default is False.
+
+        Returns
+        -------
+        np.ndarray
+            Concatenated array of the specified variables from source
+        """
+        #############################################################
         
+        idxs = []
+        if(hasattr(names, '__iter__') and not isinstance(names, str)):
+            for name in names:
+                if(isinstance(name, str)): 
+                    idxs+= list(self.idx(name))
+                elif(isinstance(name, (int,np.int32,np.intc))):
+                    idxs.append(name)
+                else:
+                    raise Exception("Invalid name specifier: {}".format(str(name)))
+        elif(isinstance(names, str)):
+            idxs+=list(self.idx(names))
+        else:
+            raise Exception("Invalid name specifier: {}".format(str(names)))
+            
+            
+        #############################################################
+
+        if(hasattr(source, '__iter__') and isinstance(source[0],numbers.Number)):
+            # Its a single vector
+            output = np.zeros(len(idxs))
+            for i,idx in enumerate(idxs):
+                output[i] = source[idx]
+                
+            if(len(output)==1 and retscalar):
+                return output[0]
+            else:
+                return output
+            
+        else: 
+            # Its a trajectory, nd array or list of lists/arrays
+            size = len(source)
+            output = np.zeros((size,len(idxs)))
+            for j in range(0,size):
+                for i,idx in enumerate(idxs):
+                    output[j][i] = source[j][idx]
+            
+            return output
+            
+            
+    
+    def idx(self,Vname):
+        return self.ode.idx(Vname)
     def phase(self,*args):
         return self.ode.phase(*args)
     def integrator(self,*args):
diff --git a/asset_asrl/ctpl-apache2.txt b/asset_asrl/ctpl-apache2.txt
new file mode 100644
index 00000000..9e40fa18
--- /dev/null
+++ b/asset_asrl/ctpl-apache2.txt
@@ -0,0 +1,194 @@
+License for ctpl thread pool included in src/Utils/ThreadPool.h. The code has been modified from its original version by renaming the original thread_pool class to
+ThreadPool and adding the push_member function to the class.
+
+Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+
+   Copyright (C) 2014 by Vitaliy Vitsentiy
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
\ No newline at end of file
diff --git a/asset_asrl/eigen-mpl2.txt b/asset_asrl/eigen-mpl2.txt
index 14e2f777..f5b14762 100644
--- a/asset_asrl/eigen-mpl2.txt
+++ b/asset_asrl/eigen-mpl2.txt
@@ -1,3 +1,8 @@
+The file src/Solvers/PardisioInterface.h is based on the PardisoInterface.h file from the Eigen library (https://eigen.tuxfamily.org/). 
+The Eigen library is licensed under the Mozilla Public License Version 2.0 (https://www.mozilla.org/en-US/MPL/2.0/). The text of the Mozilla Public License Version 2.0 is included below.
+Note that the MPL2 license is only applied to src/Solvers/PardisioInterface.h and not the rest of the project. Changes include the addition of several member functions to access  internal PARDISO parameters, and additional
+methods to perform phases of the factorization without making unnecessary copies of the input matrix.
+
 Mozilla Public License Version 2.0
 ==================================
 
diff --git a/asset_asrl/issl.pdf b/asset_asrl/intel-issl.pdf
similarity index 100%
rename from asset_asrl/issl.pdf
rename to asset_asrl/intel-issl.pdf
diff --git a/asset_asrl/keplerprop.txt b/asset_asrl/keplerprop.txt
new file mode 100644
index 00000000..86e5fb2d
--- /dev/null
+++ b/asset_asrl/keplerprop.txt
@@ -0,0 +1,26 @@
+Kepler propagator algorithm is based on the implementation found at
+https://www.mathworks.com/matlabcentral/fileexchange/35566-vectorized-analytic-two-body-propagator-kepler-universal-variables.
+License is included below.
+
+Copyright (c) 2012, Darin Koblick
+All rights reserved.
+
+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 OWNER 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.
\ No newline at end of file
diff --git a/asset_asrl/lambert.txt b/asset_asrl/lambert.txt
new file mode 100644
index 00000000..3c11778f
--- /dev/null
+++ b/asset_asrl/lambert.txt
@@ -0,0 +1,29 @@
+
+Izzo Lambert solver is partly based Rody Oldenhuis Matlab implementation https://github.com/rodyo/FEX-Lambert
+
+Copyright (c) 2018, Rody Oldenhuis
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+2. 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 OWNER 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.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of this project.
\ No newline at end of file
diff --git a/asset_asrl/rubber_types-mit.txt b/asset_asrl/rubber_types-mit.txt
new file mode 100644
index 00000000..fed8a1e5
--- /dev/null
+++ b/asset_asrl/rubber_types-mit.txt
@@ -0,0 +1,21 @@
+Rubber-Types -- Type-erasure with merged concepts
+Copyright (C) 2014 Andreas J. Herrmann
+
+Permission is hereby granted, free of charge, to any person obtaining
+a copy of this software and associated documentation files (the
+"Software"), to deal in the Software without restriction, including without
+limitation the rights to use, copy, modify, merge, publish, distribute,
+sublicense, and/or sell copies of the Software, and to permit persons to
+whom the Software is furnished to do so, subject to the following
+conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
diff --git a/doc/index.rst b/doc/index.rst
index 750deeb9..a305ac2b 100644
--- a/doc/index.rst
+++ b/doc/index.rst
@@ -38,7 +38,13 @@ Cite the following `paper <https://www.researchgate.net/publication/357567956_AS
 
 
 
-If you have questions, please email any of:
+If you have questions, please email any of the following or post an issue on the GitHub:
+
+    ajhouin@crimson.ua.edu
+
+    cgsandel@crimson.ua.edu
+
+    rsood@eng.ua.edu
 
     jbpezent@crimson.ua.edu
 
@@ -54,7 +60,7 @@ Open source development and distribution of ASSET is funded by NASA under Grant
 
 Copyright, Licensing, and Legal Notices
 #######################################
-ASSET is provided under the permissive Apache 2.0 license that can be found in the `LICENSE <https://github.com/AlabamaASRL/asset_asrl/blob/master/LICENSE.txt>`_ file on the Github repo.
+ASSET is distributed under the permissive Apache 2.0 license that can be found in the `LICENSE <https://github.com/AlabamaASRL/asset_asrl/blob/master/LICENSE.txt>`_ file on the Github repo.
 
 The license and copyright notices of ASSET's source and redistributable dependencies are shown below. The full text for each respective license can be found in the 
 `notices <https://github.com/AlabamaASRL/asset_asrl/tree/master/notices>`_ folder on the Github repo.
@@ -71,6 +77,9 @@ The license and copyright notices of ASSET's source and redistributable dependen
 
     rubber_types - MIT License : Copyright (C) 2014 Andreas J. Herrmann
 
+    kepler propagator - MIT License : Copyright (C) 2014 Darin Koblick
+
+    lambert solver - MIT License : Copyright (C) 2018 Rody Oldenhuis
 
 ..
     cpp/cpp_root.rst
diff --git a/doc/tutorials/Install.rst b/doc/tutorials/Install.rst
index 15d0f30b..755962a3 100644
--- a/doc/tutorials/Install.rst
+++ b/doc/tutorials/Install.rst
@@ -1,13 +1,23 @@
 Installing ASSET
 ================
 
-This tutorial will guide you through the complete setup process for installing and building ASSET from scratch on:
+In most cases we suggest simply installing ASSET via pip, which is as simple as running the following command in the console:
+
+.. code-block:: console
+
+	pip install asset-asrl
+
+We currently host Windows and Linux binaries for python 3.7-3.12 on PyPi.   
+
+
+If you are interested in building ASSET from source, please see the following sections for a step-by-step guide on how to do so on:
+
 * Windows
 * Linux
-* Docker
+* Docker (Linux)
 
-Windows Installation
---------------------
+Build from Source on Windows
+----------------------------
 
 In order to build ASSET on Windows the following dependencies are required:
 
@@ -136,8 +146,8 @@ Step-By-Step Guide
 
 
 
-Linux Installation
-------------------
+Build from Source on Linux
+--------------------------
 The dependencies for Linux installations are similar to that of Windows; however, we'll be using Visual Studio Code for our IDE:
 
 * `Visual Studio Code <https://code.visualstudio.com/download>`_
@@ -253,16 +263,35 @@ If it is desired to use an IDE other than Visual Studio Code, it is still requir
 Docker
 ------
 
-Want to skip installing all of the dependencies? ASSET can be installed through a Docker image. There's also great integrations of Docker and VS Code.
+The `dockerfiles <https://github.com/AlabamaASRL/asset_asrl/tree/master/dockerfiles>`_ folder in the repo contains docker files and supporting scripts for building asset_asrl .whl files for pip installation on linux systems.
+Two images are provided based on Ubuntu 18.04 and 20.04. Build wheels on 18.04 for better compatibility with older linux distros or 20.04 for better performance.
+
+
+To use, cd to the desired image's folder and build the image. You may optionally set the number of cores used building the image with the
+CMakeBuildJobs build argument.
+
+.. code-block:: console
+
+	cd Ubuntu18.04
+
+	docker build -t assetbuild . --build-arg CMakeBuildJobs=12
+	
+After building the image, start up a new container.
+
+.. code-block:: console
+
+	docker run -it assetbuild /bin/bash
+
+Once inside the container, run the BuildWheel.sh script to build a .whl file for a particular python version (-p), repo branch (-b).
+You may also set the number of jobs used when compiling with the -j argument. 
 
-Download the Docker/VS Code project template here and get to coding!
+.. code-block:: console
 
-https://github.com/jasonmeverett/asset-example-project
+	bash ~/BuildWheel.sh -b master -j 12 -p 3.10
 
-Developing Docker Images 
-^^^^^^^^^^^^^^^^^^^^^^^^
+After the build has completed, the packaged wheel file will be available in ~/asset_asrl/wheelhouse. You can then copy this file out of the container
+for installation on other systems.
 
-Interested in contributing to Docker/ASSET development? Check in-depth details in `Dockerfile` and `Dockerfile-dev` in the repository.
 
 
 
diff --git a/doc/tutorials/NewInterface.rst b/doc/tutorials/NewInterface.rst
new file mode 100644
index 00000000..9569f246
--- /dev/null
+++ b/doc/tutorials/NewInterface.rst
@@ -0,0 +1,286 @@
+.. _scale-guide
+
+======================
+Interface Enhancements
+======================
+Beginning in version 0.5.0, we have made significant improvements to the ODE, phase and OCP interfaces
+in order to simplify the definition of more complex optimal control problems. Note that these changes are backwards
+compatible and the previous tutorials and any existing code will continue to work as is. We will be updating all existing
+tutorials to reflect the new changes at a later date, but for now we will provide a brief overview of the new features. A subset of
+the existing examples have been updated to reflect the new interface and can be found in the examples/UpdatedInterface folder on the repo.
+
+
+ODE Component Naming
+####################
+
+The first major improvement is the ability to give string name aliases to the input arguments of an ODE in the constructor.
+These names may then be used in place of integer indexes when applying constraints and objectives to phases
+and optimal control problems. As an example, lets take a look at how this works for the ODE defined for the Delta-III
+launch example.
+
+Here, we define the ODE exactly as we normally would in terms of :code:`ODEArguments`. We can then add the different components of
+the arguments to a dictionary indexed by the name or names that we want to use as aliases. Multiple aliases for the same variable
+grouping can be used by indexing the dictionary with a tuple of string names. The variables of the alias can be specified with
+the list integer indices,the actual segment or element, or a list of segments and/or elements. After populating the dictionary,we pass it 
+to the :code:`Vgroups` (Variable groups) argument of :code:`ODEBase`'s constructor.
+
+.. code:: python
+    
+    class RocketODE(oc.ODEBase):
+        def __init__(self,T,mdot):
+            ####################################################
+            XtU  = oc.ODEArguments(7,3)
+        
+            R = XtU.XVec().head(3)
+            V = XtU.XVec().segment(3,3)
+            m = XtU.XVar(6)
+       
+            U = XtU.UVec()
+        
+            h       = R.norm() - Re
+            rho     = RhoAir * vf.exp(-h / h_scale)
+            Vr      = V + R.cross(np.array([0,0,We]))
+        
+            D       = (-0.5*CD*S)*rho*(Vr*Vr.norm())
+        
+            Rdot    =  V
+            Vdot    =  (-mu)*R.normalized_power3() + (T*U.normalized() + D)/m
+        
+            ode = vf.stack(Rdot,Vdot,-mdot)
+        
+            Vgroups = {}
+
+
+            Vgroups[("R","Position")]=R
+            #Vgroups[("R","Position")]=[0,1,2] # same as above
+
+            Vgroups[("V","Velocity")]=V
+            Vgroups[("U","ThrustVec")]=U
+
+            Vgroups["RV"] = [R,V]
+            #Vgroups["RV"] = [0,1,2,3,4,5] # same as above
+
+            Vgroups["Xt"] = XtU.XtVec()
+
+            Vgroups[("t","time")]=XtU.TVar()
+            Vgroups[("m","mass")]=m
+
+            ####################################################
+            super().__init__(ode,7,3,Vgroups = Vgroups)
+
+
+Having defined the variable groups for our ODE we can now use the string names or lists of string names
+anywhere where we could previously use integer indices or lists of indices. As an example, lets take a look at how this would modify the
+definition of the Delta-III launch vehicle optimal control problem below. For example, we can now simply use the alias :code:`"U"`, which we have
+defined to be our thrust vector, when placing an LUNorm bound on our control. Similarly, we can replace the :code:`range(0,8)` in the boundary value specification,
+with a list of string names specifying the different components of the states or with the combined alias :code:`"Xt"` of those same variables. Note that when specifying multiple names,
+the combined index vector is just the concatenation of sub index vectors, so the order matters.
+
+.. code:: python
+
+    #########################################
+    phase1 = ode1.phase(tmode,IG1,nsegs1)
+    phase1.setControlMode(cmode)
+
+    phase1.addLUNormBound("Path","U",.5,1.5)
+    ## Same as above
+    ## phase1.addLUNormBound("Path",[8,9,10],.5,1.5)
+
+    phase1.addBoundaryValue("Front",["R","V","m","t"],IG1[0][0:8])
+    ## Same as above
+    ##phase1.addBoundaryValue("Front","Xt",IG1[0][0:8])
+    ##phase1.addBoundaryValue("Front",range(0,8),IG1[0][0:8])
+
+
+    phase1.addLowerNormBound("Path","R",Re*.999999)
+
+    phase1.addBoundaryValue("Back","time",tf_phase1) ## Can drop brackets if scalar
+    ## Same as above
+    ## phase1.addBoundaryValue("Back",[7],[tf_phase1])
+
+
+    #########################################
+    phase2 = ode2.phase(tmode,IG2,nsegs2)
+    phase2.setControlMode(cmode)
+
+    phase2.addLowerNormBound("Path","R",Re)
+    phase2.addLUNormBound("Path","U",.5,1.5)
+
+    phase2.addBoundaryValue("Front","mass", m0_phase2)
+    phase2.addBoundaryValue("Back", "time" ,tf_phase2)
+
+    #########################################
+    phase3 = ode3.phase(tmode,IG3,nsegs3)
+    phase3.setControlMode(cmode)
+
+    phase3.addLowerNormBound("Path","R",Re)
+    phase3.addLUNormBound("Path","U",.5,1.5)
+    phase3.addBoundaryValue("Front","mass", m0_phase3)
+    phase3.addBoundaryValue("Back", "time" ,tf_phase3)
+
+    #########################################
+    phase4 = ode4.phase(tmode,IG4,nsegs4)
+    phase4.setControlMode(cmode)
+
+    phase4.addLowerNormBound("Path","R",Re)
+    phase4.addLUNormBound("Path","U",.5,1.5)
+    phase4.addBoundaryValue("Front","mass", m0_phase4)
+    phase4.addUpperVarBound("Back","time",tf_phase4)
+
+    phase4.addEqualCon("Back",TargetOrbit(at,et,istart,Ot,Wt),["R","V"])
+    ## Same as above
+    ## phase4.addEqualCon("Back",TargetOrbit(at,et,istart,Ot,Wt),"RV")
+
+    # Maximize final mass
+    phase4.addValueObjective("Back","mass",-1.0)
+
+    #########################################
+
+We can also now use these same string names when applying any link constraints and objectives to :code:`OptimalControlProblem` objects.
+So for the Delta-III example, we can modify the :code:`addForwardLinkEqualCon` as shown below. Note that you will need to define the string aliases in the ODE
+associated with each phase. Note however that the indices specified by a string name do not have to be the same in every phase/ODE linked
+(though they are in this case). This makes it much easier to enforce continuity between variables in each phase even if they
+have different indices. For example, in the old interface, models with different numbers of state variables would have different indices for time.
+Now, so long as the user names time say :code:`"t"` in both model definitions, then the call below will enforce continuity correctly. 
+
+.. code:: python
+
+    ocp = oc.OptimalControlProblem()
+    ocp.addPhase(phase1)
+    ocp.addPhase(phase2)
+    ocp.addPhase(phase3)
+    ocp.addPhase(phase4)
+
+
+
+    ## All phases continuous in everything but mass (var 6)
+    ocp.addForwardLinkEqualCon(phase1,phase4,["R","V","t","U"])
+
+    ## Same as above
+    ##ocp.addForwardLinkEqualCon(phase1,phase4,[0,1,2,3,4,5, 7,8,9,10])
+    
+
+Finally, the new string names or lists of names can also be used when applying specifying the inputs to control law for an ODE's integrator as shown below.
+
+.. code:: python
+
+        ode = RocketODE(T_phase1,mdot_phase1)
+
+        integ = ode.integrator(1.0,Args(3).normalized(),"V")
+
+
+    
+
+
+Auto-Scaling
+############
+
+The second major addition to the interface is automatic problem scaling from user defined canonical units. In the
+Delta-III and Shuttle tutorials we emphasized the importance of defining problems in non-dimensional units. This is typically
+done by defining a set of base length, mass, and time units and then redefining all constants and boundary conditions in this new 
+unit system. This is easy enough for simple problems like the Delta-III, but quickly becomes cumbersome for more complex scenarios.
+For that we reason, we have added interfaces to :code:`phase` and :code:`OptimalControlProblem` that will handle this non-dimensionalization behind the scenes
+and allow users to specify their problem in traditional units. To use this interface, the user must enable auto scaling and declare the canonical units
+associated with each ODE input variable for a phase. As shown below for the Delta-III example, we can specify the units using code:`phase.setUnits` 
+by passing a single vector with the same dimensions as the ODE's input vector, or if we defined names for our components, we can assign them by name.
+
+.. code:: python
+
+    phase1 = ode1.phase(tmode,IG1,nsegs1)
+    phase1.setControlMode(cmode)
+    
+    ## Enable AutoScaling, off by default
+    phase1.setAutoScaling(True)
+    
+    units = np.ones((11))
+    units[0:3]=Lstar
+    units[3:6]=Vstar
+    units[6]=Mstar
+    units[7]=Tstar
+    ## All others are one,i.e no auto-scaling
+    
+    phase1.setUnits(units)  # As a single vector
+    # Or
+    phase1.setUnits(R=Lstar,V=Vstar,t=Tstar,m=Mstar) 
+
+    
+    phase1.addLUNormBound("Path","U",.5,1.5)
+    phase1.addBoundaryValue("Front",["R","V","m","t"],IG1[0][0:8])
+
+    #. Continue definition
+    #.
+    #
+
+
+The specified canonical units will be used under the hood to non-dimensionalize any trajectory passed into the phase and any variables sent to the optimizer.
+From the units for the states and times, we can also uniquely determine a set of output scales for the transcription defect constraints that will make them equivalent
+to a problem that was non-dimensionalized by hand. 
+
+..  note:: 
+
+    When auto-scaling is enabled the mesh tolerance for adaptive mesh refinement refers to the scaled ODE system.
+
+
+However, since we don't track the physical units of functions, this is not possible for all other constraints and objectives added to phase.
+By default for all non-dynamics constraints and objectives, we compute a set of output scales that normalizes each row of
+the function's Jacobian at the initial guess for the problem. Alternatively, the user can override these scales manually. 
+All of this is controlled via an optional :code:`AutoScale`` argument that has been,
+added to all :code:`phase`` and :code:`OptimalControlProblem` :code:`.add###`` methods. As an example, lets take a look at a portion of the definition of the Delta-III problem again below.
+
+
+.. code:: python
+
+    ## AutoScale = "auto" if not specified
+    phase4.addBoundaryValue("Front","mass", m0_phase4)
+    phase4.addUpperVarBound("Back","time",tf_phase4)
+    # AutoScale=None, will turn it off for this constraint
+    phase4.addLUNormBound("Path","U",.5,1.5,AutoScale=None)
+
+    # Override the scale for this constraint
+    phase4.addLowerNormBound("Path","R",Re,AutoScale=1/Lstar)
+    phase4.addEqualCon("Back",TargetOrbit(at,et,istart,Ot,Wt),["R","V"],AutoScale = [1/Lstar,1.0,1.0,1.0,1.0])
+    
+    # Maximize final mass
+    phase4.addValueObjective("Back","mass",-1.0)
+
+
+
+By default :code:`AutoScale` is set to :code:`"auto"` for all constraints and objectives. This will work well in most cases, but can be overridden when the user can specify a better scale factor.
+Manual scales specified by a assigning a scalar or vector of scales to the :code:`AutoScale` parameter. These will multiply the output of the function whenever AutoScaling is enabled.
+For example, for the bound on :code:`"R"`, we know that the units of the output will have dimensions of length, so it is reasonable to set the AutoScale variable to :code:`1/Lstar`.
+Similarly, for the :code:`TargetOrbit` constraint, we know that the first component of the output(semi-major axis) has dimensions of length and all others are already non-dimensional.
+In that case, we can manually specify the output scale of the first component and then leave the others set to 1.0.
+
+
+When adding multiple phases to an :code:`OptimalControlProblem` we should also enable AutoScaling for the :code:`ocp` object as well. This will enable AutoScaling
+on all link constraints and objectives between phases. It should also be noted that units do not have to be the same for all phases in an :code:`ocp`.
+As with phases, the optional :code:`AutoScale` parameter on all link constraints and objectives can be overridden
+with custom scales if necessary.  
+
+.. code:: python
+
+    ocp = oc.OptimalControlProblem()
+    ocp.addPhase(phase1)
+    ocp.addPhase(phase2)
+    ocp.addPhase(phase3)
+    ocp.addPhase(phase4)
+    
+    # Enable AutoScaling for the OCP,and all constitiuent phases currently in ocp
+    ocp.setAutoScaling(True,True)
+    ocp.setAdaptiveMesh(True)  
+    
+    
+    for phase in ocp.Phases:
+        phase.setUnits(R=Lstar,V=Vstar,t=Tstar,m=Mstar)
+        phase.setMeshTol(1.0e-6)
+        phase.setMeshErrorCriteria('max')
+        phase.setMeshErrorEstimator('integrator')  
+
+
+    ## Each Phase does not have to have the same AutoScale units even if its the same ODE
+    phase4.setUnits(R=2*Lstar,V=Vstar,t=.8*Tstar,m=Mstar)
+
+    ## Can override the AutoScale for any link constraints and objectives as well
+    ocp.addForwardLinkEqualCon(phase1,phase4,["R","V","t","U"],AutoScale="auto")
+
+	
\ No newline at end of file
diff --git a/doc/tutorials/tutorials.rst b/doc/tutorials/tutorials.rst
index 8cbc882a..9ec40a87 100644
--- a/doc/tutorials/tutorials.rst
+++ b/doc/tutorials/tutorials.rst
@@ -16,6 +16,7 @@ New readers should begin at the beginning, since it's important to understand th
     OptimalControlProblem
     PSIOPT
     AutoMeshGuide
+    NewInterface
     OptimalControlUtilites
     
 
diff --git a/examples/CartPole.py b/examples/CartPole.py
index 5983b2f7..abd6a99c 100644
--- a/examples/CartPole.py
+++ b/examples/CartPole.py
@@ -4,6 +4,8 @@
 import seaborn as sns    # pip install seaborn if you dont have it
 import matplotlib.animation as animation
 
+
+
 vf = ast.VectorFunctions
 oc = ast.OptimalControl
 Args = vf.Arguments
diff --git a/examples/UpdatedInterface/BettsLowThrust.py b/examples/UpdatedInterface/BettsLowThrust.py
new file mode 100644
index 00000000..a0bf2cbd
--- /dev/null
+++ b/examples/UpdatedInterface/BettsLowThrust.py
@@ -0,0 +1,523 @@
+import numpy as np
+import asset_asrl as ast
+import asset_asrl.VectorFunctions as vf
+import asset_asrl.OptimalControl as oc
+from asset_asrl.VectorFunctions import Arguments as Args
+
+import matplotlib.pyplot as plt
+from asset_asrl.OptimalControl.MeshErrorPlots import PhaseMeshErrorPlot
+import time
+from mpl_toolkits.basemap import Basemap
+import seaborn as sns    # pip install seaborn if you dont have it
+from matplotlib import ticker
+
+
+
+
+'''
+Low-Thrust Orbit Transfer taken from example 6 on page 265 of
+Betts, J.T. Practical methods for Optimal Control and Estimation Using Nonlinear Programming, Cambridge University Press, 2009
+
+Optimizes a low thrust tranfer from LEO to MEO with MEE dynamics and Zonal Harmonics
+'''
+
+g0 = 32.174 
+W  = 1  #lb
+mu_e = 1.407645794e16
+
+
+Re     = 20925662.73             
+mdot   = (4.446618e-3/(450*g0))  
+mu     = 1.407645794e16                 
+Thrust = 4.446618e-3             
+Isp = 450                        
+gs = g0
+J2 = 1082.639e-6
+J3 = -2.565e-6
+J4 = -1.608e-6
+
+pt0 = 21837080.052835
+ptf = 40007346.015232
+ht0 = -0.25396764647494
+
+Lstar = 20925662.73     ## feet
+Tstar = Lstar/np.sqrt(mu/Lstar)
+Mstar = W/g0         ## slugs
+Fstar   = Mstar*Lstar/(Tstar**2)
+
+
+
+
+#######################Helper functions for plotting ,ignore #################
+def normalize(x): return np.copy(x)/np.linalg.norm(x)
+
+
+def MEEToCart(Traj):
+    Itraj=[]
+    f=MEECartFunc(mu)
+    for T in Traj:
+        Tmp = np.copy(T)
+        Xcart = f.compute(np.copy(T[0:6]))
+        Ucart = RTNtoCart(np.copy(Xcart[0:3]),np.copy(Xcart[3:6]), np.copy(T[7:10]))
+        Tmp[0:6]=Xcart
+        Itraj.append(Tmp)
+    return Itraj
+
+def RTNtoCart(r, v, u):
+    r = r/np.linalg.norm(r)
+    n = np.cross(r, v) #/ np.linalg.norm(np.cross(r, v))
+    n = normalize(n)
+    t = np.cross(n, r)
+
+    Rot = np.zeros((3, 3))
+    Rot[:, 0] = r
+    Rot[:, 1] = normalize(t)
+    Rot[:, 2] =n
+    
+    return np.dot(Rot, u)
+
+def Zlevels(Traj,zaxis=2):
+    
+    GP = []
+    GN = []
+    ns = [0]
+    for i in range(2,len(Traj)):
+        Tl = Traj[i-1]
+        T  = Traj[i]
+        switch = T[zaxis]/Tl[zaxis]
+        
+        if(switch<0):
+            ns.append(i)
+    ns.append(len(Traj)+1)
+    for i in range(0,len(ns)-1):
+        Tmp = Traj[ns[i]:ns[i+1]+1]
+        
+        if(Traj[ns[i]+1][zaxis]>0):
+            GP.append(Tmp)
+        else:
+            GN.append(Tmp)
+    return GP,GN
+
+def Plot(Traj):
+    
+    color = 'k'
+    
+    rs = 6370997.0
+    fig = plt.figure()
+    
+    axu = plt.subplot(211)
+    ax0 = plt.subplot(234)
+    ax1 = plt.subplot(235)
+    ax2 = plt.subplot(236)
+
+    axs = [ax0,ax1,ax2]
+    
+    ms = []
+    m1 = Basemap(projection='ortho',lon_0=0,lat_0=90,resolution='l',anchor='SW',ax=axs[0],suppress_ticks=False)
+    m2 = Basemap(projection='ortho',lon_0=0,lat_0=0,resolution='l',anchor='SW',ax=axs[0],suppress_ticks=False)
+    m3 = Basemap(projection='ortho',lon_0=90,lat_0=0,resolution='l',anchor='SW',ax=axs[0],suppress_ticks=False)
+
+    ms =[m1,m2,m3]
+
+    @ticker.FuncFormatter
+    def major_formatter(x, pos):
+         t = (x - rs)/rs
+         return f'{t:.1f}'
+     
+    views1 = [2,1,0]
+    views2 = [[0,1],[0,2],[1,2]]
+    
+    for i in range(0,3):
+        
+        ZPs,ZNs = Zlevels(np.copy(Traj),zaxis=views1[i])
+        
+        if(views1[i]==1):
+            tmp = ZPs
+            ZPs = ZNs
+            ZNs = tmp
+        
+        for ZP in ZPs:
+            T = np.array(ZP).T*.3048
+            axs[i].plot(T[views2[i][0]]+rs,T[views2[i][1]]+rs,zorder=10,color=color)
+            
+        for ZP in ZNs:
+            T = np.array(ZP).T*.3048
+            axs[i].plot(T[views2[i][0]]+rs,T[views2[i][1]]+rs,zorder=2,color=color)
+            
+        axs[i].xaxis.set_major_formatter(major_formatter)
+        axs[i].yaxis.set_major_formatter(major_formatter)
+
+        axs[i].xaxis.set_major_locator(ticker.MultipleLocator(rs))
+        axs[i].yaxis.set_major_locator(ticker.MultipleLocator(rs))
+
+        axs[i].grid(True)
+        
+        m = ms[i]
+        m.drawcoastlines(ax=axs[i],linewidth=.25,zorder=5)
+        m.fillcontinents(color='coral',lake_color='aqua',ax=axs[i],zorder=5)
+        m.drawparallels(np.arange(-90.,120.,30.),ax=axs[i],zorder=5)
+        m.drawmeridians(np.arange(0.,420.,60.),ax=axs[i],zorder=5)
+        m.drawmapboundary(fill_color='aqua',ax=axs[i],zorder=2)
+
+
+    axs[0].set_xlabel(r"$X (R_e)$")
+    axs[0].set_ylabel(r"$Y (R_e)$")
+
+    axs[1].set_xlabel(r"$X (R_e)$")
+    axs[1].set_ylabel(r"$Z (R_e)$")
+    
+    axs[2].set_xlabel(r"$Y (R_e)$")
+    axs[2].set_ylabel(r"$Z (R_e)$")
+    
+    
+    axs[1].set_ylim([-2*rs,5*rs])
+    axs[2].set_ylim([-2*rs,5*rs])
+    
+    
+    T = np.array(Traj).T
+    
+    axu.plot(T[7]/3600,T[8],label=r'$u_r$')
+    axu.plot(T[7]/3600,T[9],label=r'$u_t$')
+    axu.plot(T[7]/3600,T[10],label=r'$u_n$')
+    axu.grid(True)
+    
+    axu.set_ylabel(r"$\mathbf{u}$")
+    
+    axu.set_xlabel(r"$t (hrs)$")
+    
+    axu.legend()
+
+    for i in range(0,3):
+        axs[i].axis("Equal")
+        axs[i].spines[['left', 'right', 'top']].set_visible(True)
+        axs[i].set_frame_on(True)
+        
+        
+    fig.set_size_inches(12.0, 9, forward=True)
+    fig.tight_layout()
+
+    plt.show()
+    
+
+##################### Vector Functions to define problem #####################
+
+def RTNBasisFunc():
+    # Computes RTN Basis vectors from cartesian position and velocity
+    R,V = Args(6).tolist([(0,3),(3,3)])
+
+    Rhat = R.normalized()
+    Nhat = R.cross(V).normalized()
+    That = Nhat.cross(R).normalized()
+
+    return vf.stack(Rhat,That,Nhat)
+
+
+
+def MEECartFunc(mu):
+    # Function for computing cartesian position and velocity from MEEs
+    X = Args(6)
+    p,f,g,h,k,L = X.tolist()
+    
+    
+    sinL = vf.sin(L)
+    cosL = vf.cos(L)
+    sqp  = vf.sqrt(mu/p)
+    
+    w = 1+f*cosL +g*sinL
+    s2 = 1+h**2 +k**2
+    a2 = h**2 - k**2
+    r = p/w
+    r_s2 = r/s2
+    subs2 = 1.0/s2
+    
+    R = r_s2*vf.stack([cosL + a2*cosL + 2.*h*k*sinL, sinL - a2*sinL + 2.*h*k*cosL,
+                     2.0*(h*sinL - k*cosL)])
+    
+    V = -subs2*sqp*vf.stack([sinL + a2*sinL - 2.*h*k*cosL + g - 2.*f*h*k + a2*g,
+                     -cosL + a2*cosL + 2.*h*k*sinL - f + 2.*g*h*k + a2*f,
+                     -2.0*(h*cosL + k*sinL + f*h + g*k)])
+    
+    return vf.stack([R, V])
+
+def RadFunc(mu):
+    # Function for computing radius from central body from MEEs
+    X = Args(6)
+    p,f,g,h,k,L = X.tolist()
+
+    sinL = vf.sin(L)
+    cosL = vf.cos(L)
+    w  = 1.+f*cosL +g*sinL
+    r = p/w
+    return r
+
+def ZonalGrav(mu,Re,J2,J3,J4):
+    ## Function for computing Zonal Gravity in the RTN frame given the Cartesian
+    ## position and velocity
+    X = Args(6)
+    
+    R,V = X.tolist([(0,3),(3,3)])
+    
+    r = R.norm()
+    
+    
+    #### Eq. 6.46-6.49 in reference ###############
+    Ir = R.normalized()
+    North = np.array([0,0,1.0])
+    In = (North - Ir*(Ir.dot(North))).normalized()
+    
+    
+    sphi = R[2]/r
+    sphi = Ir[2]
+    cphi = vf.sqrt(1 - sphi**2)
+    
+    
+    P2 = 0.5*(3.0*(sphi**2)-1.0)
+    P3 = 0.5*(5.0*(sphi**3)- 3*sphi)
+    P4 = (35/8)*(sphi**4) - (30/8)*(sphi**2) + 3/8
+    
+    D2 = 3*sphi
+    D3 = 0.5*(15.0*(sphi**2)-3.0)
+    D4 = (35/2)*(sphi**3) - (30/4)*(sphi) 
+    
+    Js = [J2,J3,J4]
+    Ps = [P2,P3,P4]
+    Ds = [D2,D3,D4]
+    
+    grs = []
+    gns = []
+    
+    for k in range(2,5):
+        gns.append( Ds[k-2]*Js[k-2]*((Re/r)**k) )
+        grs.append( ((k+1)*Ps[k-2]*Js[k-2])*((Re/r)**k)   )
+        
+        
+    gn = vf.sum(gns)*cphi
+    gr = vf.sum(grs)
+    
+    Gcart = (gn*In - gr*Ir)*(-mu/R.squared_norm())
+    #########################
+    
+    ## Transform to RTN frame
+    RTNBasis = RTNBasisFunc()
+    M = vf.RowMatrix(RTNBasis,3,3)
+    return M*Gcart
+    
+    
+   
+def MEEDynamics(mu):
+    # MEE equations of motion with perturbing accellerations in RTN frame
+    X = Args(9)
+    
+    p,f,g,h,k,L,ur,ut,un = X.tolist()
+    
+    sinL = vf.sin(L)
+    cosL = vf.cos(L)
+    sqp  = vf.sqrt(p)/np.sqrt(mu)
+    
+    hk = X.segment2(3)
+    
+    w  = 1.+f*cosL +g*sinL
+    s2 = 1.+hk.squared_norm()
+    
+    pdot  = 2.*(p/w)*ut
+    fdot  = vf.sum([ur*sinL ,((w+1)*cosL +f)*(ut/w), -(h*sinL-k*cosL)*(g*un/w)])
+    gdot  = vf.sum([-ur*cosL ,((w+1)*sinL +g)*(ut/w) ,(h*sinL-k*cosL)*(f*un/w)])
+    hkdot = vf.stack([cosL,sinL])*((s2*un/w)/2.0)
+    Ldot  = mu*(w/p)*(w/p) + (1.0/w)*(h*sinL -k*cosL)*un
+    
+    return vf.stack([pdot,fdot,gdot,hkdot,Ldot])*sqp
+
+
+
+def MEEDynamics2(mu):
+    
+    ## More efficient way to write the same thing that eliminates expensive
+    ## sub-expressions
+    X = Args(9)
+    
+    p,f,g,h,k,L,ur,ut,un = X.tolist()
+    
+    sinL = vf.sin(L)
+    cosL = vf.cos(L)
+    sqp  = vf.sqrt(p)/np.sqrt(mu)
+    hk = X.segment2(3)
+    w  = 1.+f*cosL +g*sinL
+    
+    Xtmp = vf.stack(X,sinL,cosL,w)
+    
+    ###################################################
+    ## Sin cos and w are now addditonal args
+    X2 = Args(12)
+    p,f,g,h,k,L,ur,ut,un,sinL,cosL,w = X2.tolist()
+    hk = X2.segment2(3)
+
+    sqp  = vf.sqrt(p)/np.sqrt(mu)
+    s2 = 1.+hk.squared_norm()
+
+    pdot  = 2.*(p/w)*ut
+    fdot  = vf.sum([ur*sinL ,((w+1)*cosL +f)*(ut/w), -(h*sinL-k*cosL)*(g*un/w)])
+    gdot  = vf.sum([-ur*cosL ,((w+1)*sinL +g)*(ut/w) ,(h*sinL-k*cosL)*(f*un/w)])
+    hkdot = vf.stack([cosL,sinL])*((s2*un/w)/2.0)
+    Ldot  = mu*(w/p)*(w/p) + (1.0/w)*(h*sinL -k*cosL)*un
+    
+    ## Compose back with Xtmp
+    return (vf.stack([pdot,fdot,gdot,hkdot,Ldot])*sqp)(Xtmp)
+     
+    
+
+class LTModel(oc.ODEBase):
+     def __init__(self, mu,T,gs,Isp):
+        
+        ############################################################
+        XtUP = oc.ODEArguments(7,3,1)
+        
+        ## MEEs and vehicle weight
+        MEEs =  XtUP.XVec().head(6)
+        ww = XtUP.XVar(6)
+        
+        p,f,g,h,k,L = MEEs.tolist()  # Assumed order
+        
+        ## Control Direction in RTN
+        U = XtUP.UVec().head3().normalized()
+        ## Static throttle parameter
+        tau = XtUP.PVar(0)
+        wwdot = -T*(1+.01*tau)/(Isp)
+        acc_T =  gs*T*(1+.01*tau)*U/ww
+        
+        ## Convert MEEs to Cartesian then forward the ZonalGrav
+        acc_J2 =  ZonalGrav(mu, Re, J2, J3, J4)( MEECartFunc(mu) )(MEEs)
+        
+        # Combine perturbing accellerations and call MEE dynamics
+        acc = acc_T + acc_J2
+        Xdot   = MEEDynamics2(mu).eval(vf.stack(MEEs,acc))
+        
+        ode = vf.stack([Xdot,wwdot])
+        
+        Vgroups = {}
+        Vgroups["p"]=p
+        Vgroups["f"]=f
+        Vgroups["g"]=g
+        Vgroups["h"]=h
+        Vgroups["k"]=k
+        Vgroups["L"]=L
+        Vgroups["MEEs"]=MEEs
+        Vgroups["U"]=XtUP.UVec()
+        Vgroups["tau"]=tau
+        Vgroups["t"]=XtUP.TVar()
+        Vgroups["w"]=ww
+
+
+        
+        #############################################################
+        super().__init__(ode, 7,3,1,Vgroups=Vgroups)
+
+def MEEProgradeUlaw():
+    MEEs = Args(6).vf()
+    RV = MEECartFunc(mu)(MEEs)
+    RTNBasis = RTNBasisFunc()(RV)
+    U = vf.RowMatrix(RTNBasis,3,3)*RV.tail(3).normalized() 
+    return U
+
+def EqBCon():
+    ## Boundary constraint eq 6.52-6.55 from reference
+    X = Args(6)
+    p,f,g,h,k,L = X.tolist()
+    
+    eq1 = p - ptf
+    eq2 = X.segment2(1).squared_norm()-.73550320568829**2
+    eq3 = X.segment2(3).squared_norm()-.61761258786099**2
+    eq4 = f*h + g*k
+    return vf.stack([eq1,eq2,eq3,eq4])
+
+def IqBCon():
+    ## Boundary constraint eq 6.56 from reference
+    X = Args(6)
+    p,f,g,h,k,L = X.tolist()
+    return g*h - k*f
+
+
+#############################################################################
+
+if __name__ == "__main__":
+    
+    
+    
+    
+    ode = LTModel(mu,Thrust,gs,Isp)
+    
+    
+    X0 = ode.make_input(p = pt0,
+                        h=ht0,
+                        L=np.pi,
+                        w=1.0,
+                        U=[0.0,1.0,0.0],
+                        tau = -25.0)
+    
+    tfig = 90000
+    integ = ode.integrator(1,MEEProgradeUlaw(),"MEEs")
+    ## Same as above
+    #integ = ode.integrator(1,MEEProgradeUlaw(),["p","f","g","h","k","L"])
+    #integ = ode.integrator(1,MEEProgradeUlaw(),range(0,6))
+
+
+    integ.setAbsTol(1.0e-12)
+    integ.setRelTol(1.0e-5)
+    
+    IG = integ.integrate_dense(X0,tfig)
+    
+    #############################################
+    
+    
+    phase = ode.phase("LGL5",IG,16)
+    phase.setAutoScaling(True)
+    
+    ## If you need to make units vector manually
+    Units = np.ones((12))  # Size of ODE Input
+    Units[0]=Lstar  
+    Units[6]=Fstar
+    Units[7]=Tstar
+    phase.setUnits(Units)    
+    ## Same as above
+    # Units = ode.make_units(p=Lstar,w=Fstar,t=Tstar)
+    # phase.setUnits(Units)    
+    ## Or if you have named variables in your ODE
+    # phase.setUnits(p=Lstar,w=Fstar,t=Tstar)
+    
+    phase.addBoundaryValue("Front",["MEEs","w","t"],X0[0:8])
+    phase.addEqualCon("Path",Args(3).norm()-1,"U")
+    # Dont use control splines when placing equality path constraints on controls
+    phase.setControlMode("NoSpline")
+    phase.addLUFuncBound("Path",RadFunc(mu),"MEEs",Re,10*Re)
+
+    phase.addEqualCon("Back",EqBCon(),"MEEs")
+    phase.addInequalCon("Back",IqBCon(),"MEEs")
+    
+    phase.addLUVarBound("ODEParams","tau", -50,0)
+    phase.addLowerVarBound("Back","w",.05)
+    phase.addValueObjective("Back",6,-1.0)
+    phase.setThreads(8,8)
+    phase.optimizer.PrintLevel = 2
+    phase.optimizer.set_EContol(1.0e-9)
+    
+    
+    ## All error estimates and tolerances are in reference to the scaled ODE system
+    phase.setAdaptiveMesh(True)
+    phase.setMeshErrorEstimator("integrator")
+    phase.setMeshTol(1.0e-7)
+    
+    phase.optimize_solve()
+
+    Traj = phase.returnTraj()
+    
+    wf,tf,tauf = ode.get_vars(["w","t","tau"],Traj[-1]) 
+    
+    print(f"Final Weight:{wf} lb")
+    print(f"Final Time:{tf} s")
+    print(f"Throttle Parameter:{tauf} ")
+
+    PhaseMeshErrorPlot(phase,show=False)
+    Plot(MEEToCart(Traj))
+    
+    
+    
+
diff --git a/examples/UpdatedInterface/Delta3Launch.py b/examples/UpdatedInterface/Delta3Launch.py
new file mode 100644
index 00000000..e895411a
--- /dev/null
+++ b/examples/UpdatedInterface/Delta3Launch.py
@@ -0,0 +1,475 @@
+import numpy as np
+import asset_asrl as ast
+import asset_asrl.VectorFunctions as vf
+import asset_asrl.OptimalControl as oc
+from asset_asrl.VectorFunctions import Arguments as Args
+
+import matplotlib.pyplot as plt
+from mpl_toolkits.basemap import Basemap ## PIP INSTALL Basemap if you dont have it
+
+
+
+############################################################################
+Lstar   =  6378145           ## m   Radius of Earth
+Tstar   =  961.0             ## sec Engine Burn Time
+Mstar   =  301454.0          ## kgs Inital Mass of Rocket
+Vstar   = Lstar/Tstar
+#############################################################################
+
+g0      =  9.80665 
+
+mu      = 3.986012e14      
+Re      = 6378145
+We      = 7.29211585e-5    
+
+RhoAir  = 1.225        
+h_scale = 7200         
+g       = g0           
+
+
+CD = .5
+S  = 4*np.pi   
+
+
+
+TS = 628500      
+T1 = 1083100     
+T2 = 110094      
+
+IS = 283.33364   
+I1 = 301.68      
+I2 = 467.21       
+
+tS = 75.2        
+t1 = 261         
+t2 = 700         
+
+
+TMS = 19290      
+TM1 = 104380     
+TM2 = 19300      
+TMPay = 4164     
+
+PMS = 17010     
+PM1 = 95550     
+PM2 = 16820     
+
+SMS = TMS - PMS
+SM1 = TM1 - PM1
+SM2 = TM2 - PM2
+
+T_phase1 = 6*TS + T1
+T_phase2 = 3*TS + T1
+T_phase3 = T1
+T_phase4 = T2
+
+mdot_phase1 = (6*TS/IS + T1/I1)/g
+mdot_phase2 = (3*TS/IS + T1/I1)/g
+mdot_phase3 = T1/(g*I1)
+mdot_phase4 = T2/(g*I2)
+
+
+tf_phase1 = tS
+tf_phase2 = 2*tS
+tf_phase3 = t1
+tf_phase4 = t1+t2
+
+m0_phase1 = 9*TMS + TM1 + TM2 + TMPay
+mf_phase1 = m0_phase1 - 6*PMS - (tS/t1)*PM1
+
+m0_phase2 = mf_phase1 - 6*SMS
+mf_phase2 = m0_phase2 - 3*PMS - (tS/t1)*PM1
+
+m0_phase3 = mf_phase2 - 3*SMS
+mf_phase3 = m0_phase3 - (1 - 2*tS/t1)*PM1
+
+m0_phase4 = mf_phase3 - SM1
+mf_phase4 = m0_phase4 - PM2
+
+
+#############################################################################
+class RocketODE(oc.ODEBase):
+    def __init__(self,T,mdot):
+        ####################################################
+        XtU  = oc.ODEArguments(7,3)
+        
+        R = XtU.XVec().head(3)
+        V = XtU.XVec().segment(3,3)
+        m = XtU.XVar(6)
+       
+        U = XtU.UVec()
+        
+        h       = R.norm() - Re
+        rho     = RhoAir * vf.exp(-h / h_scale)
+        Vr      = V + R.cross(np.array([0,0,We]))
+        
+        D       = (-0.5*CD*S)*rho*(Vr*Vr.norm())
+        
+        Rdot    =  V
+        Vdot    =  (-mu)*R.normalized_power3() + (T*U.normalized() + D)/m
+        
+        ode = vf.stack(Rdot,Vdot,-mdot)
+        
+        Vgroups = {}
+        ## Multiple names allowed, just index with tuple of names
+        Vgroups[("R","Position")]=R
+        Vgroups[("V","Velocity")]=V
+        Vgroups["U"]=U
+        Vgroups[("t","time")]=XtU.TVar()
+        Vgroups[("m","mass")]=m
+        
+        Vgroups["RV"] =[0,1,2,3,4,5]
+
+
+        ####################################################
+        super().__init__(ode,7,3,Vgroups = Vgroups)
+
+def TargetOrbit(at,et,it, Ot,Wt):
+    R,V = Args(6).tolist([(0,3),(3,3)])
+    
+    r    = R.norm()
+    v    = V.norm()
+    
+    #Angular momentum vector
+    hvec = R.cross(V)
+    
+    #Node vector
+    nvec = vf.cross([0,0,1],hvec)
+    
+    # Energy
+    eps = 0.5*(v**2) - mu/r
+    
+    # Semi-major axis
+    a =  -0.5*mu/eps
+    
+    evec = V.cross(hvec)/mu - R.normalized()
+    #Eccentrcity
+    e = evec.norm()
+    
+    #inclination
+    i = vf.arccos(hvec.normalized()[2]) 
+    
+    #RAAN
+    O = vf.arccos(nvec.normalized()[0])
+    # Quadrant Check
+    O = vf.ifelse(nvec[1]>0,O,2*np.pi -O)
+    
+    # Argument of periapse
+    W = vf.arccos(nvec.normalized().dot(evec.normalized()))
+    #QuadrantCheck
+    W = vf.ifelse(evec[2]>0,W,2*np.pi-W)
+    
+    return vf.stack([a,e,i,O,W]) - np.array([at,et,it,Ot,Wt])
+    
+###############################################################################
+
+def Plot(Phase1,Phase2,Phase3,Phase4):
+    ############################################
+
+    def LatLongAlt(Traj):
+        LLs = []
+        for T in Traj:
+            x = T[0]
+            y = T[1]
+            z = T[2]
+            r   = np.linalg.norm(T[0:3])
+            
+            lat = np.rad2deg(np.arcsin(z/r))
+            
+            long = np.rad2deg(np.arctan2(y,x))
+            if(x>0):long+=0
+            elif(y>0):long+=180
+            else:long-=180
+            LLs.append([lat,long-80.649]) 
+        return LLs
+
+
+    def AltVelMass(Traj):
+        Xs =[]
+        for T in Traj:
+            r   = np.linalg.norm(T[0:3])
+            alt = (r-Re)/1000
+            v = np.linalg.norm(T[3:6])/1000
+            m = T[6]
+            t = T[7]
+            Xs.append([alt,v,m,t])
+            
+        return np.array(Xs).T
+
+    LLs = LatLongAlt([Phase1[0],Phase2[0],Phase3[0],Phase4[0],Phase4[-1]])
+            
+    P1 = AltVelMass(Phase1)
+    P2 = AltVelMass(Phase2)
+    P3 = AltVelMass(Phase3)
+    P4 = AltVelMass(Phase4)
+
+    fig = plt.figure()
+    ax0 = plt.subplot(321)
+    ax1 = plt.subplot(323)
+    ax2 = plt.subplot(325)
+    ax3 = plt.subplot(122)    
+
+    ax0.grid(True)
+    ax1.grid(True)
+    ax2.grid(True)
+
+    ax0.set_ylabel("h (km)")
+    ax1.set_ylabel("v (km)/s")
+    ax2.set_ylabel("M (kg)")
+    ax2.set_xlabel("t (s)")
+
+    PS = [P1,P2,P3,P4]
+
+    for i,P in enumerate(PS):
+        s = i+1
+        ax0.plot(P[3],P[0])
+        ax1.plot(P[3],P[1])
+        ax2.plot(P[3],P[2],label=f'Stage {s}')
+
+
+    
+    m = Basemap(projection='lcc',
+                lat_1=45.,lat_2=55,lat_0=50,lon_0=-65.,
+                resolution=None,width=9000000,height=9000000,ax=ax3)
+
+    for i in range(0,len(LLs)-1):
+        lon1 = float(LLs[i][1])
+        lat1 = float(LLs[i][0])
+        lon2 = float(LLs[i+1][1])
+        lat2 = float(LLs[i+1][0])
+        m.drawgreatcircle(lon1=lon1,lat1=lat1,lon2=lon2,lat2=lat2)
+
+    ax2.legend()
+    #m.bluemarble()
+    m.shadedrelief()
+
+    m.drawparallels(np.arange(-90.,91.,30.))
+    m.drawmeridians(np.arange(-180.,181.,60.))
+    #m.drawmapboundary(fill_color='aqua')
+    plt.title("Ground Track")
+    
+    fig.set_size_inches(15.0, 7.5, forward=True)
+    fig.tight_layout()
+
+    plt.show()
+
+###############################################################################
+
+if __name__ == "__main__":
+
+    ast.SoftwareInfo()
+    
+    
+    # Target orbital elements
+    at     = 24361140 
+    et     = .7308
+    Ot     = np.deg2rad(269.8)
+    Wt     = np.deg2rad(130.5)
+    istart = np.deg2rad(28.5)
+    
+    
+    y0      = np.zeros((6))
+    y0[0:3] = np.array([np.cos(istart),0,np.sin(istart)])*Re
+    y0[3:6] =-np.cross(y0[0:3],np.array([0,0,We]))
+    y0[3]  += 0.00001  # cant be exactly zero,our drag equation's derivative would NAN !!!
+    
+    
+    ## MF is the only magic number in the script, just trying to find
+    ## a mean anomaly such that the terminal state on the orbit is downrange
+    ## eastward from KSC in and doesnt pass through earth when LERPed from KSC
+    MF   =-.05
+    OEF  = [at,et,istart,Ot,Wt,MF]
+    yf   = ast.Astro.classic_to_cartesian(OEF,mu)
+    
+    ts   = np.linspace(0,tf_phase4,1000)
+    
+    IG1 =[]
+    IG2 =[]
+    IG3 =[]
+    IG4 =[] 
+    
+    ode1 = RocketODE(T_phase1,mdot_phase1)
+    ode2 = RocketODE(T_phase2,mdot_phase2)
+    ode3 = RocketODE(T_phase3,mdot_phase3)
+    ode4 = RocketODE(T_phase4,mdot_phase4)
+    
+    for t in ts:
+        RVi= y0 + (yf-y0)*(t/ts[-1])
+        Ui = np.array([0,1,0])
+        
+        if(t<tf_phase1):
+            m= m0_phase1 + (mf_phase1-m0_phase1)*(t/tf_phase1)
+            ## Create ODEinputs w/ named variable groups, 0 if unspecified
+            X = ode1.make_input(RV=RVi,t=t,m=m,U=Ui)
+            IG1.append(X)
+        elif(t<tf_phase2):
+            m= m0_phase2 + (mf_phase2-m0_phase2)*(( t-tf_phase1) / (tf_phase2 - tf_phase1))
+            X = ode2.make_input(RV=RVi,t=t,m=m,U=Ui)
+            IG2.append(X)
+        elif(t<tf_phase3):
+            m= m0_phase3 + (mf_phase3-m0_phase3)*(( t-tf_phase2) / (tf_phase3 - tf_phase2))
+            X = ode3.make_input(RV=RVi,t=t,m=m,U=Ui)
+            IG3.append(X)
+        elif(t<tf_phase4):
+            m= m0_phase4 + (mf_phase4-m0_phase4)*(( t-tf_phase3) / (tf_phase4 - tf_phase3))
+            X = ode4.make_input(RV=RVi,t=t,m=m,U=Ui)
+            IG4.append(X)
+        
+    
+    
+    
+    
+    tmode = "LGL3"
+    cmode = "HighestOrderSpline"
+    
+    nsegs1 = 16
+    nsegs2 = 16
+    nsegs3 = 16
+    nsegs4 = 16
+    
+    #########################################
+    phase1 = ode1.phase(tmode,IG1,nsegs1)
+    phase1.setControlMode(cmode)
+    
+    ## Enable AutoScaling, off by default
+    phase1.setAutoScaling(True)
+    
+    units = np.ones((11))
+    units[0:3]=Lstar
+    units[3:6]=Vstar
+    units[6]=Mstar
+    units[7]=Tstar
+    ## All others are one,i.e no auto-scaling
+    
+    phase1.setUnits(units)  # As a single vector
+    # Or
+    phase1.setUnits(R=Lstar,V=Vstar,t=Tstar,m=Mstar) 
+
+    
+    phase1.addLUNormBound("Path","U",.5,1.5)
+    phase1.addBoundaryValue("Front",["R","V","m","t"],IG1[0][0:8])
+    
+    #Dont want our bound to interfere with initial condition which starts at Re
+    #so i relax the Earth radius constraint slightly here
+    phase1.addLowerNormBound("Path","R",Re*.999999)
+    
+    phase1.addBoundaryValue("Back","time",tf_phase1)
+    
+    #########################################
+    phase2 = ode2.phase(tmode,IG2,nsegs2)
+    phase2.setControlMode(cmode)
+    
+    phase2.addLowerNormBound("Path","R",Re)
+    phase2.addLUNormBound("Path","U",.5,1.5)
+    
+    ## Fixing initial mass and final time on first 3 phases.
+    ## Since the engine cant be throttled, constraining final mass
+    ## as well would be redundant and overconstrained
+    phase2.addBoundaryValue("Front","mass", m0_phase2)
+    phase2.addBoundaryValue("Back", "time" ,tf_phase2)
+    
+    #########################################
+    phase3 = ode3.phase(tmode,IG3,nsegs3)
+    phase3.setControlMode(cmode)
+    
+    phase3.addLowerNormBound("Path","R",Re)
+    phase3.addLUNormBound("Path","U",.5,1.5)
+    phase3.addBoundaryValue("Front","mass", m0_phase3)
+    phase3.addBoundaryValue("Back", "time" ,tf_phase3)
+    
+    #########################################
+    phase4 = ode4.phase(tmode,IG4,nsegs4)
+    phase4.setControlMode(cmode)
+
+
+    ## AutoScale = "auto" if not specified
+    phase4.addBoundaryValue("Front","mass", m0_phase4)
+    phase4.addUpperVarBound("Back","time",tf_phase4)
+    # AutoScale=None, will turn it off for this constraint
+    phase4.addLUNormBound("Path","U",.5,1.5,AutoScale=None)
+    
+    phase4.addLowerNormBound("Path","R",Re,AutoScale=1/Lstar)
+    
+    phase4.addEqualCon("Back",TargetOrbit(at,et,istart,Ot,Wt),["RV"],AutoScale = [1/Lstar,1.0,1.0,1.0,1.0])
+    
+    # Maximize final mass
+    phase4.addValueObjective("Back","mass",-1.0)
+    
+
+    #########################################
+    
+    ocp = oc.OptimalControlProblem()
+    ocp.addPhase(phase1)
+    ocp.addPhase(phase2)
+    ocp.addPhase(phase3)
+    ocp.addPhase(phase4)
+    
+    ocp.setAutoScaling(True,True)
+    ocp.setAdaptiveMesh(True)  
+    
+    
+    for phase in ocp.Phases:
+        phase.setUnits(R=Lstar,V=Vstar,t=Tstar,m=Mstar)
+        phase.setMeshTol(1.0e-6)
+        phase.setMeshErrorCriteria('max')
+        phase.setMeshErrorEstimator('integrator')  
+
+
+    ## Each Phase does not have to have the same AutoScale units even if its the same ODE
+    phase4.setUnits(R=2*Lstar,V=Vstar,t=.8*Tstar,m=Mstar)
+
+    ## Everything but mass
+    ocp.addForwardLinkEqualCon(phase1,phase4,["R","V","t","U"],AutoScale="auto")
+
+
+    ocp.optimizer.set_OptLSMode("L1")
+    ocp.optimizer.set_SoeLSMode("L1")
+    ocp.optimizer.set_MaxLSIters(2)
+    ocp.optimizer.set_PrintLevel(0)
+    ocp.solve_optimize()
+
+    
+    Phase1Traj = phase1.returnTraj()  # or ocp.Phase(i).returnTraj()
+    Phase2Traj = phase2.returnTraj()
+    Phase3Traj = phase3.returnTraj()
+    Phase4Traj = phase4.returnTraj()
+    
+    
+    ## retrieve vars from vectors or trajs by name using ode.get_vars
+    mf = ode4.get_vars("mass", Phase4Traj[-1],retscalar=True)
+    print("Final Mass = ",mf,' kg')
+
+    
+
+    Plot(Phase1Traj,Phase2Traj,Phase3Traj,Phase4Traj)
+    
+    
+    
+    TotalTraj = Phase1Traj + Phase2Traj + Phase3Traj + Phase4Traj
+    
+    ## ode.get_vars alse works on full trajectories
+    ts,rx,ry,rz,vx,vy,vz = ode4.get_vars(["t","R","V"],TotalTraj).T
+    
+    plt.plot(ts,rx,label="rx")
+    plt.plot(ts,ry,label="ry")
+    plt.plot(ts,rz,label="rz")
+    plt.xlabel("t")
+    plt.ylabel("R (m)")
+    plt.legend()
+    plt.grid(True)
+    plt.show()
+    
+    plt.plot(ts,vx,label="vx")
+    plt.plot(ts,vy,label="vy")
+    plt.plot(ts,vz,label="vz")
+    plt.xlabel("t")
+    plt.ylabel("V (m/s)")
+    plt.legend()
+    plt.grid(True)
+    plt.show()
+    
+    
+    #########################################
+
+
+
diff --git a/examples/UpdatedInterface/GoddardRocket.py b/examples/UpdatedInterface/GoddardRocket.py
new file mode 100644
index 00000000..7857872b
--- /dev/null
+++ b/examples/UpdatedInterface/GoddardRocket.py
@@ -0,0 +1,187 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import asset_asrl as ast
+import asset_asrl.VectorFunctions as vf
+import asset_asrl.OptimalControl as oc
+from asset_asrl.VectorFunctions import Arguments as Args
+
+
+'''
+Classic Goddard rocket problem with singular arc
+
+Betts, J.T. "Practical methods for Optimal Control and Estimation Using Nonlinear Programming", Cambridge University Press, 2009
+See Section 4.14
+
+'''
+
+g0 = 32.2 
+W  = 203000
+
+Lstar = 10000.0   ## feet
+Tstar = 60.0      ## sec
+Mstar = 1         ## slugs
+Vstar   = Lstar/Tstar
+
+
+rho0  =.002378         
+h_ref = 23800          
+g     = g0             
+Tmag  = 200           
+c     = 1580.94        
+sigma = 5.4915e-5      
+
+m0   = 3
+mf   = 1
+
+
+
+class GoddardRocket(oc.ODEBase):
+    def __init__(self,sigma,c,h_ref,Tmag, g):
+        ############################################################
+        XtU  = oc.ODEArguments(3,1)
+        h,v,m = XtU.XVec().tolist()
+        u = XtU.UVar(0)
+        
+        hdot=v
+        vdot = (u*Tmag - sigma*(v**2)*vf.exp(-h/h_ref))/m -g
+        mdot = -u*Tmag/c
+        
+        ode = vf.stack(hdot,vdot,mdot)
+        ##############################################################
+        Vgroups = {}
+        Vgroups[("h","altitude")] = h
+        Vgroups[("v","velocity")] = v
+        Vgroups[("m","mass")] = m
+        Vgroups[("t","time")] = XtU.TVar()
+        Vgroups["u"]=u
+        
+        super().__init__(ode,3,1,Vgroups = Vgroups)
+ 
+def PathCon(sigma,c,h_ref,Tmag, g):
+    h,v,m,u = Args(4).tolist()
+    t1 = (u*Tmag - sigma*(v**2)*vf.exp(-h/h_ref)) -g*m
+    t2 = (m*g/( 1 + 4*(c/v) +2*(c/v)**2 ))*( c*c*(1+v/c)/(h_ref*g) -1.0 -2.0*c/v )
+    return t1-t2
+       
+
+def Plot(axs,Traj,label=''):
+    T = np.array(Traj).T
+    
+    
+    axs[0].plot(T[3],T[0],label=label)
+    axs[1].plot(T[3],T[1])
+    axs[2].plot(T[3],T[2])
+    axs[3].plot(T[3],T[4]*Tmag)
+    
+    
+
+if __name__ == "__main__":
+
+    def Ulaw():
+        m = Args(1)[0]
+        return vf.ifelse(m>mf,1,0)
+        
+    def StopFunc(x): return (x[1]<0)
+        
+    ode = GoddardRocket(sigma,c,h_ref,Tmag, g)
+    
+    units = ode.make_units(h=Lstar,v = Vstar,m = Mstar,t = Tstar)
+    
+    integ = ode.integrator(.01,Ulaw(),"m")
+    
+    X0 = ode.make_input(h=0,v=0,m=m0,u=1)
+    TrajIG = integ.integrate_dense(X0,60,1000,StopFunc)
+    
+    '''
+    Single phase formualtion has a singular arc
+    '''
+    
+    ##############################################################################
+    phase = ode.phase("LGL3",TrajIG,128)
+    phase.setAutoScaling(True)
+    phase.setUnits(units)
+    
+    phase.addBoundaryValue("Front",["h","v","m","t"],TrajIG[0][0:4])
+    phase.addLUVarBound("Path","u",0.0,1.0,1.0)
+    phase.addValueObjective("Back","h",-1.0)
+    phase.addBoundaryValue("Back",["v","m"],[0,mf])
+    phase.optimize()
+    Traj = phase.returnTraj()
+    ###############################################################################
+    
+    '''
+    Multi phase formualtion defines control using path constraint on middle
+    phase.
+    '''
+    
+    n = int(len(TrajIG)/3)
+    
+    TrajIG1 = TrajIG[0:n]
+    TrajIG2 = TrajIG[n:2*n]
+    TrajIG3 = TrajIG[2*n:-1]
+    
+    phase1 = ode.phase("LGL3",TrajIG1,32)
+    phase1.addBoundaryValue("Front",["h","v","m","t"],TrajIG[0][0:4])
+    phase1.addBoundaryValue("Path","u",1.0)
+    
+    phase2 = ode.phase("LGL3",TrajIG2,32)
+    # PathCon makse Control splines redundant for LGL>3
+    phase2.setControlMode("NoSpline") 
+    phase2.addLUVarBound("Path","u",0.0,1.0,1.0)
+    phase2.addEqualCon("Path",PathCon(sigma,c,h_ref,Tmag, g),["h","v","m","u"])
+    
+    phase3 = ode.phase("LGL3",TrajIG3,32)
+    phase3.addBoundaryValue("Path","u",0)
+    phase3.addBoundaryValue("Back",["v","m"],[0,mf])
+    phase3.addValueObjective("Back","h",-1.0)
+    
+    ocp = oc.OptimalControlProblem()
+    ocp.addPhase(phase1)
+    ocp.addPhase(phase2)
+    ocp.addPhase(phase3)
+    
+    ocp.addForwardLinkEqualCon(phase1,phase3,["h","v","m","t"])
+    
+    phase1.addLowerDeltaTimeBound(0)
+    phase2.addLowerDeltaTimeBound(0)
+    phase3.addLowerDeltaTimeBound(0)
+    
+    
+    ## Set each phase's units
+    phase1.setUnits(units)
+    phase2.setUnits(units)
+    phase3.setUnits(units)
+    
+    # Enable autoscaling for ocp and all constituent phases
+    ocp.setAutoScaling(True,True)
+
+    ocp.setThreads(8,8)
+    ocp.optimize()
+    
+    ##############################################################################
+    
+    Traj2 = phase1.returnTraj() + phase2.returnTraj() + phase3.returnTraj()
+    
+    fig,axs = plt.subplots(4,1)
+    
+    
+    Plot(axs,TrajIG,"Initial Guess")
+    Plot(axs,Traj,"Single Phase")
+    Plot(axs,Traj2,"Multi Phase")
+    
+    axs[0].grid(True)
+    axs[1].grid(True)
+    axs[2].grid(True)
+    axs[3].grid(True)
+    
+    axs[3].set_xlabel("t (s)")
+    
+    axs[0].set_ylabel("h (ft)")
+    axs[1].set_ylabel("v (ft/s)")
+    axs[2].set_ylabel("m (slug)")
+    axs[3].set_ylabel("T (lb)")
+    
+    axs[1].legend()
+    plt.show()
+
+
diff --git a/examples/UpdatedInterface/MinimumTimeToClimb.py b/examples/UpdatedInterface/MinimumTimeToClimb.py
new file mode 100644
index 00000000..d5db87a7
--- /dev/null
+++ b/examples/UpdatedInterface/MinimumTimeToClimb.py
@@ -0,0 +1,241 @@
+import numpy as np
+import asset_asrl as ast
+import asset_asrl.VectorFunctions as vf
+import asset_asrl.OptimalControl as oc
+from asset_asrl.VectorFunctions import Arguments as Args
+import matplotlib.pyplot as plt
+from asset_asrl.OptimalControl.MeshErrorPlots import PhaseMeshErrorPlot,sns
+
+## Aerodynamic Tables generated in seperate script
+from MinimumTimeToClimbTables import ClalphaTab,etaTab,CD0Tab,rhoTab,sosTab,ThrustTab
+
+'''
+Minumum time to climb for supersonic aircraft. 
+
+Original problem data comes from:
+
+A. E. Bryson, M. N. Desai, and W. C. Hoffman, Energy-State Approximation 
+in Performance Optimization of Supersonic Aircraft, 
+Journal of Aircraft, Vol. 6, No. 6, November-December, 1969, pp. 481-488.
+
+Using version reformulated in SI units by ICLOS2 
+http://www.ee.ic.ac.uk/ICLOCS/ExampleMinFuelClimb.html
+
+'''
+
+############################################################################
+
+g0      =  9.80665 
+
+Lstar   =  10000           
+Tstar   =  250.0             
+Mstar   =  19050.864
+Vstar   =  Lstar/Tstar
+
+
+#############################################################################
+
+mu       = 3.986012e14      
+Re       = 6378145          
+S        = 49.2386          
+vexhaust = 1600*g0          
+
+#############################################################################
+class AirPlane(oc.ODEBase):
+    
+    def __init__(self):
+        
+        XtU = oc.ODEArguments(4,1)
+        
+        # Altitude,velocity,flight path angle, mass
+        h,v,fpa,mass = XtU.XVec().tolist()  
+        # Angle of attack
+        alpha = XtU.UVar(0)
+        
+        
+        rho = rhoTab(h)  
+        sos = sosTab(h)
+        
+        Mach = v/sos
+        CD0 = CD0Tab(Mach)
+        Clalpha = ClalphaTab(Mach)
+        eta     = etaTab(Mach)
+        
+        Thrust = ThrustTab(Mach,h)
+        
+        CD = CD0 + eta*Clalpha*(alpha**2)
+        CL = Clalpha*alpha
+        q  = 0.5*rho*(v**2)
+        D  = q*S*CD
+        L  = q*S*CL
+        r  = h + Re
+        
+        
+        hdot   = v*vf.sin(fpa);
+        vdot   = (Thrust*vf.cos(alpha)-D)/mass - mu*vf.sin(fpa)/(r**2);
+        fpadot = (Thrust*vf.sin(alpha)+L)/(mass*v) +vf.cos(fpa)*( v/r - mu/(v*(r**2)));
+        mdot   = -Thrust/vexhaust
+        
+        ode = vf.stack([hdot,vdot,fpadot,mdot])
+        ########################################
+        Vgroups = {}
+        Vgroups[('h','altitude')] =  h
+        Vgroups[("v","velocity")] =  v
+        Vgroups["mass"] =  mass
+        Vgroups["fpa"] =  fpa
+        Vgroups[("alpha","AoA")] =  alpha
+        Vgroups[("t","time")] =  XtU.TVar()
+
+        super().__init__(ode, 4, 1,Vgroups=Vgroups)
+        
+#############################################################################
+def Plot(Traj):
+    
+    sns.set_context("paper")
+
+    fig = plt.figure()
+    ax0 = plt.subplot(421)
+    ax1 = plt.subplot(423)
+    ax2 = plt.subplot(425)
+    ax3 = plt.subplot(427)
+    ax4 = plt.subplot(122)
+
+    T1 = np.array(Traj).T
+    
+    
+    ax0.plot(T1[4],T1[0])
+    ax1.plot(T1[4],T1[1])
+    ax2.plot(T1[4],T1[2]*180/np.pi)
+    ax3.plot(T1[4],T1[5]*180/np.pi)
+    
+    v=np.linspace(0,max(T1[1])*1.1,30)
+    h =np.linspace(0,max(T1[0])*1.05,30)
+    V,H = np.meshgrid(v,h)
+    
+    ## Energy height contours from bryson's paper
+    def EnergyHeight(v,h):
+        g= mu/(h + Re)**2
+        return (.5*v**2)/g + h
+    E = EnergyHeight(V,H)
+    ax4.plot(T1[1],T1[0])
+    cs = ax4.contour(V,H,E,colors= 'k',levels = 7,linestyles='dotted')
+    
+    ax4.scatter(T1[1][0],T1[0][0],color='k')
+    ax4.scatter(T1[1][-1],T1[0][-1],color='k',marker='*')
+
+    ax0.grid(True)
+    ax1.grid(True)
+    ax2.grid(True)
+    ax3.grid(True)
+    ax4.grid(True)
+
+    ax0.set_ylabel(r"$h\;(m)$")
+    ax1.set_ylabel(r"$v\;(\frac{m}{s})$")
+    ax2.set_ylabel(r"$\gamma\;(^\circ)$")
+    ax3.set_ylabel(r"$\alpha\;(^\circ)$")
+    ax3.set_xlabel(r"$t\;(s)$")
+    ax4.set_ylabel(r"$h\;(m)$")
+    ax4.set_xlabel(r"$v\;(\frac{m}{s})$")
+    
+    fig.set_size_inches(10.0, 6.0)
+    fig.tight_layout()
+    plt.show()
+
+
+
+#############################################################################
+if __name__ == "__main__":
+    #########################
+    ht0   = .010     # Dont make exactly zero so it doesnt conflict with path constraint
+    htf   = 19994.88
+    vt0   = 129.314
+    vtf   = 295.092
+    fpat0 = 0 
+    fpatf = 0 
+    mass0 = 19050.864
+    #########################
+    hmin     = 0
+    hmax     = 21000.0
+    vmin     = 5 
+    vmax     = 600
+    fpamin   = -20*np.pi/180
+    fpamax   = 40*np.pi/180
+    massmin  = 16500
+    alphamin = -np.pi/4 
+    alphamax = np.pi/4
+    tfig = 200
+    #########################
+    ode = AirPlane()
+
+    XtU0 = ode.make_input(h=ht0,v=vt0,fpa=fpat0,mass=mass0)
+    XtUf = ode.make_input(h=htf,v=vtf,fpa=fpatf,mass=mass0,t=tfig)
+    
+    Traj = [ XtU0*(1-t) +  XtUf*t for t in np.linspace(0,1,100) ]
+    #########################
+    
+
+    phase = ode.phase("LGL3",Traj,50)
+    phase.setAutoScaling(True)
+    phase.setUnits(h = Lstar,
+                   v = Vstar,
+                   mass = Mstar,
+                   t = Tstar)
+
+    phase.setControlMode("HighestOrderSpline")
+    phase.addBoundaryValue("First",range(0,5),[ht0,vt0,fpat0,mass0,0])
+    
+    phase.addLUVarBound("Path","h",hmin,hmax)
+    phase.addLUVarBound("Path","v",vmin,vmax)
+    phase.addLUVarBound("Path","fpa",fpamin,fpamax)
+    phase.addLowerVarBound("Last","mass",massmin)
+    phase.addLUVarBound("Path","alpha",alphamin,alphamax)
+    phase.addBoundaryValue("Last",
+                           ["h","v","fpa"],
+                           [htf,vtf,fpatf])
+    phase.addDeltaTimeObjective(1.0)
+    
+    phase.optimizer.PrintLevel=1
+    phase.setThreads(8,8)
+    
+    ## All error estimates and tolerances are in reference to the scaled ODE system
+    phase.setAdaptiveMesh(True)
+    phase.setMeshErrorEstimator("integrator")
+    phase.setMeshTol(1.0e-7)
+    phase.optimize()
+    
+    Traj = phase.returnTraj()
+
+    print("Minimum Time to Climb:{0:.2f}s".format(Traj[-1][4]))
+    
+    PhaseMeshErrorPlot(phase,False)
+    Plot(Traj)
+    
+    
+    
+    
+    
+    
+
+    
+    
+    
+    
+    
+        
+    
+    
+
+    
+    
+    
+    
+    
+
+        
+        
+        
+        
+        
+        
+        
+        
\ No newline at end of file
diff --git a/examples/UpdatedInterface/MinimumTimeToClimbTables.py b/examples/UpdatedInterface/MinimumTimeToClimbTables.py
new file mode 100644
index 00000000..e4f2d82a
--- /dev/null
+++ b/examples/UpdatedInterface/MinimumTimeToClimbTables.py
@@ -0,0 +1,168 @@
+import numpy as np
+import asset_asrl.VectorFunctions as vf
+import matplotlib.pyplot as plt
+
+
+'''
+Aerodynamic Data taken from ICLOS2s implentation of the problem
+http://www.ee.ic.ac.uk/ICLOCS/ExampleMinFuelClimb.html
+'''
+
+########################################################################################
+## Aerodynamic Data for airplane as function of mach number
+## Added in dummy data at Mach .6 and .75 to remove ossiclattions from fit
+AeroMach = [     0,    0.4, .6,  .75,   0.8,    0.9,  1.0,    1.2, 1.4,   1.6, 1.8]
+Clalpha =  [ 3.44,   3.44, 3.44,3.44,  3.44,   3.58, 4.44,   3.44, 3.01,  2.86, 2.44]
+CD0     = [ 0.013, 0.013, 0.013,0.013, 0.013, 0.014, 0.031, 0.041, 0.039, 0.036, 0.035]
+eta     = [ 0.54,   0.54, 0.54,0.54, 0.54,   0.75, 0.79,   0.78, 0.89,   0.93, 0.93]
+####################################################################################
+
+
+####################################################################################
+## Density and SOS data as function of altitude from 1976 US atmosphere model
+AtmosData = [
+    [-2000, 1.478e+00, 3.479e+02],
+    [0, 1.225e+00, 3.403e+02],
+    [2000, 1.007e+00, 3.325e+02],
+    [4000, 8.193e-01, 3.246e+02],
+    [6000, 6.601e-01, 3.165e+02],
+    [8000, 5.258e-01, 3.081e+02],
+    [10000, 4.135e-01, 2.995e+02],
+    [12000, 3.119e-01, 2.951e+02],
+    [14000, 2.279e-01, 2.951e+02],
+    [16000, 1.665e-01, 2.951e+02],
+    [18000, 1.216e-01, 2.951e+02],
+    [20000, 8.891e-02, 2.951e+02],
+    [22000, 6.451e-02, 2.964e+02],
+    [24000, 4.694e-02, 2.977e+02],
+    [26000, 3.426e-02, 2.991e+02],
+    [28000, 2.508e-02, 3.004e+02],
+    [30000, 1.841e-02, 3.017e+02],
+    [32000, 1.355e-02, 3.030e+02],
+    [34000, 9.887e-03, 3.065e+02],
+    [36000, 7.257e-03, 3.101e+02],
+    [38000, 5.366e-03, 3.137e+02],
+    [40000, 3.995e-03, 3.172e+02],
+    [42000, 2.995e-03, 3.207e+02],
+    [44000, 2.259e-03, 3.241e+02],
+    [46000, 1.714e-03, 3.275e+02],
+    [48000, 1.317e-03, 3.298e+02],
+    [50000, 1.027e-03, 3.298e+02],
+    [52000, 8.055e-04, 3.288e+02],
+    [54000, 6.389e-04, 3.254e+02],
+    [56000, 5.044e-04, 3.220e+02],
+    [58000, 3.962e-04, 3.186e+02],
+    [60000, 3.096e-04, 3.151e+02],
+    [62000, 2.407e-04, 3.115e+02],
+    [64000, 1.860e-04, 3.080e+02],
+    [66000, 1.429e-04, 3.044e+02],
+    [68000, 1.091e-04, 3.007e+02],
+    [70000, 8.281e-05, 2.971e+02],
+    [72000, 6.236e-05, 2.934e+02],
+    [74000, 4.637e-05, 2.907e+02],
+    [76000, 3.430e-05, 2.880e+02],
+    [78000, 2.523e-05, 2.853e+02],
+    [80000, 1.845e-05, 2.825e+02],
+    [82000, 1.341e-05, 2.797e+02],
+    [84000, 9.690e-06, 2.769e+02],
+    [86000, 6.955e-06, 2.741e+02]
+]
+
+AtmosData = np.array(AtmosData).T
+alts =  AtmosData[0]
+rhos = AtmosData[1]
+soss = AtmosData[2]
+
+####################################################################################
+# Thrust vs Mach and altitude
+# Solution wants to fly exactly at sea level. 
+# Padded with negative altitude data to prevent reads outside of table bounds
+
+
+ThrustMach = np.array([0, 0.2, 0.4, 0.6, 0.8, 1, 1.2, 1.4, 1.6, 1.8])
+ThrustAlt = 304.8*np.array([-.5,0, 5, 10, 15, 20, 25, 30, 40, 50, 70]);
+
+ThrustData = 4448.2 * np.array([
+    [24.2, 24.2, 24.0, 20.3, 17.3, 14.5, 12.2, 10.2, 5.7, 3.4, 0.1],
+    [28.0, 28.0, 24.6, 21.1, 18.1, 15.2, 12.8, 10.7, 6.5, 3.9, 0.2],
+    [28.3, 28.3, 25.2, 21.9, 18.7, 15.9, 13.4, 11.2, 7.3, 4.4, 0.4],
+    [30.8, 30.8, 27.2, 23.8, 20.5, 17.3, 14.7, 12.3, 8.1, 4.9, 0.8],
+    [34.5, 34.5, 30.3, 26.6, 23.2, 19.8, 16.8, 14.1, 9.4, 5.6, 1.1],
+    [37.9, 37.9, 34.3, 30.4, 26.8, 23.3, 19.8, 16.8, 11.2, 6.8, 1.4],
+    [36.1, 36.1, 38.0, 34.9, 31.3, 27.3, 23.6, 20.1, 13.4, 8.3, 1.7],
+    [36.1, 36.1, 36.6, 38.5, 36.1, 31.6, 28.1, 24.2, 16.2, 10.0, 2.2],
+    [36.1, 36.1, 35.2, 42.1, 38.7, 35.7, 32.0, 28.1, 19.3, 11.9, 2.9],
+    [36.1, 36.1, 33.8, 45.7, 41.3, 39.8, 34.6, 31.1, 21.7, 13.3, 3.1]
+]).T
+
+####################################################################
+
+####################################################################
+## Load Data into Tables
+
+rhoTab = vf.InterpTable1D(alts,rhos,kind='cubic')
+sosTab = vf.InterpTable1D(alts,soss,kind='cubic')
+ClalphaTab = vf.InterpTable1D(AeroMach,Clalpha,kind='cubic')
+etaTab = vf.InterpTable1D(AeroMach,eta,kind='cubic')
+CD0Tab = vf.InterpTable1D(AeroMach,CD0,kind='cubic')
+ThrustTab = vf.InterpTable2D(ThrustMach,ThrustAlt,ThrustData,kind='cubic')
+
+if __name__ == "__main__":
+    
+    from matplotlib import cm
+
+    Mach_t = np.linspace(0,1.8,1000)
+    alt_t = np.linspace(0,ThrustAlt[-1],1000)
+    
+    Ms,As = np.meshgrid(Mach_t,alt_t)
+    
+    fig1,axs1 = plt.subplots(3,1)
+    
+    axs1[0].plot(Mach_t,ClalphaTab(Mach_t).T)
+    axs1[1].plot(Mach_t,etaTab(Mach_t).T)
+    axs1[2].plot(Mach_t,CD0Tab(Mach_t).T)
+    
+    axs1[0].scatter(AeroMach,Clalpha)
+    axs1[1].scatter(AeroMach,eta)
+    axs1[2].scatter(AeroMach,CD0)
+    
+    
+    axs1[0].set_ylabel("CLalpha")
+    axs1[1].set_ylabel("eta")
+    axs1[2].set_ylabel("CD0")
+    axs1[2].set_xlabel("Mach")
+
+    fig2,axs2 = plt.subplots(2,1)
+
+    axs2[0].plot(alt_t,rhoTab(alt_t).T)
+    axs2[1].plot(alt_t,sosTab(alt_t).T)
+    
+    axs2[0].set_ylabel("Density(kg/m^3)")
+    axs2[1].set_ylabel("SOS(m/s)")
+    axs2[1].set_xlabel("Altitude (m)")
+    
+    fig3, ax3 = plt.subplots(subplot_kw={"projection": "3d"})
+    cs =  ax3.plot_surface(Ms,As,ThrustTab(Ms,As),cmap=cm.coolwarm)
+    
+    ax3.set_xlabel("Mach")
+    ax3.set_ylabel("Altitude (m)")
+    ax3.set_zlabel("Thrust (N)")
+
+    plt.show()
+    
+    
+    
+    
+    
+    
+
+    
+    
+    
+    
+    
+
+
+
+
+
diff --git a/examples/UpdatedInterface/MultiPhaseCannon.py b/examples/UpdatedInterface/MultiPhaseCannon.py
new file mode 100644
index 00000000..a906e613
--- /dev/null
+++ b/examples/UpdatedInterface/MultiPhaseCannon.py
@@ -0,0 +1,225 @@
+import numpy as np
+import asset_asrl as ast
+import asset_asrl.VectorFunctions as vf
+import asset_asrl.OptimalControl as oc
+from asset_asrl.VectorFunctions import Arguments as Args
+
+import matplotlib.pyplot as plt
+
+
+
+'''
+This example was taken from the Dymos Optimal control library. It is an excellent
+teaching example of what ODE parameters are used for.
+https://openmdao.github.io/dymos/examples/multi_phase_cannonball/multi_phase_cannonball.html
+Only difference is that we use a simple exponenital atnospheric density function
+
+Assuming constant materiel density, and launch energy we need to find
+the radius of the cannon ball that maximizes range.
+'''
+
+##########################################
+
+g0      =  9.81 
+Lstar   =  1000           ## m
+Tstar   =  60.0           ## sec
+Mstar   =  10             ## kgs
+Vstar   =  Lstar/Tstar
+
+CD      = .5
+RhoAir  = 1.225     
+RhoIron = 7870      
+h_scale = 8.44e3    
+E0      = 400000    
+g       = g0
+
+###########################################
+
+
+def MFunc(rad,RhoIron):return (4/3)*(np.pi*RhoIron)*(rad**3)
+def SFunc(rad):  return np.pi*(rad**2)
+    
+    
+##############################################################################
+
+class Cannon(oc.ODEBase):
+    def __init__(self, CD,RhoAir,RhoIron,h_scale,g):
+        ############################################################
+        args  = oc.ODEArguments(4,0,1)
+        
+        v     = args.XVar(0)
+        gamma = args.XVar(1)
+        h     = args.XVar(2)
+        r     = args.XVar(3)
+        
+        rad = args.PVar(0)
+        
+        S    = SFunc(rad)
+        M    = MFunc(rad,RhoIron)
+        
+        rho     = RhoAir * vf.exp(-h / h_scale)
+        
+        D       = (0.5*CD)*rho*(v**2)*S
+        
+        vdot     = -D/M - g*vf.sin(gamma)
+        gammadot = -g*vf.cos(gamma)/v
+        hdot     = v*vf.sin(gamma)
+        rdot     = v*vf.cos(gamma)
+        
+        ode = vf.stack([vdot,gammadot,hdot,rdot])
+        
+        ##############################################################
+        Vgroups = {}
+        Vgroups["v"]=v
+        Vgroups["gamma"]=gamma
+        Vgroups["h"]=h
+        Vgroups[("r","range")]=r
+        Vgroups["rad"]=rad
+        Vgroups["t"]=args.TVar()
+
+        super().__init__(ode,4,0,1,Vgroups=Vgroups)
+        
+        self.apogee_event = v*vf.sin(gamma)
+        self.ground_contact_event = h
+        
+        
+def EFunc():
+    v,rad =  Args(2).tolist()
+    M = MFunc(rad,RhoIron)
+    E = 0.5*M*(v**2)
+    return E - E0
+
+##############################################################################        
+def Plot(Ascent,Descent):
+    AT = np.array(Ascent).T
+    DT = np.array(Descent).T
+
+    
+    AT[1]*=180/np.pi
+    DT[1]*=180/np.pi
+
+    
+
+    fig = plt.figure()
+    ax0 = plt.subplot(421)
+    ax1 = plt.subplot(423)
+    ax2 = plt.subplot(425)
+    ax3 = plt.subplot(427)
+    axs =[ax0,ax1,ax2,ax3]
+
+    labs = [r'$v\; \frac{m}{s}$',r'$\gamma\;^\circ$', r'Altitude $m$', r' Range $m$']
+
+    for i in range(0,4):
+        
+        axs[i].plot(AT[4],AT[i],color='r')
+        axs[i].plot(DT[4],DT[i],color='b')
+        axs[i].set_ylabel(labs[i])
+        axs[i].set_xlabel(r"$t$")
+        
+        axs[i].grid(True)
+
+
+    ax4 = plt.subplot(122)
+    ax4.plot(AT[3],AT[2],color='r',label='Ascent')
+    ax4.plot(DT[3],DT[2],color='b',label='Descent')
+    ax4.grid(True)
+
+
+    ax4.set_ylabel(" Altitude (m) ")
+    ax4.set_xlabel(" Down Range (m) ")
+    ax4.legend()
+    
+    fig.set_size_inches(15.0, 7.5, forward=True)
+    fig.tight_layout()
+
+    plt.show()
+        
+
+##############################################################################        
+if __name__ == "__main__":
+
+    rad0   = .1 
+    h0     = 100 
+    r0     = 0
+    m0     = MFunc(rad0,RhoIron)
+    gamma0 = np.deg2rad(45)
+    v0     = np.sqrt(2*E0/m0)*.99
+    
+    
+    
+    ode = Cannon(CD,RhoAir,RhoIron,h_scale,g)
+    integ = ode.integrator(.01)
+    integ.setAbsTol(1.0e-12)    
+    
+    IG = np.zeros((6))
+    IG[0] = v0
+    IG[1] =gamma0
+    IG[2] = h0
+    IG[3] = r0
+    IG[5] = rad0
+    
+    
+    XtP0 = ode.make_input(v=v0,gamma=gamma0,h=h0,r=r0,rad=rad0)
+    
+    AscentIG = integ.integrate_dense(XtP0,60,[(ode.apogee_event,0,1)])[0]
+
+    DescentIG = integ.integrate_dense(AscentIG[-1],
+                                      AscentIG[-1][4]+ 1000,
+                                      [(ode.ground_contact_event,0,1)])[0]
+    
+
+    ##########################################################################
+    
+   
+    
+    tmode = "LGL5"
+    nsegs = 128
+    
+    units = ode.make_units(v=Vstar,h=Lstar,r=Lstar,rad=Lstar)
+    
+    aphase = ode.phase(tmode,AscentIG,nsegs)
+    aphase.setAutoScaling(True)
+    aphase.setUnits(units)
+    aphase.addLowerVarBound("ODEParams","rad",0.0)
+    aphase.addLowerVarBound("Front","gamma",0.0)
+    aphase.addBoundaryValue("Front",["h","r","t"],[h0,r0,0])
+    
+    aphase.addInequalCon("Front",EFunc(),["v"],["rad"],[])
+    aphase.addBoundaryValue("Back","gamma",0.0)
+        
+    dphase = ode.phase(tmode,DescentIG,nsegs)
+    dphase.setAutoScaling(True)
+    dphase.setUnits(units)
+    
+    dphase.addBoundaryValue("Back","h",0.0)
+    dphase.addValueObjective("Back","r",-1.0)
+    
+    ocp = oc.OptimalControlProblem()
+    ocp.setAutoScaling(True)
+    ocp.addPhase(aphase)
+    ocp.addPhase(dphase)
+    
+    # Enforce continuity in time dependent vars
+    ocp.addForwardLinkEqualCon(aphase,dphase,range(0,5))
+    # Enforce continuity in ODEParams
+    ocp.addParamLinkEqualCon(aphase,dphase,"ODEParams","rad")
+    
+    ocp.optimize()
+    
+    
+    Ascent  = aphase.returnTraj()
+    Descent = dphase.returnTraj()
+    
+    gammaopt = ode.get_vars("gamma",Ascent[0])[0]
+    ropt,radopt = ode.get_vars(["r","rad"],Descent[-1])
+    
+    
+    print("Launch Angle:",gammaopt*180/np.pi," deg")
+    print("Optimized Range:",ropt," m")
+    print("Optimized Radius:"  ,radopt," cm")
+
+    Plot(Ascent,Descent)
+
+    ##########################################################################
+
+
diff --git a/examples/UpdatedInterface/Reentry.py b/examples/UpdatedInterface/Reentry.py
new file mode 100644
index 00000000..4e5447fd
--- /dev/null
+++ b/examples/UpdatedInterface/Reentry.py
@@ -0,0 +1,260 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import asset_asrl as ast
+import asset_asrl.VectorFunctions as vf
+import asset_asrl.OptimalControl as oc
+from asset_asrl.VectorFunctions import Arguments as Args
+
+'''
+Space Shuttle Reentry
+Betts, J.T. Practical methods for Optimal Control and Estimation Using Nonlinear Programming, Cambridge University Press, 2009
+'''
+
+################### Non Dimensionalize ##################################
+g0 = 32.2 
+W  = 203000
+
+Lstar = 100000.0     ## feet
+Tstar = 60.0         ## sec
+Vstar = Lstar/Tstar
+
+
+tmax = 2500           
+Re = 20902900          
+S  = 2690.0            
+m  = (W/g0)            
+mu = (0.140765e17)    
+rho0 =.002378          
+h_ref = 23800        
+
+a0 = -.20704
+a1 = .029244
+
+b0 = .07854
+b1 = -.61592e-2
+b2 = .621408e-3
+
+c0 =  1.0672181
+c1 = -.19213774e-1
+c2 = .21286289e-3
+c3 = -.10117e-5
+
+Qlimit = 70.0
+
+##############################################################################
+class ShuttleReentry(oc.ODEBase):
+    def __init__(self):
+        
+        Xvars = 5
+        Uvars = 2
+        
+        ############################################################
+        XtU  = oc.ODEArguments(Xvars,Uvars)
+        
+        
+        h,theta,v,gamma,psi = XtU.XVec().tolist()
+        
+        alpha,beta = XtU.UVec().tolist()
+        
+        
+        alphadeg = (180.0/np.pi)*alpha
+        
+        CL  = a0 + a1*alphadeg
+        CD  = b0 + b1*alphadeg + b2*(alphadeg**2)
+        rho = rho0*vf.exp(-h/h_ref)
+        r   = h + Re
+        
+        L   = 0.5*CL*S*rho*(v**2)
+        D   = 0.5*CD*S*rho*(v**2)
+        g   = mu/(r**2)
+        
+        sgam = vf.sin(gamma)
+        cgam = vf.cos(gamma)
+        
+        sbet = vf.sin(beta)
+        cbet = vf.cos(beta)
+        
+        spsi = vf.sin(psi)
+        cpsi = vf.cos(psi)
+        tantheta = vf.tan(theta)
+        
+        hdot     = v*sgam
+        thetadot = (v/r)*cgam*cpsi
+        vdot     = -D/m - g*sgam
+        gammadot = (L/(m*v))*cbet +cgam*(v/r - g/v)
+        psidot   = L*sbet/(m*v*cgam) + (v/(r))*cgam*spsi*tantheta
+        
+    
+        ode = vf.stack([hdot,thetadot,vdot,gammadot,psidot])
+        
+        ## Create dict of aliases for different variables in the ODE
+        ## We can use these later in our constraints instead of integer indices
+        Vgroups = {}
+        Vgroups[('h','altitude')] =  h
+        Vgroups[("v","velocity")] =  v
+        Vgroups["theta"] =  theta
+        Vgroups["gamma"] =  gamma
+        Vgroups["psi"]   =  psi
+        Vgroups[("alpha","AoA")] =  alpha
+        Vgroups["beta"]  =  beta
+        Vgroups[("t","time")] =  XtU.TVar()
+
+        ##############################################################
+        super().__init__(ode,Xvars,Uvars,Vgroups = Vgroups) ## Pass to CTor
+
+def QFunc():
+    h,v,alpha = Args(3).tolist()
+    alphadeg = (180.0/np.pi)*alpha
+    rhodim = rho0*vf.exp(-h/h_ref)
+    vdim = v
+    qr = 17700*vf.sqrt(rhodim)*((.0001*vdim)**3.07)
+    qa = c0 + c1*alphadeg + c2*(alphadeg**2)+ c3*(alphadeg**3)
+    
+    return qa*qr
+ 
+#############################################################################
+
+def Plot(Traj1,Traj2):
+    TT1 = np.array(Traj1).T
+    TT2 = np.array(Traj2).T
+
+    fig, axs = plt.subplots(4,1)
+
+    axs[0].plot(TT1[5]/60.0,TT1[0]/5280,label='No Q limit',color='r')
+    axs[0].plot(TT2[5]/60.0,TT2[0]/5280,label='Q limited',color='b')
+    axs[0].set_ylabel("Altitude (Miles)")
+
+
+    axs[1].plot(TT1[5]/60.0,TT1[2],label='No Q limit',color='r')
+    axs[1].plot(TT2[5]/60.0,TT2[2],label='Q limited',color='b')
+
+    axs[1].set_ylabel(r"Velocity $\frac{ft}{s}$")
+
+
+    axs[2].plot(TT1[5]/60.0,np.rad2deg(TT1[6]),label='Angle of Attack No Q limit',color='r')
+    axs[2].plot(TT1[5]/60.0,np.rad2deg(TT1[7]),label='Bank Angle No Q limit',color='r',linestyle='dotted')
+    axs[2].plot(TT2[5]/60.0,np.rad2deg(TT2[6]),label='Angle of Attack  Q limited',color='b')
+    axs[2].plot(TT2[5]/60.0,np.rad2deg(TT2[7]),label='Bank Angle  Q limited',color='b',linestyle='dotted')
+
+    axs[2].set_ylabel("Angle(deg)")
+
+    qfunc = QFunc().eval(8,[0,2,6])
+
+    qs1 = [qfunc(T)[0] for T in Traj1]
+    qs2 = [qfunc(T)[0] for T in Traj2]
+
+    axs[3].plot(TT1[5]/60.0,qs1,label='No Q limit',color='r')
+    axs[3].plot(TT2[5]/60.0,qs2,label='Q limited',color='b')
+    axs[3].set_ylabel(r"Q $(\frac{BTU}{ft^2 *s})$")
+
+    axs[3].plot(TT2[5]/60.0,np.ones_like(TT2[5])*70,label=r'Q = 70 $\frac{BTU}{ft^2 *s}$',color='k',linestyle='dashed')
+
+
+    for i in range(0,4):
+        axs[i].grid(True)
+        axs[i].set_xlabel("Time (min)")
+        axs[i].legend()
+
+    fig.set_size_inches(8.0, 11.0, forward=True)
+    fig.tight_layout()
+
+    plt.show()
+    
+
+
+
+if __name__ == "__main__":
+    ##########################################################################
+    
+    ode = ShuttleReentry()
+
+    
+
+    tf  = 2000
+
+    ht0  = 260000
+    htf  = 80000 
+    vt0  = 25600
+    vtf  = 2500 
+
+    gammat0 = np.deg2rad(-1.0)
+    gammatf = np.deg2rad(-5.0)
+    psit0   = np.deg2rad(90.0)
+    
+    
+    ## Construct state,time,control vector using names with ode.make_input
+    X0 = ode.make_input(h=ht0,v=vt0,gamma=gammat0,psi=psit0)
+    Xf = ode.make_input(h=htf,v=vtf,gamma=gammatf,psi=psit0,t=tf)
+    # Lerp IG
+    TrajIG = [X0*(1.0-t) + Xf*t for t in np.linspace(0.,1.,200)]
+
+    ################################################################
+
+    phase = ode.phase("LGL5",TrajIG,40)
+
+    ############################
+    phase.setAutoScaling(True)  
+    ## Declare canonical units for state time, control, and parm variables
+    ## All default to 1, which is fine for all of our angular variables
+    ## Only need to scale altitude, velocity and time.
+    phase.setUnits(h = Lstar,
+                   v = Vstar,
+                   t = Tstar)
+    ###########################
+    
+    
+    phase.addBoundaryValue("Front",range(0,6),TrajIG[0][0:6])
+    
+    phase.addLUVarBound("Path","theta",np.deg2rad(-89.0),np.deg2rad(89.0))
+    phase.addLUVarBound("Path","gamma",np.deg2rad(-89.0),np.deg2rad(89.0))
+
+    phase.addLUVarBound("Path","AoA",np.deg2rad(-90.0),np.deg2rad(90.0))
+    phase.addLUVarBound("Path","beta" ,np.deg2rad(-90.0),np.deg2rad(1.0))
+    
+    phase.addUpperDeltaTimeBound(tmax,1.0)
+    
+    phase.addBoundaryValue("Back" 
+                              ,["h","v","gamma"]
+                              ,[htf,vtf,gammatf])
+    
+    phase.addDeltaVarObjective("theta",-1.0)
+    
+    phase.setThreads(8,8)
+    
+    phase.optimizer.set_SoeLSMode("L1")
+    phase.optimizer.set_OptLSMode("L1")
+    phase.optimizer.set_PrintLevel(1)
+    
+    ## All error estimates and tolerances are in reference to the scaled ODE system
+    phase.setAdaptiveMesh(True)
+    phase.setMeshTol(1.0e-7)
+    phase.setMeshErrorEstimator('integrator')
+    
+    phase.solve_optimize()
+
+    # Returns unscaled trajectory original units
+    Traj1 = phase.returnTraj()
+    
+    ## Add in Heating Rate Constraint
+    phase.addUpperFuncBound("Path",QFunc(),["h","v","alpha"],Qlimit)
+    
+    phase.optimize()
+    
+    Traj2 = phase.returnTraj()
+    
+    print("Final Time:",Traj1[-1][5],"(s) , Final Cross Range:",Traj1[-1][1]*180/np.pi, " deg")
+    print("Final Time:",Traj2[-1][5],"(s) , Final Cross Range:",Traj2[-1][1]*180/np.pi, " deg")
+   
+
+    Plot(Traj1,Traj2)
+
+
+    
+    
+
+
+ 
+
+
+
+
diff --git a/extensions/CMakeLists.txt b/extensions/CMakeLists.txt
index 120bc1a4..f414f468 100644
--- a/extensions/CMakeLists.txt
+++ b/extensions/CMakeLists.txt
@@ -17,7 +17,6 @@ add_dependencies(asset_extensions pch)
 target_include_directories(asset_extensions PRIVATE ${INCLUDE_DIRS})
 
 message(STATUS  ${INCLUDE_DIRS})
-message(STATUS "HERE")
 
 
 target_compile_options(asset_extensions PUBLIC ${COMPILE_FLAGS})
diff --git a/notices/ctpl-apache2.txt b/notices/ctpl-apache2.txt
index 2ce3a27b..9e40fa18 100644
--- a/notices/ctpl-apache2.txt
+++ b/notices/ctpl-apache2.txt
@@ -1,3 +1,6 @@
+License for ctpl thread pool included in src/Utils/ThreadPool.h. The code has been modified from its original version by renaming the original thread_pool class to
+ThreadPool and adding the push_member function to the class.
+
 Apache License
                            Version 2.0, January 2004
                         http://www.apache.org/licenses/
diff --git a/notices/eigen-mpl2.txt b/notices/eigen-mpl2.txt
index 14e2f777..f5b14762 100644
--- a/notices/eigen-mpl2.txt
+++ b/notices/eigen-mpl2.txt
@@ -1,3 +1,8 @@
+The file src/Solvers/PardisioInterface.h is based on the PardisoInterface.h file from the Eigen library (https://eigen.tuxfamily.org/). 
+The Eigen library is licensed under the Mozilla Public License Version 2.0 (https://www.mozilla.org/en-US/MPL/2.0/). The text of the Mozilla Public License Version 2.0 is included below.
+Note that the MPL2 license is only applied to src/Solvers/PardisioInterface.h and not the rest of the project. Changes include the addition of several member functions to access  internal PARDISO parameters, and additional
+methods to perform phases of the factorization without making unnecessary copies of the input matrix.
+
 Mozilla Public License Version 2.0
 ==================================
 
diff --git a/notices/keplerprop.txt b/notices/keplerprop.txt
new file mode 100644
index 00000000..86e5fb2d
--- /dev/null
+++ b/notices/keplerprop.txt
@@ -0,0 +1,26 @@
+Kepler propagator algorithm is based on the implementation found at
+https://www.mathworks.com/matlabcentral/fileexchange/35566-vectorized-analytic-two-body-propagator-kepler-universal-variables.
+License is included below.
+
+Copyright (c) 2012, Darin Koblick
+All rights reserved.
+
+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 OWNER 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.
\ No newline at end of file
diff --git a/notices/lambert.txt b/notices/lambert.txt
new file mode 100644
index 00000000..3c11778f
--- /dev/null
+++ b/notices/lambert.txt
@@ -0,0 +1,29 @@
+
+Izzo Lambert solver is partly based Rody Oldenhuis Matlab implementation https://github.com/rodyo/FEX-Lambert
+
+Copyright (c) 2018, Rody Oldenhuis
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+2. 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 OWNER 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.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of this project.
\ No newline at end of file
diff --git a/pypiwheel/setup.py.in b/pypiwheel/setup.py.in
index c6b1bc94..50618d9c 100644
--- a/pypiwheel/setup.py.in
+++ b/pypiwheel/setup.py.in
@@ -48,7 +48,7 @@ def main():
         license = 'Apache 2.0',
         url ='https://github.com/AlabamaASRL/asset_asrl',
         data_files=[
-          ('asset_asrl',['asset_asrl/asset-apache2.txt','asset_asrl/eigen-mpl2.txt','asset_asrl/fmt-mit.txt','asset_asrl/issl.pdf','asset_asrl/pybind11-bsd.txt']),
+          ('asset_asrl',['asset_asrl/asset-apache2.txt','asset_asrl/eigen-mpl2.txt','asset_asrl/fmt-mit.txt','asset_asrl/intel-issl.pdf','asset_asrl/pybind11-bsd.txt','asset_asrl/lambert.txt','asset_asrl/keplerprop.txt']),
         ],
         ext_modules=[
             Precompiled(
diff --git a/src/Astro/KeplerPropagator.h b/src/Astro/KeplerPropagator.h
index 49729583..d8bee83f 100644
--- a/src/Astro/KeplerPropagator.h
+++ b/src/Astro/KeplerPropagator.h
@@ -3,6 +3,36 @@
 
 namespace ASSET {
 
+/*
+Kepler propagator implementation is partly based on the implementation found at
+https://www.mathworks.com/matlabcentral/fileexchange/35566-vectorized-analytic-two-body-propagator-kepler-universal-variables
+Reproducing the license here for reference.
+
+
+Copyright (c) 2012, Darin Koblick
+All rights reserved.
+
+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 OWNER 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.
+
+*/
 
   struct KeperPropagator_Impl {
 
diff --git a/src/Astro/LambertSolvers.h b/src/Astro/LambertSolvers.h
index e1bcb5dc..6b5ea0c4 100644
--- a/src/Astro/LambertSolvers.h
+++ b/src/Astro/LambertSolvers.h
@@ -7,6 +7,43 @@ namespace ASSET {
   void LambertSolversBuild(FunctionRegistry& reg, py::module& m);
 
 
+
+
+  /*
+    Dario Izzo's Lambert algorithm vectorizable with Eigen::Array types. 2x increase in throughput with array size of 4 or 8.
+
+    Implementation based on Rudy OldenHuis's multiple-revolution matlab implementation https://github.com/rodyo/FEX-Lambert.
+    Reproducing his license here.
+
+    Copyright (c) 2018, Rody Oldenhuis
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+
+    1. Redistributions of source code must retain the above copyright notice, this
+       list of conditions and the following disclaimer.
+    2. 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 OWNER 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.
+
+    The views and conclusions contained in the software and documentation are those
+    of the authors and should not be interpreted as representing official policies,
+    either expressed or implied, of this project.The MIT License (MIT)
+   
+  */
+  
   template<class Scalar, class WayBool, class IntType, class BranchBool, class ExitInt>
   void lambert_izzo_impl(const Vector3<Scalar>& R1dim,
                          const Vector3<Scalar>& R2dim,
@@ -20,11 +57,7 @@ namespace ASSET {
                          ExitInt& exint) {
 
 
-    /// <summary>
-    /// An implementation of Dario Izzo's Lambert algorithm vectorized with Eigen::Array types.  2x increase
-    /// in throughput with array size of 4 or 8.
-    /// </summary>
-    /// <returns></returns>
+    
 
     constexpr bool RisScalar = std::is_floating_point<Scalar>::value;
     constexpr bool WayisScalar = std::is_same<WayBool, bool>::value;
@@ -36,8 +69,7 @@ namespace ASSET {
     constexpr int maxiters = 20;
 
 
-    // Using Eigen's array types as scalars causes annoying issues w/ the cross and norm methods, rewriting
-    // here
+    // Using Eigen's array types as scalars causes annoying issues w/ the cross and norm methods, rewriting here
     auto cross = [](const Vector3<Scalar>& x1, const Vector3<Scalar>& x2, Vector3<Scalar>& out) {
       out[0] = (x1[1] * x2[2] - x1[2] * x2[1]);
       out[1] = (x2[0] * x1[2] - x2[2] * x1[0]);
@@ -46,7 +78,6 @@ namespace ASSET {
 
     auto norm = [](const Vector3<Scalar>& x1) { return Scalar(sqrt(x1.dot(x1))); };
 
-
     Scalar lstar = norm(R1dim);
     Scalar vstar = sqrt(Scalar(mu) / lstar);
     Scalar tstar = lstar / vstar;
@@ -99,6 +130,7 @@ namespace ASSET {
 
     Scalar x1, x2, N;
 
+    // pre-calculating the initial guesses to remove log/tan calls, see OldenHuis github for origin
     if constexpr (NisScalar) {
       N = Scalar(double(Nin));
       if (int(Nin) == 0) {
@@ -108,20 +140,20 @@ namespace ASSET {
         // N = std::max(N, Nmax);
         if constexpr (BranchisScalar) {
           if (rightbranch) {
-            x1 = Scalar(tan(.7234 * Pi / 2.0));
-            x2 = Scalar(tan(.5234 * Pi / 2.0));
+            x1 = Scalar(2.154906449673281);
+            x2 = Scalar(1.076354179950629);
           } else {
-            x1 = Scalar(tan(-.5234 * Pi / 2.0));
-            x2 = Scalar(tan(-.2234 * Pi / 2.0));
+            x1 = Scalar(-1.0763541799506295);
+            x2 = Scalar(-0.36606678140737947);
           }
         } else {
           for (int i = 0; i < Scalar::SizeAtCompileTime; i++) {
             if (rightbranch[i]) {
-              x1[i] = tan(.7234 * Pi / 2.0);
-              x2[i] = tan(.5234 * Pi / 2.0);
+              x1[i] = 2.154906449673281;
+              x2[i] = 1.076354179950629;
             } else {
-              x1[i] = tan(-.5234 * Pi / 2.0);
-              x2[i] = tan(-.2234 * Pi / 2.0);
+              x1[i] = -1.0763541799506295;
+              x2[i] = -0.36606678140737947;
             }
           }
         }
@@ -135,19 +167,19 @@ namespace ASSET {
         } else {
           if constexpr (BranchisScalar) {
             if (rightbranch) {
-              x1[i] = tan(.7234 * Pi / 2.0);
-              x2[i] = tan(.5234 * Pi / 2.0);
+              x1[i] = 2.154906449673281;
+              x2[i] = 1.0763541799506295;
             } else {
-              x1[i] = tan(-.5234 * Pi / 2.0);
-              x2[i] = tan(-.2234 * Pi / 2.0);
+              x1[i] = -1.0763541799506295;
+              x2[i] = -0.36606678140737947;
             }
           } else {
             if (rightbranch[i]) {
-              x1[i] = tan(.7234 * Pi / 2.0);
-              x2[i] = tan(.5234 * Pi / 2.0);
+              x1[i] = 2.154906449673281;
+              x2[i] = 1.0763541799506295;
             } else {
-              x1[i] = tan(-.5234 * Pi / 2.0);
-              x2[i] = tan(-.2234 * Pi / 2.0);
+              x1[i] = -1.0763541799506295;
+              x2[i] = -0.36606678140737947;
             }
           }
         }
@@ -281,6 +313,8 @@ namespace ASSET {
       y1 = y2;
       y2 = yn;
       if constexpr (!RisScalar) {
+        // Fuzz y1 when vectorizing to prevent division by zero, not sure if this is the best way to do it
+        // Doesnt seem to hurt convergence or accuracy
         y1 += Scalar(1.0e-15);
       }
 
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ac4b6c7d..ce502b62 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -62,6 +62,7 @@ add_library(optimalcontrol STATIC OptimalControl/ASSET_OptimalControl.h
 OptimalControl/ASSET_OptimalControl.cpp
 OptimalControl/ODEPhaseBase.h
 OptimalControl/ODEPhaseBase.cpp
+OptimalControl/ODEPhaseBaseBind.cpp
 OptimalControl/GenericODESBuildPart1.cpp
 OptimalControl/GenericODESBuildPart2.cpp
 OptimalControl/GenericODESBuildPart3.cpp
@@ -72,6 +73,7 @@ OptimalControl/PhaseIndexer.h
 OptimalControl/PhaseIndexer.cpp
 OptimalControl/OptimalControlProblem.h
 OptimalControl/OptimalControlProblem.cpp
+OptimalControl/OptimalControlProblemBind.cpp
 OptimalControl/LGLInterpTable.h
 OptimalControl/LGLInterpTable.cpp
 )
diff --git a/src/Integrators/Integrator.h b/src/Integrators/Integrator.h
index 15c12a6e..c8bbfd2c 100644
--- a/src/Integrators/Integrator.h
+++ b/src/Integrators/Integrator.h
@@ -60,6 +60,7 @@ namespace ASSET {
     using DenseEventRet = std::tuple<DenseRet, EventLocsType>;
     using STMEventRet = std::tuple<IntegRet, Jacobian<double>, EventLocsType>;
 
+    using ControlIndexType = std::variant<int, Eigen::VectorXi, std::string, std::vector<std::string>>;;
 
     friend CentralShootingDefect<DODE, Integrator>;
 
@@ -97,14 +98,15 @@ namespace ASSET {
                std::string meth,
                double defstep,
                const ControllerType& ucon,
-               const Eigen::VectorXi& varlocs)
+               const ControlIndexType& varlocs_t)
         : Integrator() {
-      this->setMethod(meth, dode, defstep, true, ucon, varlocs);
+
+      this->setMethod(meth, dode, defstep, true, ucon, varlocs_t);
       this->setAbsTol(1.0e-12);  // Must Be called after setMethod!!!
       this->setRelTol(0);        // Must Be called after setMethod!!!
     }
-    Integrator(const DODE& dode, double defstep, const ControllerType& ucon, const Eigen::VectorXi& varlocs)
-        : Integrator(dode, "DOPRI87", defstep, ucon, varlocs) {
+    Integrator(const DODE& dode, double defstep, const ControllerType& ucon, const ControlIndexType& varlocs_t)
+        : Integrator(dode, "DOPRI87", defstep, ucon, varlocs_t) {
     }
     Integrator(const DODE& dode, double defstep, const Eigen::VectorXd& v) : Integrator() {
 
@@ -165,7 +167,7 @@ namespace ASSET {
                    double defstep,
                    bool usecontrol,
                    const GenericFunction<-1, -1>& ucon,
-                   const Eigen::VectorXi& varlocs) {
+                   const ControlIndexType & varlocs_t) {
 
       this->setStepSizes(defstep, defstep / 10000, defstep * 10000);
 
@@ -173,11 +175,11 @@ namespace ASSET {
         this->RKMethod = RKOptions::DOPRI54;
         this->ErrorOrder = 4;
         // Using DOPRI5 rather than DOPRI54 here is not a mistake
-        this->initStepperAndController<RKOptions::DOPRI5>(dode, usecontrol, ucon, varlocs);
+        this->initStepperAndController<RKOptions::DOPRI5>(dode, usecontrol, ucon, varlocs_t);
       } else if (str == "DOPRI87" || str == "DP87") {
         this->RKMethod = RKOptions::DOPRI87;
         this->ErrorOrder = 7;
-        this->initStepperAndController<RKOptions::DOPRI87>(dode, usecontrol, ucon, varlocs);
+        this->initStepperAndController<RKOptions::DOPRI87>(dode, usecontrol, ucon, varlocs_t);
       } else {
         throw std::invalid_argument("Invalid integration method '{0:}'.");
       }
@@ -188,9 +190,10 @@ namespace ASSET {
     void initStepperAndController(const DODE& odet,
                                   bool usecontrol,
                                   const GenericFunction<-1, -1>& ucon,
-                                  const Eigen::VectorXi& varlocs) {
+                                  const ControlIndexType& varlocs_t) {
 
       this->ode = odet;
+      Eigen::VectorXi varlocs = this->getvarlocs(varlocs_t);
       this->setIORows(this->ode.IRows() + 1, this->ode.IRows());
 
       this->usecontroller = usecontrol;
@@ -251,6 +254,45 @@ namespace ASSET {
       return this->stepper;
     }
 
+    Eigen::VectorXi getvarlocs(const ControlIndexType& varlocs_t) {
+
+
+        Eigen::VectorXi varlocs;
+
+        /////////////////////////////////////////////////
+        if (std::holds_alternative<int>(varlocs_t)) {
+            varlocs.resize(1);
+            varlocs[0] = std::get<int>(varlocs_t);
+        }
+        else if (std::holds_alternative<Eigen::VectorXi>(varlocs_t)) {
+            varlocs = std::get<Eigen::VectorXi>(varlocs_t);
+        }
+        else if (std::holds_alternative<std::string>(varlocs_t)) {
+            varlocs = this->ode.idx(std::get<std::string>(varlocs_t));
+        }
+        else if (std::holds_alternative<std::vector<std::string>>(varlocs_t)) {
+
+            std::vector<Eigen::VectorXi> varvec;
+            int size = 0;
+            auto tmpvars = std::get<std::vector<std::string>>(varlocs_t);
+
+            for (auto tmpv : tmpvars) {
+                varvec.push_back(this->ode.idx(tmpv));
+                size += varvec.back().size();
+            }
+            varlocs.resize(size);
+            int next = 0;
+            for (auto varv : varvec) {
+                for (int i = 0; i < varv.size(); i++) {
+                    varlocs[next] = varv[i];
+                    next++;
+                }
+            }
+        }
+        return varlocs;
+
+	}
+
 
     double ErrorOrder = 7.0;
     double MinStepSize = 0.1;
@@ -2202,7 +2244,7 @@ namespace ASSET {
                    std::string meth,
                    double ds,
                    const ControllerType& u,
-                   const Eigen::VectorXi& v) { return Integrator<DODE>(od, meth, ds, u, v); });
+                   const ControlIndexType& v) { return Integrator<DODE>(od, meth, ds, u, v); });
         obj.def("integrator",
                 [](const DODE& od,
                    std::string meth,
@@ -2214,7 +2256,7 @@ namespace ASSET {
                   return Integrator<DODE>(od, meth, ds, u);
                 });
         obj.def("integrator",
-                [](const DODE& od, double ds, const ControllerType& u, const Eigen::VectorXi& v) {
+                [](const DODE& od, double ds, const ControllerType& u, const ControlIndexType& v) {
                   return Integrator<DODE>(od, ds, u, v);
                 });
         obj.def("integrator", [](const DODE& od, double ds, const Eigen::VectorXd& v) {
@@ -2238,8 +2280,8 @@ namespace ASSET {
 
 
       if constexpr (DODE::UV != 0) {
-        obj.def(py::init<const DODE&, std::string, double, const ControllerType&, const Eigen::VectorXi&>());
-        obj.def(py::init<const DODE&, double, const ControllerType&, const Eigen::VectorXi&>());
+        obj.def(py::init<const DODE&, std::string, double, const ControllerType&, const ControlIndexType&>());
+        obj.def(py::init<const DODE&, double, const ControllerType&, const ControlIndexType&>());
         obj.def(py::init<const DODE&, double, const Eigen::VectorXd&>());
 
         obj.def(py::init<const DODE&,
diff --git a/src/OptimalControl/AutoScalingUtils.h b/src/OptimalControl/AutoScalingUtils.h
new file mode 100644
index 00000000..80bbd426
--- /dev/null
+++ b/src/OptimalControl/AutoScalingUtils.h
@@ -0,0 +1,71 @@
+#pragma once
+#include "pch.h"
+#include "VectorFunctions/ASSET_VectorFunctions.h"
+#include "CommonFunctions/IOScaled.h"
+
+
+namespace ASSET {
+
+	template<class Func>
+	Eigen::VectorXd calc_jacobian_row_scales(const Func & func, const Eigen::VectorXd & input_scales, 
+		std::vector<Eigen::VectorXd> & test_inputs,
+		std::string normtype,
+		std::string avgtype) {
+
+		Eigen::MatrixXd rownorms(func.ORows(), test_inputs.size());
+		Eigen::VectorXd output_scales(func.ORows());
+		output_scales.setOnes();
+		IOScaled<Func> scaled_func(func, input_scales, output_scales);
+
+
+		Eigen::VectorXd fx(func.ORows());
+		Eigen::MatrixXd jx(func.ORows(), func.IRows());
+
+		for (int i = 0; i < test_inputs.size(); i++) {
+			fx.setZero();
+			jx.setZero();
+			scaled_func.compute_jacobian(test_inputs[i], fx, jx);
+			for (int j = 0; j < func.ORows(); j++) {
+				if (normtype == "norm") {
+					rownorms(j, i) = jx.row(j).norm();
+				}
+				else if (normtype == "infnorm") {
+					rownorms(j, i) = jx.row(j).lpNorm<Eigen::Infinity>();
+				}
+				else {
+					throw std::invalid_argument("Unknown row norm type");
+				}
+			}
+		}
+
+		for (int j = 0; j < func.ORows(); j++) {
+			double avg = 1.0;
+			if (rownorms.row(j).mean() < 1.0e-12) {
+				
+			}
+			else if (!std::isfinite(rownorms.row(j).mean())) {
+
+			}
+			else if (avgtype == "mean") {
+				avg = rownorms.row(j).mean();
+			}
+			else if (avgtype == "geomean") {
+				avg = std::exp(rownorms.row(j).array().log().sum()/double(rownorms.cols()));
+			}
+			else {
+				throw std::invalid_argument("Unknown row average type");
+			}
+
+			output_scales[j] = 1.0 / avg;
+		}
+
+		return output_scales;
+
+
+	}
+
+
+
+
+}
+
diff --git a/src/OptimalControl/InterfaceTypes.h b/src/OptimalControl/InterfaceTypes.h
new file mode 100644
index 00000000..5da5cc94
--- /dev/null
+++ b/src/OptimalControl/InterfaceTypes.h
@@ -0,0 +1,72 @@
+#pragma once
+#include "pch.h"
+#include "OptimalControlFlags.h"
+#include <variant>
+#include <string>
+#include <vector>
+
+namespace ASSET {
+
+
+    using VarIndexType = std::variant<int, Eigen::VectorXi, std::string, std::vector<std::string>>;
+    using ScaleType    = std::variant<double, Eigen::VectorXd, std::string,py::none>;
+    using RegionType   = std::variant<PhaseRegionFlags, std::string>;
+
+    static PhaseRegionFlags get_PhaseRegion(RegionType reg_t)  {
+        PhaseRegionFlags reg;
+
+        if (std::holds_alternative<PhaseRegionFlags>(reg_t)) {
+            reg = std::get<PhaseRegionFlags>(reg_t);
+        }
+        else if (std::holds_alternative<std::string>(reg_t)) {
+            reg = strto_PhaseRegionFlag(std::get<std::string>(reg_t));
+        }
+        return reg;
+    }
+
+    static std::tuple<std::string,bool,Eigen::VectorXd> get_scale_info(int orows, ScaleType scale_t) {
+
+        Eigen::VectorXd OutputScales(orows);
+        OutputScales.setOnes();
+        std::string ScaleMode = "auto";
+        bool ScalesSet = false;
+        if (std::holds_alternative < double >(scale_t)) {
+            OutputScales *= std::get<double>(scale_t);
+            ScaleMode = "custom";
+            ScalesSet = true;
+
+        }
+        else if (std::holds_alternative<Eigen::VectorXd>(scale_t)) {
+            OutputScales = std::get<Eigen::VectorXd>(scale_t);
+            ScaleMode = "custom";
+            ScalesSet = true;
+
+            if (OutputScales.size() != orows) {
+                throw std::invalid_argument("Scaling vector size does not match output size of function");
+            }
+        }
+        else if (std::holds_alternative<std::string>(scale_t)) {
+            ScaleMode = std::get<std::string>(scale_t);
+
+            if (ScaleMode == "auto") {
+                ScalesSet = false;
+            }
+            else if (ScaleMode == "none") {
+                ScalesSet = true;
+            }
+            else {
+                throw std::invalid_argument(fmt::format("Unrecognized Scale Mode:{0:}", ScaleMode));
+            }
+        }
+        else if (std::holds_alternative<py::none>(scale_t)) {
+               ScaleMode = "none";
+               ScalesSet = true;
+        }
+
+
+        return std::tuple{ ScaleMode,ScalesSet,OutputScales };
+
+    }
+
+
+}
\ No newline at end of file
diff --git a/src/OptimalControl/LinkFunction.h b/src/OptimalControl/LinkFunction.h
index 38deef7b..5e8c0c9c 100644
--- a/src/OptimalControl/LinkFunction.h
+++ b/src/OptimalControl/LinkFunction.h
@@ -2,6 +2,7 @@
 
 #include "OptimalControlFlags.h"
 #include "pch.h"
+#include "InterfaceTypes.h"
 
 namespace ASSET {
 
@@ -18,6 +19,11 @@ namespace ASSET {
     std::vector<Eigen::VectorXi> SPVars;
     std::vector<Eigen::VectorXi> LinkParams;
 
+    std::string ScaleMode = "auto"; // auto,custom
+    bool ScalesSet = false;
+    Eigen::VectorXd OutputScales;
+
+
     int StorageIndex = 0;
     int GlobalIndex = 0;
 
@@ -89,6 +95,9 @@ namespace ASSET {
       this->OPVars = opv;
       this->SPVars = spv;
       this->LinkParams = lv;
+      this->OutputScales = Eigen::VectorXd::Ones(this->Func.ORows());
+
+
     }
 
     Eigen::Matrix<PhaseRegionFlags, -1, 1> makePhaseRegFlags(LinkFlags Flag) {
@@ -147,6 +156,23 @@ namespace ASSET {
                  std::vector<Eigen::VectorXi> lv) {
       this->init(f, RegFlags, PTL, xtv, opv, spv, lv);
     }
+
+    LinkFunction(FuncType f,
+        Eigen::Matrix<PhaseRegionFlags, -1, 1> RegFlags,
+        std::vector<Eigen::VectorXi> PTL,
+        std::vector<Eigen::VectorXi> xtv,
+        std::vector<Eigen::VectorXi> opv,
+        std::vector<Eigen::VectorXi> spv,
+        std::vector<Eigen::VectorXi> lv,
+        ScaleType scale_t) {
+        this->init(f, RegFlags, PTL, xtv, opv, spv, lv);
+
+        auto [ScaleMode, ScalesSet, OutputScales] = get_scale_info(this->Func.ORows(), scale_t);
+        this->OutputScales = OutputScales;
+        this->ScaleMode = ScaleMode;
+        this->ScalesSet = ScalesSet;
+    }
+
     LinkFunction(FuncType f,
                  LinkFlags Flag,
                  std::vector<Eigen::VectorXi> PTL,
@@ -156,7 +182,20 @@ namespace ASSET {
                  std::vector<Eigen::VectorXi> lv) {
       this->init(f, makePhaseRegFlags(Flag), PTL, xtv, opv, spv, lv);
     }
-
+    LinkFunction(FuncType f,
+        LinkFlags Flag,
+        std::vector<Eigen::VectorXi> PTL,
+        std::vector<Eigen::VectorXi> xtv,
+        std::vector<Eigen::VectorXi> opv,
+        std::vector<Eigen::VectorXi> spv,
+        std::vector<Eigen::VectorXi> lv,
+        ScaleType scale_t) {
+        this->init(f, makePhaseRegFlags(Flag), PTL, xtv, opv, spv, lv);
+        auto [ScaleMode, ScalesSet, OutputScales] = get_scale_info(this->Func.ORows(), scale_t);
+        this->OutputScales = OutputScales;
+        this->ScaleMode = ScaleMode;
+        this->ScalesSet = ScalesSet;
+    }
     LinkFunction(FuncType f,
                  std::vector<std::string> RegFlags,
                  std::vector<Eigen::VectorXi> PTL,
diff --git a/src/OptimalControl/ODE.h b/src/OptimalControl/ODE.h
index 21760d0d..aac1b3a6 100644
--- a/src/OptimalControl/ODE.h
+++ b/src/OptimalControl/ODE.h
@@ -147,6 +147,12 @@ namespace ASSET {
       obj.def("phase", [](const Derived& od, std::string Tmode) {
         return std::make_shared<ODEPhase<Derived>>(od, Tmode);
       });
+
+      obj.def("phase",
+          [](const Derived& od, std::string Tmode, const std::vector<Eigen::VectorXd>& Traj) {
+              return std::make_shared<ODEPhase<Derived>>(od, Tmode, Traj);
+          });
+
       obj.def("phase",
               [](const Derived& od, std::string Tmode, const std::vector<Eigen::VectorXd>& Traj, int numdef) {
                 return std::make_shared<ODEPhase<Derived>>(od, Tmode, Traj, numdef);
diff --git a/src/OptimalControl/ODEPhase.h b/src/OptimalControl/ODEPhase.h
index 4a3ba9a0..9eb16b77 100644
--- a/src/OptimalControl/ODEPhase.h
+++ b/src/OptimalControl/ODEPhase.h
@@ -32,8 +32,13 @@ namespace ASSET {
     template<class Scalar>
     using ODEDeriv = typename DODE::template Output<Scalar>;
 
+    using ScaledODE =  GenericODE<GenericFunction<-1, -1>, DODE::XV, DODE::UV, DODE::PV>;
+
 
     DODE ode;
+    ScaledODE ode_scaled;
+
+
     Integrator<DODE> integrator;
     bool EnableHessianSparsity = false;
     bool OldShootingDefect = false;
@@ -42,7 +47,10 @@ namespace ASSET {
         : ODEPhaseBase(ode.XVars(), ode.UVars(), ode.PVars()) {
       this->ode = ode;
       this->integrator = Integrator<DODE>(ode, .01);
+      this->set_idxs(this->ode.get_idxs());
       this->setTranscriptionMode(Tmode);
+      this->setUnits(Eigen::VectorXd::Ones(this->XtUPVars()));
+
     }
     ODEPhase(const DODE& ode, TranscriptionModes Tmode, const std::vector<Eigen::VectorXd>& Traj, int numdef)
         : ODEPhase(ode, Tmode) {
@@ -56,17 +64,104 @@ namespace ASSET {
         : ODEPhase(ode, Tmode) {
       this->setTraj(Traj, numdef, LerpIG);
     }
+    ODEPhase(const DODE& ode,
+        TranscriptionModes Tmode,
+        const std::vector<Eigen::VectorXd>& Traj)
+        : ODEPhase(ode, Tmode) {
+        this->setTraj(Traj);
+    }
 
     ODEPhase(const DODE& ode, std::string Tmode) : ODEPhase(ode, strto_TranscriptionMode(Tmode)) {
     }
     ODEPhase(const DODE& ode, std::string Tmode, const std::vector<Eigen::VectorXd>& Traj, int numdef)
         : ODEPhase(ode, strto_TranscriptionMode(Tmode), Traj, numdef) {
     }
+    ODEPhase(const DODE& ode, std::string Tmode, const std::vector<Eigen::VectorXd>& Traj)
+        : ODEPhase(ode, strto_TranscriptionMode(Tmode), Traj) {
+    }
     ODEPhase(
         const DODE& ode, std::string Tmode, const std::vector<Eigen::VectorXd>& Traj, int numdef, bool LerpIG)
         : ODEPhase(ode, strto_TranscriptionMode(Tmode), Traj, numdef, LerpIG) {
     }
 
+    void setUnits(const Eigen::VectorXd& XtUPUnits_) {
+
+        if (XtUPUnits_.size() != this->XtUPVars()) {
+            throw std::invalid_argument("Incorrect size for input units vector");
+        }
+
+        this->XtUPUnits = XtUPUnits_;
+        VectorXd output_scales = XtUPUnits.head(this->XVars()).cwiseInverse()*this->XtUPUnits[this->XVars()];
+        VectorFunctionalX odetemp;
+
+        if constexpr (DODE::IsGenericODE) {
+            odetemp = this->ode.func;
+        }
+        else {
+            odetemp = this->ode;
+        }
+
+        auto tmp = IOScaled<VectorFunctionalX>(odetemp, this->XtUPUnits, output_scales);
+
+        this->ode_scaled = ScaledODE(tmp, this->XVars(), this->UVars(), this->PVars());
+
+
+    }
+
+    virtual Integrator<ScaledODE> make_scaled_reintegrator() const {
+        Integrator<ScaledODE> Integ;
+        if (this->UVars() == 0 || this->ControlMode == BlockConstant) {
+            Integ = Integrator<ScaledODE>{ this->ode_scaled, this->integrator.DefStepSize };
+        }
+        else {
+            Eigen::VectorXi ulocs;
+            ulocs.setLinSpaced(this->UVars(), this->TVar() + 1, this->TVar() + this->UVars());
+
+            Eigen::VectorXi varlocs(1);
+            varlocs[0] = this->TVar();
+
+            double tscale = this->XtUPUnits[this->TVar()];
+            auto tabcon = InterpFunction<-1>(std::make_shared<LGLInterpTable>(this->Table), ulocs).eval(Arguments<1>(1) * tscale);
+            VectorXd Uscales = this->XtUPUnits.segment(this->TVar() + 1, this->UVars()).cwiseInverse();
+            auto ucon = RowScaled<decltype(tabcon)>(tabcon, Uscales);
+            Integ = Integrator<ScaledODE>{
+                this->ode_scaled, this->integrator.DefStepSize, ucon, varlocs };
+        }
+
+        Integ.Adaptive = this->integrator.Adaptive;
+        Integ.FastAdaptiveSTM = this->integrator.FastAdaptiveSTM;
+        Integ.AbsTols = this->integrator.AbsTols;
+        Integ.RelTols = this->integrator.RelTols;
+        Integ.MinStepSize = this->integrator.MinStepSize;
+        Integ.MaxStepSize = this->integrator.MaxStepSize;
+        Integ.EnableVectorization = this->EnableVectorization;
+
+        return Integ;
+    }
+
+    virtual Integrator<DODE> make_reintegrator() const{
+        Integrator<DODE> Integ;
+
+        if (this->UVars() == 0 || this->ControlMode == BlockConstant) {
+            Integ = Integrator<DODE>{ this->ode, this->integrator.DefStepSize };
+        }
+        else {
+            Integ = Integrator<DODE>{
+                this->ode, this->integrator.DefStepSize, std::make_shared<LGLInterpTable>(this->Table) };
+        }
+
+        Integ.Adaptive = this->integrator.Adaptive;
+        Integ.FastAdaptiveSTM = this->integrator.FastAdaptiveSTM;
+        Integ.AbsTols = this->integrator.AbsTols;
+        Integ.RelTols = this->integrator.RelTols;
+        Integ.MinStepSize = this->integrator.MinStepSize;
+        Integ.MaxStepSize = this->integrator.MaxStepSize;
+        Integ.EnableVectorization = this->EnableVectorization;
+
+        return Integ;
+    }
+
+
     virtual void setTranscriptionMode(TranscriptionModes m) {
       this->resetTranscription();
 
@@ -131,71 +226,104 @@ namespace ASSET {
       empty.resize(0);
 
 
-      auto lgldef = [&](auto cs) {
+      auto lgldef = [&](auto cs, auto ode_t) {
         if (this->ControlMode == BlockConstant) {
 
           if constexpr (DODE::UV == 0 && DODE::PV == 0) {
-            LGLType<DODE, cs.value> lgl(this->ode);
+            LGLType<decltype(ode_t), cs.value> lgl(ode_t);
             lgl.EnableVectorization = this->EnableVectorization;
             this->DynamicsFuncIndex = this->indexer.addEquality(
                 lgl, PhaseRegionFlags::DefectPath, StateT, OParT, empty, ThreadingFlags::ByApplication);
           } else {
-            LGLType<Blocked_ODE_Wrapper<DODE>, cs.value> lgl(Blocked_ODE_Wrapper<DODE>(this->ode));
+
+            using BlockedODE =  Blocked_ODE_Wrapper<decltype(ode_t)>;
+
+            auto lgl = LGLDefects<BlockedODE, cs.value>(BlockedODE(ode_t));
             lgl.EnableVectorization = this->EnableVectorization;
             this->DynamicsFuncIndex = this->indexer.addEquality(
                 lgl, PhaseRegionFlags::BlockDefectPath, StateT, OParT, empty, ThreadingFlags::ByApplication);
           }
         } else {
-          LGLType<DODE, cs.value> lgl(this->ode);
+          LGLType<decltype(ode_t), cs.value> lgl(ode_t);
           lgl.EnableVectorization = this->EnableVectorization;
           this->DynamicsFuncIndex = this->indexer.addEquality(
               lgl, PhaseRegionFlags::DefectPath, StateT, OParT, empty, ThreadingFlags::ByApplication);
         }
       };
 
+      auto trapdef = [&](auto ode_t) {
+
+          if (this->ControlMode == BlockConstant) {
+
+              if constexpr (DODE::UV == 0 && DODE::PV == 0) {
+                  TrapezoidalDefects<decltype(ode_t)> trap(ode_t);
+                  trap.EnableVectorization = this->EnableVectorization;
+                  this->DynamicsFuncIndex = this->indexer.addEquality(
+                      trap, PhaseRegionFlags::DefectPath, StateT, OParT, empty, ThreadingFlags::ByApplication);
+              }
+              else {
+                  using BlockedODE =  Blocked_ODE_Wrapper<decltype(ode_t)>;
+
+                  auto trap = TrapezoidalDefects<BlockedODE>(BlockedODE(ode_t));
+                  trap.EnableVectorization = this->EnableVectorization;
+                  //trap.EnableHessianSparsity = this->EnableHessianSparsity;
+                  this->DynamicsFuncIndex = this->indexer.addEquality(trap,
+                      PhaseRegionFlags::BlockDefectPath,
+                      StateT,
+                      OParT,
+                      empty,
+                      ThreadingFlags::ByApplication);
+              }
+
+
+          }
+          else {
+              TrapezoidalDefects<decltype(ode_t)> trap(ode_t);
+              trap.EnableVectorization = this->EnableVectorization;
+              this->DynamicsFuncIndex = this->indexer.addEquality(
+                  trap, PhaseRegionFlags::DefectPath, StateT, OParT, empty, ThreadingFlags::ByApplication);
+          }
+
+      };
+
 
       switch (this->TranscriptionMode) {
         case TranscriptionModes::LGL7: {
-          lgldef(int_const<4>());
+
+          if (this->AutoScaling) {
+              lgldef(int_const<4>(),this->ode_scaled);
+          }
+          else {
+              lgldef(int_const<4>(),this->ode);
+          }
+
           break;
         }
         case TranscriptionModes::LGL5: {
-          lgldef(int_const<3>());
+            if (this->AutoScaling) {
+                lgldef(int_const<3>(), this->ode_scaled);
+            }
+            else {
+                lgldef(int_const<3>(), this->ode);
+            }
           break;
         }
         case TranscriptionModes::LGL3: {
-          lgldef(int_const<2>());
+            if (this->AutoScaling) {
+                lgldef(int_const<2>(), this->ode_scaled);
+            }
+            else {
+                lgldef(int_const<2>(), this->ode);
+            }
           break;
         }
         case TranscriptionModes::Trapezoidal: {
-          if (this->ControlMode == BlockConstant) {
-
-            if constexpr (DODE::UV == 0 && DODE::PV == 0) {
-              TrapezoidalDefects<DODE> trap(this->ode);
-              trap.EnableVectorization = this->EnableVectorization;
-              // trap.EnableHessianSparsity = this->EnableHessianSparsity;
-              this->DynamicsFuncIndex = this->indexer.addEquality(
-                  trap, PhaseRegionFlags::DefectPath, StateT, OParT, empty, ThreadingFlags::ByApplication);
-            } else {
-              TrapezoidalDefects<Blocked_ODE_Wrapper<DODE>> trap(Blocked_ODE_Wrapper<DODE>(this->ode));
-              trap.EnableVectorization = this->EnableVectorization;
-              trap.EnableHessianSparsity = this->EnableHessianSparsity;
-              this->DynamicsFuncIndex = this->indexer.addEquality(trap,
-                                                                  PhaseRegionFlags::BlockDefectPath,
-                                                                  StateT,
-                                                                  OParT,
-                                                                  empty,
-                                                                  ThreadingFlags::ByApplication);
+            if (this->AutoScaling) {
+                trapdef(this->ode_scaled);
+            }
+            else {
+                trapdef(this->ode);
             }
-
-
-          } else {
-            TrapezoidalDefects<DODE> trap(this->ode);
-            trap.EnableVectorization = this->EnableVectorization;
-            // trap.EnableHessianSparsity = this->EnableHessianSparsity;
-            this->DynamicsFuncIndex = this->indexer.addEquality(
-                trap, PhaseRegionFlags::DefectPath, StateT, OParT, empty, ThreadingFlags::ByApplication);
-          }
           break;
         }
         case TranscriptionModes::CentralShooting: {
@@ -213,75 +341,104 @@ namespace ASSET {
     }
 
     virtual ASSET::ConstraintInterface make_shooter() {
-      auto Integ = Integrator<DODE> {this->ode, this->integrator.DefStepSize};
-      Integ.Adaptive = this->integrator.Adaptive;
-      Integ.FastAdaptiveSTM = this->integrator.FastAdaptiveSTM;
-      Integ.AbsTols = this->integrator.AbsTols;
-      Integ.MinStepSize = this->integrator.MinStepSize;
-      Integ.MaxStepSize = this->integrator.MaxStepSize;
-      Integ.EnableVectorization = this->EnableVectorization;
-      Integ.VectorizeBatchCalls = this->integrator.VectorizeBatchCalls;
-
-      if (OldShootingDefect) {
-        auto shooter = ShootingDefect {this->ode, Integ};
-        shooter.EnableHessianSparsity = this->EnableHessianSparsity;
-        return ASSET::ConstraintInterface(shooter);
-      } else {
-        auto shooter = CentralShootingDefect {this->ode, Integ};
-        shooter.EnableHessianSparsity = this->EnableHessianSparsity;
-        shooter.EnableVectorization = this->EnableVectorization;
-        return ASSET::ConstraintInterface(shooter);
+
+
+      if (this->AutoScaling) {
+          auto Integ = Integrator<ScaledODE>{ this->ode_scaled, this->integrator.DefStepSize };
+          Integ.Adaptive = this->integrator.Adaptive;
+          Integ.FastAdaptiveSTM = this->integrator.FastAdaptiveSTM;
+          Integ.AbsTols = this->integrator.AbsTols;
+          Integ.MinStepSize = this->integrator.MinStepSize;
+          Integ.MaxStepSize = this->integrator.MaxStepSize;
+          Integ.EnableVectorization = this->EnableVectorization;
+          Integ.VectorizeBatchCalls = this->integrator.VectorizeBatchCalls;
+
+          if (OldShootingDefect) {
+              auto shooter = ShootingDefect{ this->ode_scaled, Integ };
+              shooter.EnableHessianSparsity = this->EnableHessianSparsity;
+              return ASSET::ConstraintInterface(shooter);
+          }
+          else {
+              auto shooter = CentralShootingDefect{ this->ode_scaled, Integ };
+              shooter.EnableHessianSparsity = this->EnableHessianSparsity;
+              shooter.EnableVectorization = this->EnableVectorization;
+              return ASSET::ConstraintInterface(shooter);
+          }
+
+      } {
+
+          auto Integ = Integrator<DODE>{ this->ode, this->integrator.DefStepSize };
+          Integ.Adaptive = this->integrator.Adaptive;
+          Integ.FastAdaptiveSTM = this->integrator.FastAdaptiveSTM;
+          Integ.AbsTols = this->integrator.AbsTols;
+          Integ.MinStepSize = this->integrator.MinStepSize;
+          Integ.MaxStepSize = this->integrator.MaxStepSize;
+          Integ.EnableVectorization = this->EnableVectorization;
+          Integ.VectorizeBatchCalls = this->integrator.VectorizeBatchCalls;
+
+          if (OldShootingDefect) {
+              auto shooter = ShootingDefect{ this->ode, Integ };
+              shooter.EnableHessianSparsity = this->EnableHessianSparsity;
+              return ASSET::ConstraintInterface(shooter);
+          }
+          else {
+              auto shooter = CentralShootingDefect{ this->ode, Integ };
+              shooter.EnableHessianSparsity = this->EnableHessianSparsity;
+              shooter.EnableVectorization = this->EnableVectorization;
+              return ASSET::ConstraintInterface(shooter);
+          }
+
       }
+
+      
     }
 
 
     virtual Eigen::VectorXd calc_global_error() const {
-      LGLInterpTable tabtmp = this->Table;
-      tabtmp.loadExactData(this->ActiveTraj);
-
-      Integrator<DODE> Integ;
+      
+        auto CalcError = [&](auto& Integ, const auto& Traj) {
 
-      if (this->UVars() != 0) {
-        Integ = Integrator<DODE> {
-            this->ode, this->integrator.DefStepSize, std::make_shared<LGLInterpTable>(tabtmp)};
-      } else {
-        Integ = Integrator<DODE> {this->ode, this->integrator.DefStepSize};
-      }
+            ODEState<double> Xin;
+            ODEState<double> Xout;
 
+            double T0 = Traj[0][this->TVar()];
+            double TF = Traj.back()[this->TVar()];
 
-      Integ.Adaptive = this->integrator.Adaptive;
-      Integ.FastAdaptiveSTM = this->integrator.FastAdaptiveSTM;
-      Integ.AbsTols = this->integrator.AbsTols;
-      Integ.RelTols = this->integrator.RelTols;
-      Integ.MinStepSize = this->integrator.MinStepSize;
-      Integ.MaxStepSize = this->integrator.MaxStepSize;
-      Integ.EnableVectorization = this->EnableVectorization;
+            int BlockSize = this->numTranCardStates;
+            int numBlocks = (Traj.size() - 1) / (BlockSize - 1);
 
-      ODEState<double> Xin;
-      ODEState<double> Xout;
 
+            Xin =Traj[0];
 
-      double T0 = this->ActiveTraj[0][this->TVar()];
-      double TF = this->ActiveTraj.back()[this->TVar()];
+            for (int i = 0; i < numBlocks; i++) {
+                int start = (BlockSize - 1) * i;
+                int stop = (BlockSize - 1) * (i + 1);
 
+                double tf = Traj[stop][this->TVar()];
+                Xout = Integ.integrate(Xin, tf);
+                Xin = Xout;
+            }
 
-      int BlockSize = this->numTranCardStates;
-      int numBlocks = (this->ActiveTraj.size() - 1) / (BlockSize - 1);
-      ;
+            Eigen::VectorXd gerror = (Traj.back() - Xout).head(this->XVars()).cwiseAbs();
+            return gerror;
+        };
 
-      Xin = this->ActiveTraj[0];
 
-      for (int i = 0; i < numBlocks; i++) {
-        int start = (BlockSize - 1) * i;
-        int stop = (BlockSize - 1) * (i + 1);
+      if (this->AutoScaling) {
+          Integrator<ScaledODE> Integ = this->make_scaled_reintegrator();
+          auto ActiveTrajTmp = this->ActiveTraj;
+          for (auto& T : ActiveTrajTmp) {
+              T = T.cwiseQuotient(this->XtUPUnits);
+          }
+          return CalcError(Integ, ActiveTrajTmp);
 
-        double tf = this->ActiveTraj[stop][this->TVar()];
-        Xout = Integ.integrate(Xin, tf);
-        Xin = Xout;
+      }
+      else {
+          Integrator<DODE> Integ = this->make_reintegrator();
+          return CalcError(Integ, this->ActiveTraj);
       }
 
-      Eigen::VectorXd gerror = (this->ActiveTraj.back() - Xout).head(this->XVars()).cwiseAbs();
-      return gerror;
+
     }
 
     virtual void get_meshinfo_deboor(Eigen::VectorXd& tsnd,
@@ -293,8 +450,7 @@ namespace ASSET {
 
       int BlockSize = this->numTranCardStates;
       int numBlocks = (this->ActiveTraj.size() - 1) / (BlockSize - 1);
-      ;
-
+      
 
       mesh_errors.resize(this->XVars(), numBlocks + 1);
       mesh_dist.resize(this->XVars(), numBlocks + 1);
@@ -395,6 +551,18 @@ namespace ASSET {
         yvecs[i] = yvec;
       }
 
+      /////////////////////////
+      if (this->AutoScaling) {
+          // All errors assessed in scaled units
+          // yvecs has dims of X/t^Order
+          for (int i = 0; i < numBlocks;i++) {
+              yvecs[i] = (yvecs[i].cwiseQuotient(this->XtUPUnits.head(this->XVars()))).eval();
+              yvecs[i] *= std::pow(this->XtUPUnits[this->XVars()], this->Order);
+              hs[i] /= this->XtUPUnits[this->XVars()];
+          }
+      }
+      ////////////////////////
+
       tsnd[numBlocks] = 1.0;
 
 
@@ -425,95 +593,131 @@ namespace ASSET {
                                          Eigen::MatrixXd& mesh_errors,
                                          Eigen::MatrixXd& mesh_dist) const {
 
-      Integrator<DODE> Integ;
 
-      if (this->UVars() == 0 || this->ControlMode == BlockConstant) {
-        Integ = Integrator<DODE> {this->ode, this->integrator.DefStepSize};
-      } else {
-        Integ = Integrator<DODE> {
-            this->ode, this->integrator.DefStepSize, std::make_shared<LGLInterpTable>(this->Table)};
-      }
 
 
-      Integ.Adaptive = this->integrator.Adaptive;
-      Integ.FastAdaptiveSTM = this->integrator.FastAdaptiveSTM;
-      Integ.AbsTols = this->integrator.AbsTols;
-      Integ.RelTols = this->integrator.RelTols;
-      Integ.MinStepSize = this->integrator.MinStepSize;
-      Integ.MaxStepSize = this->integrator.MaxStepSize;
-      Integ.EnableVectorization = this->EnableVectorization;
+      auto CalcError = [&](auto& Integ, const auto& Traj) {
 
+          
+          double T0 = Traj[0][this->TVar()];
+          double TF = Traj.back()[this->TVar()];
 
-     
+          int BlockSize = this->numTranCardStates;
+          int numBlocks = (Traj.size() - 1) / (BlockSize - 1);
 
-      double T0 = this->ActiveTraj[0][this->TVar()];
-      double TF = this->ActiveTraj.back()[this->TVar()];
+          mesh_errors.resize(this->XVars(), numBlocks + 1);
+          mesh_dist.resize(this->XVars(), numBlocks + 1);
+          tsnd.resize(numBlocks + 1);
 
-      int BlockSize = this->numTranCardStates;
-      int numBlocks = (this->ActiveTraj.size() - 1) / (BlockSize - 1);
+          Eigen::MatrixXd tmp_mat(this->XVars(), Traj.size() - 1);
 
-      mesh_errors.resize(this->XVars(), numBlocks + 1);
-      mesh_dist.resize(this->XVars(), numBlocks + 1);
-      tsnd.resize(numBlocks + 1);
 
-      Eigen::MatrixXd tmp_mat(this->XVars(), this->ActiveTraj.size() - 1);
+          std::vector<ODEState<double>> Xins(Traj.size() - 1);
+          Eigen::VectorXd tfs(Traj.size() - 1);
 
+          for (int i = 0; i < Traj.size() - 1; i++) {
+              Xins[i] = Traj[i];
+              tfs[i] = Traj[i + 1][this->TVar()];
+          }
+          auto Xouts = Integ.integrate(Xins, tfs);
 
-      std::vector<ODEState<double>> Xins(this->ActiveTraj.size() - 1);
-      Eigen::VectorXd tfs(this->ActiveTraj.size() - 1);
+          for (int i = 0; i < Traj.size() - 1; i++) {
+              tmp_mat.col(i) =
+                  (Xouts[i].head(this->XVars()) - Traj[i + 1].head(this->XVars())).cwiseAbs();
+          }
 
-      for (int i = 0; i < this->ActiveTraj.size() - 1; i++) {
-        Xins[i] = this->ActiveTraj[i];
-        tfs[i] = this->ActiveTraj[i + 1][this->TVar()];
-      }
-      auto Xouts = Integ.integrate(Xins, tfs);
 
-      for (int i = 0; i < this->ActiveTraj.size() - 1; i++) {
-        tmp_mat.col(i) =
-            (Xouts[i].head(this->XVars()) - this->ActiveTraj[i + 1].head(this->XVars())).cwiseAbs();
-      }
+          double max_err = tmp_mat.maxCoeff();
+          ODEDeriv<double> evec(this->XVars());
 
-      
-      double max_err = tmp_mat.maxCoeff();
-      ODEDeriv<double> evec(this->XVars());
+          for (int i = 0; i < numBlocks; i++) {
+              int start = (BlockSize - 1) * i;
+              int stop = (BlockSize - 1) * (i + 1);
 
-      for (int i = 0; i < numBlocks; i++) {
-        int start = (BlockSize - 1) * i;
-        int stop = (BlockSize - 1) * (i + 1);
+              double t0 = Traj[start][this->TVar()];
+              double tf = Traj[stop][this->TVar()];
 
-        double t0 = this->ActiveTraj[start][this->TVar()];
-        double tf = this->ActiveTraj[stop][this->TVar()];
+              tsnd[i] = (t0 - T0) / (TF - T0);
 
-        tsnd[i] = (t0 - T0) / (TF - T0);
+              evec.setZero();
 
-        evec.setZero();
+              for (int j = 0; j < BlockSize - 1; j++) {
+                  evec += tmp_mat.col(start + j) / (BlockSize - 1);
+              }
 
-        for (int j = 0; j < BlockSize - 1; j++) {
-          evec += tmp_mat.col(start + j) / (BlockSize - 1);
-        }
+              evec.setZero();
 
-        evec.setZero();
+              for (int j = 0; j < BlockSize - 1; j++) {
+                  double ti = Traj[start + j][this->TVar()];
+                  double tn = Traj[start + j + 1][this->TVar()];
 
-        for (int j = 0; j < BlockSize - 1; j++) {
-          double ti = this->ActiveTraj[start + j][this->TVar()];
-          double tn = this->ActiveTraj[start + j + 1][this->TVar()];
+                  evec += tmp_mat.col(start + j) * std::abs((tn - ti) / (tf - t0));
 
-          evec += tmp_mat.col(start + j) * std::abs((tn - ti) / (tf - t0));
-          
-        }
+              }
+
+              double h = std::abs(tf - t0);
+              mesh_errors.col(i) = evec;
+              mesh_dist.col(i) = mesh_errors.col(i) / (std::pow(h, this->Order + 1) * max_err);
+              mesh_dist.col(i) = (mesh_dist.col(i).array().pow(1 / (this->Order + 1))).eval();
+          }
 
-        double h = std::abs(tf - t0);
-        mesh_errors.col(i) = evec;
-        mesh_dist.col(i) = mesh_errors.col(i) / (std::pow(h, this->Order + 1) * max_err);
-        mesh_dist.col(i) = (mesh_dist.col(i).array().pow(1 / (this->Order + 1))).eval();
+          mesh_errors.col(numBlocks) = mesh_errors.col(numBlocks - 1);
+          mesh_dist.col(numBlocks) = mesh_dist.col(numBlocks - 1);
+          tsnd[numBlocks] = 1.0;
+
+
+      };
+     
+      if (this->AutoScaling) {
+          Integrator<ScaledODE> Integ = this->make_scaled_reintegrator();
+          auto ActiveTrajTmp = this->ActiveTraj;
+          for (auto& T : ActiveTrajTmp) {
+              T = T.cwiseQuotient(this->XtUPUnits);
+          }
+          return CalcError(Integ, ActiveTrajTmp);
+
+      }
+      else {
+          Integrator<DODE> Integ = this->make_reintegrator();
+          return CalcError(Integ, this->ActiveTraj);
       }
+      
+    }
 
-      mesh_errors.col(numBlocks) = mesh_errors.col(numBlocks - 1);
-      mesh_dist.col(numBlocks) = mesh_dist.col(numBlocks - 1);
-      tsnd[numBlocks] = 1.0;
+    VectorFunctionalX get_defect() {
+        VectorFunctionalX func;
+
+
+        switch (this->TranscriptionMode) {
+        case TranscriptionModes::LGL7: {
+            return func = LGLDefects<DODE, 4>(this->ode);
+            break;
+        }
+        case TranscriptionModes::LGL5: {
+            return func = LGLDefects<DODE, 3>(this->ode);
+            break;
+        }
+        case TranscriptionModes::LGL3: {
+            return func = LGLDefects<DODE, 2>(this->ode);
+            break;
+        }
+        case TranscriptionModes::Trapezoidal: {
+            return func = LGLDefects<DODE, 2>(this->ode);
+            break;
+        }
+        case TranscriptionModes::CentralShooting: {
+            return func = LGLDefects<DODE, 2>(this->ode);
+            break;
+        }
+        default:
+            throw std::invalid_argument("Invalid Transcription Method");
+        }
+
+        return func;
     }
 
 
+    
 
     template<class PyClass>
     static void BuildImpl(PyClass& phase) {
@@ -522,6 +726,8 @@ namespace ASSET {
       phase.def(py::init<DODE, TranscriptionModes, const std::vector<Eigen::VectorXd>&, int, bool>());
 
       phase.def(py::init<DODE, std::string>());
+      phase.def(py::init<DODE, std::string, const std::vector<Eigen::VectorXd>&>());
+
       phase.def(py::init<DODE, std::string, const std::vector<Eigen::VectorXd>&, int>());
       phase.def(py::init<DODE, std::string, const std::vector<Eigen::VectorXd>&, int, bool>());
     }
@@ -533,6 +739,13 @@ namespace ASSET {
       phase.def_readwrite("integrator", &ODEPhase<DODE>::integrator);
       phase.def_readwrite("EnableHessianSparsity", &ODEPhase<DODE>::EnableHessianSparsity);
       phase.def_readwrite("OldShootingDefect", &ODEPhase<DODE>::OldShootingDefect);
+
+
+      phase.def("get_input_scale", &ODEPhase<DODE>::get_input_scale);
+      phase.def("get_defect", &ODEPhase<DODE>::get_defect);
+
+
+
     }
   };
 
diff --git a/src/OptimalControl/ODEPhaseBase.cpp b/src/OptimalControl/ODEPhaseBase.cpp
index 6d86f435..6b594d5d 100644
--- a/src/OptimalControl/ODEPhaseBase.cpp
+++ b/src/OptimalControl/ODEPhaseBase.cpp
@@ -1,427 +1,433 @@
 #include "ODEPhaseBase.h"
-
 #include "LGLControlSplines.h"
 #include "LGLIntegrals.h"
 #include "MeshSpacingConstraints.h"
 #include "PyDocString/OptimalControl/ODEPhaseBase_doc.h"
 #include "ValueLock.h"
+#include "AutoScalingUtils.h"
 
-int ASSET::ODEPhaseBase::calc_threads() {
-  if (this->Threads > 1 && false) {
-    return this->Threads;
-  }
 
-  auto Cost = [](int N, int T, int Tmax, int V) {
-    int N_t = N / T;
-    int Nmt = N_t % V;
-    int NN = N % T;
-    int NN_t = NN / T;
-    int NNmt = NN_t % V;
+void ASSET::ODEPhaseBase::setUnits(const py::kwargs& kwargs)
+{
+   
 
-    double tscale = double(T) / double(std::min(T, Tmax));
-  };
+    py::module builtins = py::module::import("builtins");
+    py::object py_int = builtins.attr("int");
+    py::object py_float = builtins.attr("float");
+    py::object py_list = builtins.attr("list");
+    py::object np_array = (py::object)py::module::import("numpy").attr("ndarray");
+    py::object np_float = (py::object)py::module::import("numpy").attr("float64");
+    py::object np_int = (py::object)py::module::import("numpy").attr("int32");
 
-  return 0;
-}
 
-int ASSET::ODEPhaseBase::addDeltaVarEqualCon(PhaseRegionFlags reg, int var, double value, double scale) {
-  auto args = Arguments<2>();
-  auto x0 = args.coeff<0>();
-  auto x1 = args.coeff<1>();
+    VectorXd Units(this->XtUPVars());
+    Units.setOnes();
 
-  auto func = ((x1 - x0) - value) * scale;
-  VectorXi v(1);
-  v[0] = var;
-  return this->addEqualCon(reg, func, v);
-}
 
-Eigen::VectorXi ASSET::ODEPhaseBase::addBoundaryValues(PhaseRegionFlags reg,
-                                                       VectorXi args,
-                                                       const VectorXd& value) {
-  if (args.size() != value.size()) {
-    fmt::print(
-        fmt::fg(fmt::color::red),
-        "Transcription Error!!!\n"
-        "Size of boundary value arguments({0:}) and size of boundary value vector({1:}) do not match\n",
-        args.size(),
-        value.size());
-    throw std::invalid_argument("");
-  }
-  auto Args = Arguments<1>();
-  VectorXi fnums(args.size());
-  for (int i = 0; i < args.size(); i++) {
-    VectorXi atemp(1);
-    atemp[0] = args[i];
-    auto bv = Args - value[i];
-    fnums[i] = this->addEqualCon(StateConstraint(bv, reg, atemp));
-  }
-  return fnums;
+
+    for (auto & kw : kwargs) {
+        auto name = kw.first.cast<std::string>();
+        auto idxs = this->idx(name);
+        VectorXd units(idxs.size());
+        units.setOnes();
+
+        if (kw.second.get_type().is(py_int)   ||
+            kw.second.get_type().is(py_float) ||
+            kw.second.get_type().is(np_float) ||
+            kw.second.get_type().is(np_int)) {
+            double unit = kw.second.cast<double>();
+            units *= unit;
+        }
+        else if (
+            kw.second.get_type().is(np_array) ||
+            kw.second.get_type().is(py_list)) {
+
+            int lenvec = kw.second.attr("__len__")().cast<int>();
+
+            if (lenvec != idxs.size()) {
+                throw std::invalid_argument(fmt::format("Size of index group {0:} does not match units vector.", name));
+            }
+
+            for (int i = 0; i < lenvec; i++) {
+                auto elem = kw.second.attr("__getitem__")(py::int_(i)).get_type();
+                if (!(elem.is(py_float) || elem.is(py_int) || elem.is(np_int) || elem.is(np_float))) {
+                    py::print(py::str(elem));
+                    throw std::invalid_argument("Vectors and lists must only contain doubles or floats");
+                }
+                units[i]= kw.second.attr("__getitem__")(py::int_(i)).cast<double>();
+            }
+        }
+        else {
+            throw std::invalid_argument("Invalid unit type");
+        }
+
+        for (int i = 0; i < idxs.size(); i++) {
+            Units[idxs[i]] = units[i];
+        }
+    }
+
+    this->setUnits(Units);
 }
 
-int ASSET::ODEPhaseBase::addValueLock(PhaseRegionFlags reg, VectorXi args) {
-  return this->addEqualCon(StateConstraint(LockArgs<-1>(args.size()), reg, args));
-  ;
+int ASSET::ODEPhaseBase::addBoundaryValue(RegionType reg, VarIndexType args, const std::variant<double, VectorXd>& value_t, ScaleType scale_t)
+{
+    Eigen::VectorXd value;
+    if (std::holds_alternative<double>(value_t)) {
+        value.resize(1);
+        value[0] = std::get<double>(value_t);
+    }
+    else if (std::holds_alternative<VectorXd>(value_t)) {
+        value = std::get<VectorXd>(value_t);
+    }
+    auto Func = Arguments<-1>(value.size()) - value;
+    return this->addEqualCon(reg,Func,args,scale_t);
 }
 
-int ASSET::ODEPhaseBase::addBoundaryValue(PhaseRegionFlags reg, VectorXi args, const VectorXd& value) {
-  if (args.size() != value.size()) {
-    fmt::print(
-        fmt::fg(fmt::color::red),
-        "Transcription Error!!!\n"
-        "Size of boundary value arguments({0:}) and size of boundary value vector({1:}) do not match\n",
-        args.size(),
-        value.size());
-    throw std::invalid_argument("");
-  }
-  auto Func = Arguments<-1>(args.size()) - value;
-  return this->addEqualCon(StateConstraint(Func, reg, args));
+int ASSET::ODEPhaseBase::addDeltaVarEqualCon(VarIndexType var, double value, double scale, ScaleType scale_t)
+{
+    auto args = Arguments<2>();
+    auto x0 = args.coeff<0>();
+    auto x1 = args.coeff<1>();
+    auto func = ((x1 - x0) - value) * scale;
+    return this->addEqualCon(PhaseRegionFlags::FrontandBack, func, var,scale_t);
 }
 
-int ASSET::ODEPhaseBase::addPeriodicityCon(VectorXi args) {
-  auto X = Arguments<-1>(args.size() * 2);
-  auto Func = X.head(args.size()) - X.tail(args.size());
-  return this->addEqualCon(StateConstraint(Func, PhaseRegionFlags::FrontandBack, args));
+int ASSET::ODEPhaseBase::addValueLock(RegionType reg, VarIndexType args, ScaleType scale_t)
+{
+    int argsize = this->getXtUPVars(getRegion(reg), args).size();
+    return this->addEqualCon(reg,LockArgs<-1>(argsize), args,scale_t);
 }
 
-int ASSET::ODEPhaseBase::addLUVarBound(
-    PhaseRegionFlags reg, int var, double lowerbound, double upperbound, double lbscale, double ubscale) {
+int ASSET::ODEPhaseBase::addPeriodicityCon(VarIndexType args, ScaleType scale_t)
+{
+    int argsize = this->getXtUPVars(PhaseRegionFlags::FrontandBack, args).size();
+    auto X = Arguments<-1>(argsize * 2);
+    auto Func = X.head(argsize) - X.tail(argsize);
+    return this->addEqualCon(PhaseRegionFlags::FrontandBack,Func, args,scale_t);
+}
+/////////////////////////////////////////////////////////////////////////////////////////////
 
+int ASSET::ODEPhaseBase::addLUVarBound(RegionType reg, VarIndexType var, double lowerbound, double upperbound, double lbscale, double ubscale, ScaleType scale_t)
+{
+    if (lowerbound > upperbound) {
+        fmt::print(fmt::fg(fmt::color::red),
+            "Transcription Error!!!\n"
+            "Lower-bound({0:.3e}) greater than Upper-bound({1:.3e}) \n",
+            lowerbound,
+            upperbound);
+        throw std::invalid_argument("");
+    }
+    check_lbscale(lbscale);
+    check_ubscale(ubscale);
 
-  if (lowerbound > upperbound) {
-    fmt::print(fmt::fg(fmt::color::red),
-               "Transcription Error!!!\n"
-               "Lower-bound({0:.3e}) greater than Upper-bound({1:.3e}) \n",
-               lowerbound,
-               upperbound);
-    throw std::invalid_argument("");
-  }
-  check_lbscale(lbscale);
-  check_ubscale(ubscale);
+    auto x = Arguments<1>();
+    auto lowbound = (lowerbound - x) * lbscale;
+    auto ubound = (x - upperbound) * ubscale;
+    auto lubound = StackedOutputs{ lowbound, ubound };
 
-  auto x = Arguments<1>();
-  auto lowbound = (lowerbound - x) * lbscale;
-  auto ubound = (x - upperbound) * ubscale;
+    return this->addInequalCon(reg,lubound, var,scale_t);
 
-  auto lubound = StackedOutputs {lowbound, ubound};
-  VectorXi v(1);
-  v[0] = var;
-  return this->addInequalCon(StateConstraint(lubound, reg, v));
 }
-
-int ASSET::ODEPhaseBase::addLowerVarBound(PhaseRegionFlags reg, int var, double lowerbound, double lbscale) {
+int ASSET::ODEPhaseBase::addLUVarBound(
+    PhaseRegionFlags reg, int var, double lowerbound, double upperbound, double lbscale, double ubscale) {
 
 
-  check_lbscale(lbscale);
+    if (lowerbound > upperbound) {
+        fmt::print(fmt::fg(fmt::color::red),
+            "Transcription Error!!!\n"
+            "Lower-bound({0:.3e}) greater than Upper-bound({1:.3e}) \n",
+            lowerbound,
+            upperbound);
+        throw std::invalid_argument("");
+    }
+    check_lbscale(lbscale);
+    check_ubscale(ubscale);
 
-  auto x = Arguments<1>();
-  auto lbound = (lowerbound - x) * lbscale;
-  VectorXi v(1);
-  v[0] = var;
+    auto x = Arguments<1>();
+    auto lowbound = (lowerbound - x) * lbscale;
+    auto ubound = (x - upperbound) * ubscale;
 
-  return this->addInequalCon(StateConstraint(lbound, reg, v));
+    auto lubound = StackedOutputs{ lowbound, ubound };
+    VectorXi v(1);
+    v[0] = var;
+    return this->addInequalCon(StateConstraint(lubound, reg, v));
 }
 
-int ASSET::ODEPhaseBase::addUpperVarBound(PhaseRegionFlags reg, int var, double upperbound, double ubscale) {
 
+int ASSET::ODEPhaseBase::addLowerVarBound(RegionType reg, VarIndexType var, double lowerbound, double lbscale, ScaleType scale_t)
+{
+    check_lbscale(lbscale);
+    auto x = Arguments<1>();
+    auto lbound = (lowerbound - x) * lbscale;
+    
+    return this->addInequalCon(reg,lbound, var, scale_t);
+}
 
-  check_ubscale(ubscale);
+int ASSET::ODEPhaseBase::addUpperVarBound(RegionType reg, VarIndexType var, double upperbound, double ubscale, ScaleType scale_t)
+{
+    check_ubscale(ubscale);
+    auto x = Arguments<1>();
+    auto ubound = (x - upperbound) * ubscale;
+    return this->addInequalCon(reg, ubound, var, scale_t);
+}
 
+int ASSET::ODEPhaseBase::addLUFuncBound(RegionType reg, ScalarFunctionalX func, VarIndexType XtUPvars,
+    VarIndexType OPvars,
+    VarIndexType SPvars,
+    double lowerbound, double upperbound, double lbscale, double ubscale, ScaleType scale_t)
+{
 
-  auto x = Arguments<1>();
-  auto ubound = (x - upperbound) * ubscale;
-  VectorXi v(1);
-  v[0] = var;
-  return this->addInequalCon(StateConstraint(ubound, reg, v));
+    if (lowerbound > upperbound) {
+        fmt::print(fmt::fg(fmt::color::red),
+            "Transcription Error!!!\n"
+            "Lower-bound({0:.3e}) greater than Upper-bound({1:.3e}) \n",
+            lowerbound,
+            upperbound);
+        throw std::invalid_argument("");
+    }
+    check_lbscale(lbscale);
+    check_ubscale(ubscale);
+    auto x = Arguments<1>();
+    auto lubound = StackedOutputs{ (lowerbound - x) * lbscale, (x - upperbound) * ubscale };
+    auto lufun = lubound.eval(func);
+    return this->addInequalCon(reg,lufun, XtUPvars,OPvars,SPvars,scale_t);
 }
 
-int ASSET::ODEPhaseBase::addLUNormBound(PhaseRegionFlags reg,
-                                        VectorXi vars,
-                                        double lowerbound,
-                                        double upperbound,
-                                        double lbscale,
-                                        double ubscale) {
+int ASSET::ODEPhaseBase::addLowerFuncBound(RegionType reg, ScalarFunctionalX func, 
+    VarIndexType XtUPvars,
+    VarIndexType OPvars, 
+    VarIndexType SPvars, 
+    double lowerbound, double lbscale, ScaleType scale_t)
+{
+    check_lbscale(lbscale);
 
+    Vector1<double> rhs;
+    rhs[0] = lowerbound;
+    auto lbfun = ((-1.0 * func) + rhs) * lbscale;
 
-  if (lowerbound > upperbound) {
-    fmt::print(fmt::fg(fmt::color::red),
-               "Transcription Error!!!\n"
-               "Lower-bound({0:.3e}) greater than Upper-bound({1:.3e}) \n",
-               lowerbound,
-               upperbound);
-    throw std::invalid_argument("");
-  }
-  check_lbscale(lbscale);
-  check_ubscale(ubscale);
+    return this->addInequalCon(reg, lbfun, XtUPvars, OPvars, SPvars, scale_t);
+}
 
-  int size = vars.size();
-  auto impl = [&](auto sz) {
-    auto x = Arguments<1>();
-    auto lubound = StackedOutputs {(lowerbound - x) * lbscale, (x - upperbound) * ubscale};
-    auto normfun = lubound.eval(Arguments<sz.value>(size).norm());
-    return this->addInequalCon(StateConstraint(normfun, reg, vars));
-  };
+int ASSET::ODEPhaseBase::addUpperFuncBound(RegionType reg, ScalarFunctionalX func, 
+    VarIndexType XtUPvars, 
+    VarIndexType OPvars, 
+    VarIndexType SPvars, 
+    double upperbound, double ubscale, ScaleType scale_t)
+{
+
+    check_ubscale(ubscale);
 
-  switch (size) {
+    Vector1<double> rhs;
+    rhs[0] = -upperbound;
+    auto ubfun = ((func)+rhs) * ubscale;
+    return this->addInequalCon(reg, ubfun, XtUPvars, OPvars, SPvars, scale_t);
+}
 
+int ASSET::ODEPhaseBase::addLUNormBound(RegionType reg, VarIndexType XtUPvars,
+    double lowerbound, double upperbound, double lbscale, double ubscale, ScaleType scale_t)
+{
+    if (lowerbound > upperbound) {
+        fmt::print(fmt::fg(fmt::color::red),
+            "Transcription Error!!!\n"
+            "Lower-bound({0:.3e}) greater than Upper-bound({1:.3e}) \n",
+            lowerbound,
+            upperbound);
+        throw std::invalid_argument("");
+    }
+    check_lbscale(lbscale);
+    check_ubscale(ubscale);
+
+    int size = this->getXtUPVars(getRegion(reg), XtUPvars).size();
+    
+    auto impl = [&](auto sz) {
+        auto x = Arguments<1>();
+        auto lubound = StackedOutputs{ (lowerbound - x) * lbscale, (x - upperbound) * ubscale };
+        auto normfun = lubound.eval(Arguments<sz.value>(size).norm());
+        return this->addInequalCon(reg,normfun, XtUPvars,scale_t);
+    };
+
+    switch (size) {
     case 2:
-      return impl(int_const<2>());
+        return impl(int_const<2>());
     case 3:
-      return impl(int_const<3>());
+        return impl(int_const<3>());
     case 4:
-      return impl(int_const<4>());
+        return impl(int_const<4>());
     default:
-      return impl(int_const<-1>());
-  }
-  return 0;
+        return impl(int_const<-1>());
+    }
+    return 0;
 }
 
-int ASSET::ODEPhaseBase::addLowerNormBound(PhaseRegionFlags reg,
-                                           VectorXi vars,
-                                           double lowerbound,
-                                           double lbscale) {
+int ASSET::ODEPhaseBase::addLUSquaredNormBound(RegionType reg, VarIndexType XtUPvars,
+    double lowerbound, double upperbound, double lbscale, double ubscale, ScaleType scale_t)
+{
+    if (lowerbound > upperbound) {
+        fmt::print(fmt::fg(fmt::color::red),
+            "Transcription Error!!!\n"
+            "Lower-bound({0:.3e}) greater than Upper-bound({1:.3e}) \n",
+            lowerbound,
+            upperbound);
+        throw std::invalid_argument("");
+    }
+    check_lbscale(lbscale);
+    check_ubscale(ubscale);
 
+    int size = this->getXtUPVars(getRegion(reg), XtUPvars).size();
 
-  check_lbscale(lbscale);
+    auto impl = [&](auto sz) {
+        auto x = Arguments<1>();
+        auto lubound = StackedOutputs{ (lowerbound - x) * lbscale, (x - upperbound) * ubscale };
+        auto normfun = lubound.eval(Arguments<sz.value>(size).squared_norm());
+        return this->addInequalCon(reg, normfun, XtUPvars, scale_t);
+    };
 
-  int size = vars.size();
-  auto impl = [&](auto sz) {
-    auto x = Arguments<1>();
-    auto normfun = (lowerbound - Arguments<sz.value>(size).norm()) * lbscale;
-    return this->addInequalCon(StateConstraint(normfun, reg, vars));
-  };
-  switch (size) {
+    switch (size) {
     case 2:
-      return impl(int_const<2>());
+        return impl(int_const<2>());
     case 3:
-      return impl(int_const<3>());
+        return impl(int_const<3>());
     case 4:
-      return impl(int_const<4>());
+        return impl(int_const<4>());
     default:
-      return impl(int_const<-1>());
-  }
+        return impl(int_const<-1>());
+    }
+    return 0;
 }
 
-int ASSET::ODEPhaseBase::addUpperNormBound(PhaseRegionFlags reg,
-                                           VectorXi vars,
-                                           double upperbound,
-                                           double ubscale) {
-
-
-  check_ubscale(ubscale);
-
-
-  int size = vars.size();
-  auto impl = [&](auto sz) {
-    auto x = Arguments<1>();
-    auto normfun = (Arguments<sz.value>(size).norm() - upperbound) * ubscale;
-    return this->addInequalCon(StateConstraint(normfun, reg, vars));
-  };
-  switch (size) {
+int ASSET::ODEPhaseBase::addLowerNormBound(RegionType reg, VarIndexType XtUPvars, double lowerbound, double lbscale, ScaleType scale_t)
+{
+    check_lbscale(lbscale);
+
+    int size = this->getXtUPVars(getRegion(reg), XtUPvars).size();
+    auto impl = [&](auto sz) {
+        auto x = Arguments<1>();
+        auto normfun = (lowerbound - Arguments<sz.value>(size).norm()) * lbscale;
+        return this->addInequalCon(reg,normfun, XtUPvars,scale_t);
+    };
+    switch (size) {
     case 2:
-      return impl(int_const<2>());
+        return impl(int_const<2>());
     case 3:
-      return impl(int_const<3>());
+        return impl(int_const<3>());
     case 4:
-      return impl(int_const<4>());
+        return impl(int_const<4>());
     default:
-      return impl(int_const<-1>());
-  }
+        return impl(int_const<-1>());
+    }
 }
 
-
-int ASSET::ODEPhaseBase::addLUSquaredNormBound(PhaseRegionFlags reg,
-                                               VectorXi vars,
-                                               double lowerbound,
-                                               double upperbound,
-                                               double lbscale,
-                                               double ubscale) {
-
-
-  if (lowerbound > upperbound) {
-    fmt::print(fmt::fg(fmt::color::red),
-               "Transcription Error!!!\n"
-               "Lower-bound({0:.3e}) greater than Upper-bound({1:.3e}) \n",
-               lowerbound,
-               upperbound);
-    throw std::invalid_argument("");
-  }
-  check_lbscale(lbscale);
-  check_ubscale(ubscale);
-
-  int size = vars.size();
-  auto impl = [&](auto sz) {
-    auto x = Arguments<1>();
-    auto lubound = StackedOutputs {(lowerbound - x) * lbscale, (x - upperbound) * ubscale};
-    auto normfun = lubound.eval(Arguments<sz.value>(size).squared_norm());
-    return this->addInequalCon(StateConstraint(normfun, reg, vars));
-  };
-
-  switch (size) {
-
+int ASSET::ODEPhaseBase::addLowerSquaredNormBound(RegionType reg, VarIndexType XtUPvars, double lowerbound, double lbscale, ScaleType scale_t)
+{
+    check_lbscale(lbscale);
+
+    int size = this->getXtUPVars(getRegion(reg), XtUPvars).size();
+    auto impl = [&](auto sz) {
+        auto x = Arguments<1>();
+        auto normfun = (lowerbound - Arguments<sz.value>(size).squared_norm()) * lbscale;
+        return this->addInequalCon(reg, normfun, XtUPvars, scale_t);
+    };
+    switch (size) {
     case 2:
-      return impl(int_const<2>());
+        return impl(int_const<2>());
     case 3:
-      return impl(int_const<3>());
+        return impl(int_const<3>());
     case 4:
-      return impl(int_const<4>());
+        return impl(int_const<4>());
     default:
-      return impl(int_const<-1>());
-  }
-  return 0;
+        return impl(int_const<-1>());
+    }
 }
 
-int ASSET::ODEPhaseBase::addLowerSquaredNormBound(PhaseRegionFlags reg,
-                                                  VectorXi vars,
-                                                  double lowerbound,
-                                                  double lbscale) {
-
-
-  check_lbscale(lbscale);
-
-  int size = vars.size();
-  auto impl = [&](auto sz) {
-    auto x = Arguments<1>();
-    auto normfun = (lowerbound - Arguments<sz.value>(size).squared_norm()) * lbscale;
-    return this->addInequalCon(StateConstraint(normfun, reg, vars));
-  };
-  switch (size) {
+int ASSET::ODEPhaseBase::addUpperNormBound(RegionType reg, VarIndexType XtUPvars, double upperbound, double ubscale, ScaleType scale_t)
+{
+    check_ubscale(ubscale);
+
+    int size = this->getXtUPVars(getRegion(reg), XtUPvars).size();
+    auto impl = [&](auto sz) {
+        auto x = Arguments<1>();
+        auto normfun = (Arguments<sz.value>(size).norm() - upperbound) * ubscale;
+        return this->addInequalCon(reg, normfun, XtUPvars, scale_t);
+    };
+    switch (size) {
     case 2:
-      return impl(int_const<2>());
+        return impl(int_const<2>());
     case 3:
-      return impl(int_const<3>());
+        return impl(int_const<3>());
     case 4:
-      return impl(int_const<4>());
+        return impl(int_const<4>());
     default:
-      return impl(int_const<-1>());
-  }
-  return 0;
+        return impl(int_const<-1>());
+    }
+    return 0;
 }
 
-int ASSET::ODEPhaseBase::addUpperSquaredNormBound(PhaseRegionFlags reg,
-                                                  VectorXi vars,
-                                                  double upperbound,
-                                                  double ubscale) {
-
-
-  check_ubscale(ubscale);
-
-
-  int size = vars.size();
-  auto impl = [&](auto sz) {
-    auto x = Arguments<1>();
-    auto normfun = (Arguments<sz.value>(size).squared_norm() - upperbound) * ubscale;
-    return this->addInequalCon(StateConstraint(normfun, reg, vars));
-  };
-  switch (size) {
+int ASSET::ODEPhaseBase::addUpperSquaredNormBound(RegionType reg, VarIndexType XtUPvars, double upperbound, double ubscale, ScaleType scale_t)
+{
+    check_ubscale(ubscale);
+
+    int size = this->getXtUPVars(getRegion(reg), XtUPvars).size();
+    auto impl = [&](auto sz) {
+        auto x = Arguments<1>();
+        auto normfun = (Arguments<sz.value>(size).squared_norm() - upperbound) * ubscale;
+        return this->addInequalCon(reg, normfun, XtUPvars, scale_t);
+    };
+    switch (size) {
     case 2:
-      return impl(int_const<2>());
+        return impl(int_const<2>());
     case 3:
-      return impl(int_const<3>());
+        return impl(int_const<3>());
     case 4:
-      return impl(int_const<4>());
+        return impl(int_const<4>());
     default:
-      return impl(int_const<-1>());
-  }
-  return 0;
+        return impl(int_const<-1>());
+    }
+    return 0;
 }
 
-
-int ASSET::ODEPhaseBase::addLowerFuncBound(
-    PhaseRegionFlags reg, ScalarFunctionalX func, VectorXi vars, double lowerbound, double lbscale) {
-
-  check_lbscale(lbscale);
-
-  Vector1<double> rhs;
-  rhs[0] = lowerbound;
-  auto lbfun = ((-1.0 * func) + rhs) * lbscale;
-  return this->addInequalCon(StateConstraint(lbfun, reg, vars));
+int ASSET::ODEPhaseBase::addLowerDeltaVarBound(RegionType reg, VarIndexType var, double lowerbound, double lbscale, ScaleType scale_t)
+{
+    check_lbscale(lbscale);
+
+    auto args = Arguments<2>();
+    auto x0 = args.coeff<0>();
+    auto x1 = args.coeff<1>();
+    auto func = (lowerbound - (x1 - x0)) * lbscale;
+   
+    return this->addInequalCon(reg,func,var,scale_t);
 }
-int ASSET::ODEPhaseBase::addUpperFuncBound(
-    PhaseRegionFlags reg, ScalarFunctionalX func, VectorXi vars, double upperbound, double ubscale) {
-
-
-  check_ubscale(ubscale);
-
-
-  Vector1<double> rhs;
-  rhs[0] = -upperbound;
-  auto lbfun = ((func) + rhs) * ubscale;
 
-  return this->addInequalCon(StateConstraint(lbfun, reg, vars));
-}
-int ASSET::ODEPhaseBase::addLUFuncBound(PhaseRegionFlags reg,
-                                        ScalarFunctionalX func,
-                                        VectorXi vars,
-                                        double lowerbound,
-                                        double upperbound,
-                                        double lbscale,
-                                        double ubscale) {
-  if (lowerbound > upperbound) {
-    fmt::print(fmt::fg(fmt::color::red),
-               "Transcription Error!!!\n"
-               "Lower-bound({0:.3e}) greater than Upper-bound({1:.3e}) \n",
-               lowerbound,
-               upperbound);
-    throw std::invalid_argument("");
-  }
-  check_lbscale(lbscale);
-  check_ubscale(ubscale);
-  auto x = Arguments<1>();
-  auto lubound = StackedOutputs {(lowerbound - x) * lbscale, (x - upperbound) * ubscale};
-  auto lufun = lubound.eval(func);
-  return this->addInequalCon(StateConstraint(lufun, reg, vars));
-}
-////////////////////////////////////////////////////////////
-int ASSET::ODEPhaseBase::addLowerDeltaVarBound(PhaseRegionFlags reg,
-                                               int var,
-                                               double lowerbound,
-                                               double lbscale) {
-
-  check_lbscale(lbscale);
-
-
-  auto args = Arguments<2>();
-  auto x0 = args.coeff<0>();
-  auto x1 = args.coeff<1>();
-  auto func = (lowerbound - (x1 - x0)) * lbscale;
-  VectorXi v(1);
-  v[0] = var;
-  return this->addInequalCon(reg, func, v);
+int ASSET::ODEPhaseBase::addUpperDeltaVarBound(RegionType reg, VarIndexType var, double upperbound, double ubscale, ScaleType scale_t)
+{
+    check_ubscale(ubscale);
+    auto args = Arguments<2>();
+    auto x0 = args.coeff<0>();
+    auto x1 = args.coeff<1>();
+    auto func = ((x1 - x0) - upperbound) * ubscale;
+    return this->addInequalCon(reg, func, var, scale_t);
 }
 
-int ASSET::ODEPhaseBase::addUpperDeltaVarBound(PhaseRegionFlags reg,
-                                               int var,
-                                               double upperbound,
-                                               double ubscale) {
 
 
-  check_ubscale(ubscale);
+//////////////////////////////
 
-  auto args = Arguments<2>();
-  auto x0 = args.coeff<0>();
-  auto x1 = args.coeff<1>();
-  auto func = ((x1 - x0) - upperbound) * ubscale;
-  VectorXi v(1);
-  v[0] = var;
-  return this->addInequalCon(reg, func, v);
+int ASSET::ODEPhaseBase::addValueObjective(RegionType reg, VarIndexType var, double scale, ScaleType scale_t)
+{
+    auto obj = Arguments<1>() * scale;
+    return this->addStateObjective(reg, obj, var, scale_t);
 }
 
-int ASSET::ODEPhaseBase::addValueObjective(PhaseRegionFlags reg, int var, double scale) {
-  auto obj = Arguments<1>() * scale;
-  VectorXi v(1);
-  v[0] = var;
-  return this->addStateObjective(StateObjective(obj, reg, v));
+int ASSET::ODEPhaseBase::addDeltaVarObjective(VarIndexType var, double scale, ScaleType scale_t)
+{
+    auto args = Arguments<2>();
+    auto x0 = args.coeff<0>();
+    auto x1 = args.coeff<1>();
+    auto func = ((x1 - x0)) * scale;
+    
+    return this->addStateObjective(PhaseRegionFlags::FrontandBack, func, var,scale_t);
 }
 
-int ASSET::ODEPhaseBase::addDeltaVarObjective(int var, double scale) {
-  auto args = Arguments<2>();
-  auto x0 = args.coeff<0>();
-  auto x1 = args.coeff<1>();
-  auto func = ((x1 - x0)) * scale;
-  VectorXi v(1);
-  v[0] = var;
-  return this->addStateObjective(PhaseRegionFlags::FrontandBack, func, v);
-}
+
 
 std::vector<Eigen::VectorXd> ASSET::ODEPhaseBase::returnCostateTraj() const {
   if (!this->PostOptInfoValid) {
@@ -613,6 +619,56 @@ void ASSET::ODEPhaseBase::setTraj(const std::vector<Eigen::VectorXd>& mesh,
   this->invalidatePostOptInfo();
 }
 
+void ASSET::ODEPhaseBase::setTraj(const std::vector<Eigen::VectorXd>& mesh)
+{
+    /// Sets phase to have same number of defects and spacing as input trajectory
+
+    int numdefects = (mesh.size() - 1) / (this->numTranCardStates - 1);
+    int numrem = (mesh.size() - 1) % (this->numTranCardStates - 1);
+
+    if (numrem != 0) {
+        throw std::invalid_argument("Number of states in mesh inconsistent with transcription type.");
+    }
+    if (mesh.size() == 0) {
+        throw std::invalid_argument("Input trajectory is empty");
+    }
+    int msize = mesh[0].size();
+    if (msize != this->Table.XtUVars) {
+        std::cout << "User Input Error in function setInitTraj for ODE:" << this->Table.ode.name() << std::endl;
+        std::cout << " Dimension of Input States(" << msize << ") does not match expected dimensions of the ODE("
+            << this->Table.XtUVars << ")" << std::endl;
+        throw std::invalid_argument("");
+    }
+    for (auto& X : mesh) {
+        if (X.hasNaN()) {
+            throw std::invalid_argument("NaN detected in State Vector in ODEPhaseBase::setTraj");
+        }
+    }
+
+    Eigen::VectorXd DBS;
+
+
+    DBS.resize(numdefects + 1);
+    DBS[0] = 0;
+
+    double t0 = mesh.front()[this->TVar()];
+    double tf = mesh.back()[this->TVar()];
+
+
+    for (int i = 0; i < numdefects; i++) {
+        int elem = (this->numTranCardStates - 1) * (i + 1);
+        double ti = mesh[elem][this->TVar()];
+        DBS[i + 1] = (ti - t0) / (tf - t0);
+    }
+
+    Eigen::VectorXi DPB;
+
+    DPB.resize(numdefects);
+    DPB.setOnes();
+
+    this->setTraj(mesh, DBS, DPB, false);
+}
+
 
 void ASSET::ODEPhaseBase::refineTrajManual(VectorXd DBS, VectorXi DPB) {
   if ((DBS.size() - 1) != DPB.size()) {
@@ -736,16 +792,27 @@ void ASSET::ODEPhaseBase::transcribe_integrals() {
     ObjectiveInterface obj;
     PhaseRegionFlags PhaseReg = PhaseRegionFlags::PairWisePath;
 
+    auto Func = ob.Func;
+    if (this->AutoScaling) {
+        VectorXd input_scales = this->get_input_scale(ob.RegionFlag, ob.XtUVars, ob.OPVars, ob.SPVars);
+        VectorXd output_scales(Func.ORows());
+        output_scales.setOnes(); 
+        output_scales = ob.OutputScales;
+
+        Func = IOScaled<decltype(Func)>(ob.Func, input_scales, output_scales);
+    }
+
+
     if (this->IntegralMode == IntegralModes::BaseIntegral
         && ((this->TranscriptionMode == LGL5) || (this->TranscriptionMode == LGL7))) {
       if (this->TranscriptionMode == LGL5) {
-        obj = SwitchC(3, xp, sop, ob.Func, xp, sop);
+        obj = SwitchC(3, xp, sop, Func, xp, sop);
       } else if (this->TranscriptionMode == LGL7) {
-        obj = SwitchC(4, xp, sop, ob.Func, xp, sop);
+        obj = SwitchC(4, xp, sop, Func, xp, sop);
       }
       PhaseReg = PhaseRegionFlags::DefectPath;
     } else {
-      obj = SwitchC(2, xp, sop, ob.Func, xp, sop);
+      obj = SwitchC(2, xp, sop, Func, xp, sop);
       if (this->TranscriptionMode == LGL3 || this->TranscriptionMode == Trapezoidal
           || this->TranscriptionMode == CentralShooting) {
         PhaseReg = PhaseRegionFlags::DefectPath;
@@ -775,23 +842,43 @@ void ASSET::ODEPhaseBase::transcribe_integrals() {
     ObjectiveInterface obj;
     PhaseRegionFlags PhaseReg = PhaseRegionFlags::PairWisePath;
 
+    auto Func = ob.Func;
+    double AccScale = 1.0;
+
+    if (this->AutoScaling) {
+        VectorXd input_scales = this->get_input_scale(ob.RegionFlag, ob.XtUVars, ob.OPVars, ob.SPVars);
+        VectorXd output_scales(Func.ORows());
+        output_scales.setOnes(); 
+        output_scales = ob.OutputScales;
+
+        Func = IOScaled<decltype(Func)>(ob.Func, input_scales, output_scales);
+
+
+        double pscale = this->SPUnits[ob.EXTVars[0]];
+
+        AccScale = pscale*output_scales[0] / (this->XtUPUnits[this->TVar()]);
+
+    }
+
+
     if (this->IntegralMode == IntegralModes::BaseIntegral
         && ((this->TranscriptionMode == LGL5) || (this->TranscriptionMode == LGL7))) {
       if (this->TranscriptionMode == LGL5) {
-        obj = SwitchC(3, xp, sop, ob.Func, xp, sop);
+        obj = SwitchC(3, xp, sop, Func, xp, sop);
       } else if (this->TranscriptionMode == LGL7) {
-        obj = SwitchC(4, xp, sop, ob.Func, xp, sop);
+        obj = SwitchC(4, xp, sop, Func, xp, sop);
       }
       PhaseReg = PhaseRegionFlags::DefectPath;
     } else {
-      obj = SwitchC(2, xp, sop, ob.Func, xp, sop);
+      obj = SwitchC(2, xp, sop, Func, xp, sop);
       PhaseReg = PhaseRegionFlags::PairWisePath;
     }
 
     ThreadingFlags ThreadMode =
         ob.Func.thread_safe() ? ThreadingFlags::ByApplication : ThreadingFlags::MainThread;
 
-    auto AccFunc = Arguments<1>() * -1.0;
+    auto AccFunc = Arguments<1>() * -AccScale;
+
 
     int Gindex = this->indexer.addAccumulation(
         obj, PhaseReg, xtrap, ob.OPVars, ob.SPVars, AccFunc, ob.EXTVars, ThreadMode);
@@ -807,8 +894,19 @@ void ASSET::ODEPhaseBase::transcribe_basic_funcs() {
         eq.Func.thread_safe() ? ThreadingFlags::ByApplication : ThreadingFlags::MainThread;
     if (eq.RegionFlag == PhaseRegionFlags::Path || eq.RegionFlag == PhaseRegionFlags::PairWisePath)
       eq.Func.enable_vectorization(this->EnableVectorization);
+
+
+    auto Func = eq.Func;
+    if (this->AutoScaling) {
+        VectorXd input_scales = this->get_input_scale(eq.RegionFlag, eq.XtUVars, eq.OPVars, eq.SPVars);
+        VectorXd output_scales(Func.ORows());
+        output_scales.setOnes(); 
+        output_scales = eq.OutputScales;
+        Func = IOScaled<decltype(Func)>(eq.Func, input_scales, output_scales);
+    }
+
     int Gindex =
-        this->indexer.addEquality(eq.Func, eq.RegionFlag, eq.XtUVars, eq.OPVars, eq.SPVars, ThreadMode);
+        this->indexer.addEquality(Func, eq.RegionFlag, eq.XtUVars, eq.OPVars, eq.SPVars, ThreadMode);
 
     int PLindex = Gindex - this->indexer.StartEq;
     eq.GlobalIndex = Gindex;
@@ -822,8 +920,18 @@ void ASSET::ODEPhaseBase::transcribe_basic_funcs() {
       iq.Func.enable_vectorization(this->EnableVectorization);
     }
 
+    auto Func = iq.Func;
+    if (this->AutoScaling) {
+        VectorXd input_scales = this->get_input_scale(iq.RegionFlag, iq.XtUVars, iq.OPVars, iq.SPVars);
+        VectorXd output_scales(Func.ORows());
+        output_scales.setOnes(); 
+        output_scales = iq.OutputScales;
+
+        Func = IOScaled<decltype(Func)>(iq.Func, input_scales, output_scales);
+    }
+
     int Gindex =
-        this->indexer.addInequality(iq.Func, iq.RegionFlag, iq.XtUVars, iq.OPVars, iq.SPVars, ThreadMode);
+        this->indexer.addInequality(Func, iq.RegionFlag, iq.XtUVars, iq.OPVars, iq.SPVars, ThreadMode);
     int PLindex = Gindex - this->indexer.StartIq;
     iq.GlobalIndex = Gindex;
     iq.PhaseLocalIndex = PLindex;
@@ -833,8 +941,19 @@ void ASSET::ODEPhaseBase::transcribe_basic_funcs() {
         ob.Func.thread_safe() ? ThreadingFlags::ByApplication : ThreadingFlags::MainThread;
     if (ob.RegionFlag == PhaseRegionFlags::Path || ob.RegionFlag == PhaseRegionFlags::PairWisePath)
       ob.Func.enable_vectorization(this->EnableVectorization);
+
+    auto Func = ob.Func;
+    if (this->AutoScaling) {
+        VectorXd input_scales = this->get_input_scale(ob.RegionFlag, ob.XtUVars, ob.OPVars, ob.SPVars);
+        VectorXd output_scales(Func.ORows());
+        output_scales.setOnes(); 
+        output_scales = ob.OutputScales;
+
+        Func = IOScaled<decltype(Func)>(ob.Func, input_scales, output_scales);
+    }
+
     int Gindex =
-        this->indexer.addObjective(ob.Func, ob.RegionFlag, ob.XtUVars, ob.OPVars, ob.SPVars, ThreadMode);
+        this->indexer.addObjective(Func, ob.RegionFlag, ob.XtUVars, ob.OPVars, ob.SPVars, ThreadMode);
     int PLindex = Gindex - this->indexer.StartObj;
     ob.GlobalIndex = Gindex;
     ob.PhaseLocalIndex = PLindex;
@@ -942,6 +1061,13 @@ void ASSET::ODEPhaseBase::transcribe_control_funcs() {
 }
 
 void ASSET::ODEPhaseBase::check_functions(int pnum) {
+
+    /*
+   Loops through all user defined functions and checks that they do not
+   reference non-existent variables. Should be run prior to any transcribing any
+   problem functions.
+   */
+
   auto CheckFun = [&](const std::string& type, auto& func) {
     if (func.XtUVars.size() > 0) {
       if (func.XtUVars.maxCoeff() >= this->XtUPVars() || func.XtUVars.minCoeff() < 0) {
@@ -1022,13 +1148,225 @@ void ASSET::ODEPhaseBase::check_functions(int pnum) {
     CheckFun(sobj, f);
 }
 
+Eigen::VectorXd ASSET::ODEPhaseBase::get_input_scale(PhaseRegionFlags flag, VectorXi XtUV, VectorXi OPV, VectorXi SPV) const
+{
+
+    int nloops;
+    switch (flag) {
+    case Front:
+    case Back:
+    case Path:
+    case Params:
+    case ODEParams:
+    case StaticParams:
+    case InnerPath: {
+        nloops = 1;
+        break;
+    }
+    case FrontandBack:
+    case BackandFront:
+    case PairWisePath: {
+        nloops = 2;
+        break;
+    }
+    default: {
+        throw std::invalid_argument("Cannot scale this phase region");
+        break;
+    }
+    }
+
+
+    int isize = XtUV.size() * nloops + OPV.size() + SPV.size();
+    VectorXd scales(isize);
+
+    int next = 0;
+    for (int n = 0; n < nloops; n++) {
+        for (int i = 0; i < XtUV.size(); i++) {
+            scales[next] = this->XtUPUnits[XtUV[i]];
+            next++;
+        }
+    }
+    for (int i = 0; i < OPV.size(); i++) {
+        scales[next] = this->XtUPUnits[OPV[i] + this->XtUVars()];
+        next++;
+    }
+    for (int i = 0; i < SPV.size(); i++) {
+        scales[next] = this->SPUnits[SPV[i]];
+        next++;
+    }
+
+    return scales;
+}
+
+std::vector<Eigen::VectorXd> ASSET::ODEPhaseBase::get_test_inputs(PhaseRegionFlags flag, VectorXi XtUV, VectorXi OPV, VectorXi SPV) const
+{
+
+    std::vector<std::vector<int>> test_states;
+
+    int nloops = 0;
+    switch (flag) {
+    case Front: {
+        test_states.push_back({ 0 });
+        break;
+    }
+    case Back: {
+        test_states.push_back({ int(this->ActiveTraj.size() - 1) });
+        break;
+    }
+    case Params: {
+        test_states.push_back({ 0 });
+        break;
+    }
+    case ODEParams: {
+        test_states.push_back({ 0 });
+        break;
+    }
+    case StaticParams: {
+        test_states.push_back({ 0 });
+        break;
+    }
+    case Path: {
+        for (int i = 0; i < this->ActiveTraj.size(); i++)
+            test_states.push_back({ i });
+        break;
+    }
+    case InnerPath: {
+        for (int i = 1; i < this->ActiveTraj.size() - 1; i++)
+            test_states.push_back({ i });
+        break;
+    }
+    case FrontandBack: {
+        test_states.push_back({ 0, int(this->ActiveTraj.size() - 1) });
+        break;
+    }
+    case BackandFront: {
+        test_states.push_back({ int(this->ActiveTraj.size() - 1),0 });
+        break;
+    }
+    case PairWisePath: {
+        for (int i = 0; i < this->ActiveTraj.size() - 1; i++)
+            test_states.push_back({ i,i + 1 });
+        break;
+    }
+    default: {
+        throw std::invalid_argument("Cannot scale this phase region");
+        break;
+    }
+    }
+
+
+    int isize = XtUV.size() * test_states[0].size() + OPV.size() + SPV.size();
+
+
+
+    std::vector<Eigen::VectorXd> inputs;
+    VectorXd input_scales = this->get_input_scale(flag, XtUV, OPV, SPV);
+
+
+    for (int ncalls = 0; ncalls < test_states.size(); ncalls++) {
+
+        VectorXd input(isize);
+
+        int next = 0;
+
+        for (int i = 0; i < test_states[ncalls].size(); i++) {
+            int state = test_states[ncalls][i];
+            for (int j = 0; j < XtUV.size(); j++) {
+                input[next] = this->ActiveTraj[state][XtUV[j]];
+                next++;
+            }
+        }
+        for (int j = 0; j < OPV.size(); j++) {
+            input[next] = this->ActiveTraj[0][OPV[j] + this->XtUVars()];
+            next++;
+        }
+        for (int j = 0; j < SPV.size(); j++) {
+            input[next] = this->ActiveStaticParams[SPV[j]];
+            next++;
+        }
+        input = input.cwiseQuotient(input_scales);
+
+        inputs.push_back(input);
+    }
+
+    return inputs;
+}
+
+void ASSET::ODEPhaseBase::calc_auto_scales()
+{
+    auto calc_impl = [&](auto& funcmap) {
+        for (auto& [key, func] : funcmap) {
+            if (func.ScaleMode == "auto") {
+                VectorXd input_scales = this->get_input_scale(func.RegionFlag, func.XtUVars, func.OPVars, func.SPVars);
+                std::vector<VectorXd> test_inputs = this->get_test_inputs(func.RegionFlag, func.XtUVars, func.OPVars, func.SPVars);
+                VectorXd output_scales = calc_jacobian_row_scales(func.Func, input_scales, test_inputs, "norm", "mean");
+                func.OutputScales = output_scales;
+            }
+            else {
+				
+
+            }
+
+        }
+    };
+
+    calc_impl(this->userEqualities);
+    calc_impl(this->userInequalities);
+    calc_impl(this->userStateObjectives);
+    calc_impl(this->userIntegrands);
+    calc_impl(this->userParamIntegrands);
+
+
+}
+
+std::vector<double> ASSET::ODEPhaseBase::get_objective_scales()
+{
+    // If we have mixed integral and state objectives, we assume that they have the same units
+    // since the output scales are computed for the integrands we need to divide by tstar 
+    // before averaging to make the units consistent
+
+    std::vector<double> scales;
+    for (auto& [key, obj] : this->userStateObjectives) {
+        if (obj.ScaleMode == "auto") {
+            // OutputScales units 1/obj
+            scales.push_back(obj.OutputScales[0]);
+        }
+    }
+    for (auto& [key, obj] : this->userIntegrands) {
+        if (obj.ScaleMode == "auto") {
+            // OutputScales units tstar/obj
+            // Divide by tstar, since this function is the integrand not the total integral
+		     scales.push_back(obj.OutputScales[0]/this->XtUPUnits[this->TVar()]);
+			
+
+        }
+    }
+
+    return scales;
+}
+
+void ASSET::ODEPhaseBase::update_objective_scales(double scale)
+{
+    for (auto& [key, obj] : this->userStateObjectives) {
+        if (obj.ScaleMode == "auto") {
+            obj.OutputScales[0] = scale;
+        }
+    }
+    for (auto& [key, obj] : this->userIntegrands) {
+        if (obj.ScaleMode == "auto") {
+            // Multiply by tstar, since this function is the integrand not the total integral
+            obj.OutputScales[0] = scale * this->XtUPUnits[this->TVar()];
+            
+        }
+    }
+}
+
 void ASSET::ODEPhaseBase::transcribe_phase(
     int vo, int eqo, int iqo, std::shared_ptr<NonLinearProgram> np, int pnum)
 
 {
   this->indexer.begin_indexing(np, vo, eqo, iqo);
-  this->check_functions(pnum);
-
+  
 
   this->transcribe_dynamics();
   this->transcribe_axis_funcs();
@@ -1046,6 +1384,21 @@ void ASSET::ODEPhaseBase::transcribe(bool showstats, bool showfuns) {
   this->nlp = std::make_shared<NonLinearProgram>(this->Threads);
 
   this->initIndexing();
+
+  this->check_functions(0);
+  if (this->AutoScaling) {
+      this->calc_auto_scales();
+      if (this->SyncObjectiveScales) {
+          // Ensure that all objectives have same scale factor to preserve meaning of un-scaled problem
+          // Common scale is mean of all separate scale factors
+          auto objscales = this->get_objective_scales();
+          if (objscales.size() > 0) {
+              double meanobjscale = std::accumulate(objscales.begin(), objscales.end(), 0.0) / double(objscales.size());
+              this->update_objective_scales(meanobjscale);
+          }
+      }
+  }
+
   this->transcribe_phase(0, 0, 0, this->nlp, 0);
   if (showstats)
     this->indexer.print_stats(showfuns);
@@ -1344,569 +1697,3 @@ ASSET::PSIOPT::ConvergenceFlags ASSET::ODEPhaseBase::phase_call_impl(std::string
   return flag;
 }
 
-void ASSET::ODEPhaseBase::Build(py::module& m) {
-  using namespace pybind11::literals;
-  using namespace doc;
-  auto obj =
-      py::class_<ODEPhaseBase, std::shared_ptr<ODEPhaseBase>, OptimizationProblemBase>(m, "PhaseInterface");
-  obj.doc() = "Base Class for All Optimal Control Phases";
-
-  obj.def("enable_vectorization", &ODEPhaseBase::enable_vectorization);
-
-  obj.def("setTraj",
-          py::overload_cast<const std::vector<Eigen::VectorXd>&, Eigen::VectorXd, Eigen::VectorXi>(
-              &ODEPhaseBase::setTraj),
-          ODEPhaseBase_setTraj1);
-
-  obj.def("setTraj",
-          py::overload_cast<const std::vector<Eigen::VectorXd>&, Eigen::VectorXd, Eigen::VectorXi, bool>(
-              &ODEPhaseBase::setTraj));
-
-  obj.def("setTraj",
-          py::overload_cast<const std::vector<Eigen::VectorXd>&, int>(&ODEPhaseBase::setTraj),
-          ODEPhaseBase_setTraj2);
-
-  obj.def("setTraj",
-          py::overload_cast<const std::vector<Eigen::VectorXd>&, int, bool>(&ODEPhaseBase::setTraj));
-
-
-  obj.def("switchTranscriptionMode",
-          py::overload_cast<TranscriptionModes, VectorXd, VectorXi>(&ODEPhaseBase::switchTranscriptionMode),
-          ODEPhaseBase_switchTranscriptionMode1);
-  obj.def("switchTranscriptionMode",
-          py::overload_cast<TranscriptionModes>(&ODEPhaseBase::switchTranscriptionMode),
-          ODEPhaseBase_switchTranscriptionMode2);
-
-
-  obj.def("switchTranscriptionMode",
-          py::overload_cast<std::string, VectorXd, VectorXi>(&ODEPhaseBase::switchTranscriptionMode),
-          ODEPhaseBase_switchTranscriptionMode1);
-  obj.def("switchTranscriptionMode",
-          py::overload_cast<std::string>(&ODEPhaseBase::switchTranscriptionMode),
-          ODEPhaseBase_switchTranscriptionMode2);
-
-
-  obj.def("transcribe", py::overload_cast<bool, bool>(&ODEPhaseBase::transcribe), ODEPhaseBase_transcribe);
-
-  obj.def("refineTrajManual",
-          py::overload_cast<int>(&ODEPhaseBase::refineTrajManual),
-          ODEPhaseBase_refineTrajManual1);
-  obj.def("refineTrajManual",
-          py::overload_cast<VectorXd, VectorXi>(&ODEPhaseBase::refineTrajManual),
-          ODEPhaseBase_refineTrajManual2);
-  obj.def("refineTrajEqual", &ODEPhaseBase::refineTrajEqual, ODEPhaseBase_refineTrajEqual);
-
-  obj.def("setStaticParams", &ODEPhaseBase::setStaticParams, ODEPhaseBase_setStaticParams);
-
-
-  obj.def("setControlMode",
-          py::overload_cast<ControlModes>(&ODEPhaseBase::setControlMode),
-          ODEPhaseBase_setControlMode);
-  obj.def("setControlMode",
-          py::overload_cast<std::string>(&ODEPhaseBase::setControlMode),
-          ODEPhaseBase_setControlMode);
-
-  obj.def("setIntegralMode", &ODEPhaseBase::setIntegralMode, ODEPhaseBase_setIntegralMode);
-
-  obj.def("subStaticParams", &ODEPhaseBase::subStaticParams, ODEPhaseBase_subStaticParams);
-
-  obj.def("subVariables",
-          py::overload_cast<PhaseRegionFlags, VectorXi, VectorXd>(&ODEPhaseBase::subVariables),
-          ODEPhaseBase_subVariables);
-  obj.def("subVariable",
-          py::overload_cast<PhaseRegionFlags, int, double>(&ODEPhaseBase::subVariable),
-          ODEPhaseBase_subVariable);
-
-  obj.def("subVariables",
-          py::overload_cast<std::string, VectorXi, VectorXd>(&ODEPhaseBase::subVariables),
-          ODEPhaseBase_subVariables);
-  obj.def("subVariable",
-          py::overload_cast<std::string, int, double>(&ODEPhaseBase::subVariable),
-          ODEPhaseBase_subVariable);
-
-  obj.def("returnTraj", &ODEPhaseBase::returnTraj, ODEPhaseBase_returnTraj);
-  obj.def("returnTrajRange", &ODEPhaseBase::returnTrajRange, ODEPhaseBase_returnTrajRange);
-  obj.def("returnTrajRangeND", &ODEPhaseBase::returnTrajRangeND, ODEPhaseBase_returnTrajRangeND);
-  obj.def("returnTrajTable", &ODEPhaseBase::returnTrajTable);
-
-  obj.def("returnCostateTraj", &ODEPhaseBase::returnCostateTraj, ODEPhaseBase_returnCostateTraj);
-  obj.def("returnTrajError", &ODEPhaseBase::returnTrajError);
-
-  obj.def("returnUSplineConLmults", &ODEPhaseBase::returnUSplineConLmults);
-  obj.def("returnUSplineConVals", &ODEPhaseBase::returnUSplineConVals);
-
-
-  obj.def("returnEqualConLmults", &ODEPhaseBase::returnEqualConLmults, ODEPhaseBase_returnEqualConLmults);
-  obj.def("returnEqualConVals", &ODEPhaseBase::returnEqualConVals);
-
-  obj.def(
-      "returnInequalConLmults", &ODEPhaseBase::returnInequalConLmults, ODEPhaseBase_returnInequalConLmults);
-  obj.def("returnInequalConVals", &ODEPhaseBase::returnInequalConVals);
-
-
-  obj.def("returnStaticParams", &ODEPhaseBase::returnStaticParams, ODEPhaseBase_returnStaticParam);
-
-  obj.def("test_threads", &ODEPhaseBase::test_threads);
-
-  obj.def("removeEqualCon", &ODEPhaseBase::removeEqualCon, ODEPhaseBase_removeEqualCon);
-  obj.def("removeInequalCon", &ODEPhaseBase::removeInequalCon, ODEPhaseBase_removeInequalCon);
-  obj.def("removeStateObjective", &ODEPhaseBase::removeStateObjective, ODEPhaseBase_removeStateObjective);
-  obj.def("removeIntegralObjective",
-          &ODEPhaseBase::removeIntegralObjective,
-          ODEPhaseBase_removeIntegralObjective);
-  obj.def("removeIntegralParamFunction",
-          &ODEPhaseBase::removeIntegralParamFunction,
-          ODEPhaseBase_removeIntegralParamFunction);
-  ////////////////////////////////////////////////////////////////////////////////////////////////////////
-  obj.def("addEqualCon",
-          py::overload_cast<StateConstraint>(&ODEPhaseBase::addEqualCon),
-          ODEPhaseBase_addEqualCon1);
-  obj.def("addEqualCon",
-          py::overload_cast<PhaseRegionFlags, VectorFunctionalX, VectorXi>(&ODEPhaseBase::addEqualCon),
-          ODEPhaseBase_addEqualCon2);
-  obj.def("addEqualCon",
-          py::overload_cast<PhaseRegionFlags, VectorFunctionalX, VectorXi, VectorXi, VectorXi>(
-              &ODEPhaseBase::addEqualCon),
-          ODEPhaseBase_addEqualCon3);
-
-  obj.def("addEqualCon",
-          py::overload_cast<std::string, VectorFunctionalX, VectorXi>(&ODEPhaseBase::addEqualCon),
-          ODEPhaseBase_addEqualCon2);
-  obj.def("addEqualCon",
-          py::overload_cast<std::string, VectorFunctionalX, VectorXi, VectorXi, VectorXi>(
-              &ODEPhaseBase::addEqualCon),
-          ODEPhaseBase_addEqualCon3);
-
-
-  obj.def("addDeltaVarEqualCon",
-          py::overload_cast<PhaseRegionFlags, int, double, double>(&ODEPhaseBase::addDeltaVarEqualCon),
-          ODEPhaseBase_addDeltaVarEqualCon1);
-  obj.def("addDeltaVarEqualCon",
-          py::overload_cast<int, double, double>(&ODEPhaseBase::addDeltaVarEqualCon),
-          ODEPhaseBase_addDeltaVarEqualCon2);
-  obj.def("addDeltaVarEqualCon",
-          py::overload_cast<int, double>(&ODEPhaseBase::addDeltaVarEqualCon),
-          ODEPhaseBase_addDeltaVarEqualCon3);
-
-  obj.def("addDeltaTimeEqualCon",
-          py::overload_cast<double, double>(&ODEPhaseBase::addDeltaTimeEqualCon),
-          ODEPhaseBase_addDeltaTimeEqualCon1);
-  obj.def("addDeltaTimeEqualCon",
-          py::overload_cast<double>(&ODEPhaseBase::addDeltaTimeEqualCon),
-          ODEPhaseBase_addDeltaTimeEqualCon2);
-
-  obj.def("addBoundaryValue",
-          py::overload_cast<PhaseRegionFlags, VectorXi, const VectorXd&>(&ODEPhaseBase::addBoundaryValue),
-          ODEPhaseBase_addBoundaryValue);
-  obj.def("addBoundaryValue",
-          py::overload_cast<std::string, VectorXi, const VectorXd&>(&ODEPhaseBase::addBoundaryValue),
-          ODEPhaseBase_addBoundaryValue);
-
-  obj.def("addValueLock",
-          py::overload_cast<std::string, VectorXi>(&ODEPhaseBase::addValueLock),
-          ODEPhaseBase_addValueLock);
-  obj.def("addValueLock",
-          py::overload_cast<PhaseRegionFlags, VectorXi>(&ODEPhaseBase::addValueLock),
-          ODEPhaseBase_addValueLock);
-
-  obj.def("addBoundaryValues", &ODEPhaseBase::addBoundaryValues, ODEPhaseBase_addBoundaryValues);
-
-  obj.def("addPeriodicityCon", &ODEPhaseBase::addPeriodicityCon);
-  ///////////////////////////////////////////////////////////////////////////////
-
-  obj.def("addInequalCon",
-          py::overload_cast<StateConstraint>(&ODEPhaseBase::addInequalCon),
-          ODEPhaseBase_addInequalCon1);
-  obj.def("addInequalCon",
-          py::overload_cast<PhaseRegionFlags, VectorFunctionalX, VectorXi>(&ODEPhaseBase::addInequalCon),
-          ODEPhaseBase_addInequalCon2);
-  obj.def("addInequalCon",
-          py::overload_cast<PhaseRegionFlags, VectorFunctionalX, VectorXi, VectorXi, VectorXi>(
-              &ODEPhaseBase::addInequalCon),
-          ODEPhaseBase_addInequalCon3);
-
-  obj.def("addInequalCon",
-          py::overload_cast<std::string, VectorFunctionalX, VectorXi>(&ODEPhaseBase::addInequalCon),
-          ODEPhaseBase_addInequalCon2);
-  obj.def("addInequalCon",
-          py::overload_cast<std::string, VectorFunctionalX, VectorXi, VectorXi, VectorXi>(
-              &ODEPhaseBase::addInequalCon),
-          ODEPhaseBase_addInequalCon3);
-
-
-  obj.def("addLUVarBound",
-          py::overload_cast<PhaseRegionFlags, int, double, double>(&ODEPhaseBase::addLUVarBound),
-          ODEPhaseBase_addLUVarBound1);
-  obj.def("addLUVarBound",
-          py::overload_cast<PhaseRegionFlags, int, double, double, double>(&ODEPhaseBase::addLUVarBound),
-          ODEPhaseBase_addLUVarBound2);
-  obj.def(
-      "addLUVarBound",
-      py::overload_cast<PhaseRegionFlags, int, double, double, double, double>(&ODEPhaseBase::addLUVarBound),
-      ODEPhaseBase_addLUVarBound3);
-  obj.def("addLUVarBounds",
-          py::overload_cast<PhaseRegionFlags, Eigen::VectorXi, double, double, double>(
-              &ODEPhaseBase::addLUVarBounds),
-          ODEPhaseBase_addLUVarBounds);
-
-
-  obj.def("addLUVarBound",
-          py::overload_cast<std::string, int, double, double>(&ODEPhaseBase::addLUVarBound),
-          ODEPhaseBase_addLUVarBound1);
-  obj.def("addLUVarBound",
-          py::overload_cast<std::string, int, double, double, double>(&ODEPhaseBase::addLUVarBound),
-          ODEPhaseBase_addLUVarBound2);
-  obj.def("addLUVarBound",
-          py::overload_cast<std::string, int, double, double, double, double>(&ODEPhaseBase::addLUVarBound),
-          ODEPhaseBase_addLUVarBound3);
-  obj.def(
-      "addLUVarBounds",
-      py::overload_cast<std::string, Eigen::VectorXi, double, double, double>(&ODEPhaseBase::addLUVarBounds),
-      ODEPhaseBase_addLUVarBounds);
-
-
-  obj.def("addLowerVarBound",
-          py::overload_cast<PhaseRegionFlags, int, double, double>(&ODEPhaseBase::addLowerVarBound),
-          ODEPhaseBase_addLowerVarBound1);
-  obj.def("addLowerVarBound",
-          py::overload_cast<PhaseRegionFlags, int, double>(&ODEPhaseBase::addLowerVarBound),
-          ODEPhaseBase_addLowerVarBound2);
-
-  obj.def("addUpperVarBound",
-          py::overload_cast<PhaseRegionFlags, int, double, double>(&ODEPhaseBase::addUpperVarBound),
-          ODEPhaseBase_addUpperVarBound1);
-  obj.def("addUpperVarBound",
-          py::overload_cast<PhaseRegionFlags, int, double>(&ODEPhaseBase::addUpperVarBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-  obj.def("addLowerVarBound",
-          py::overload_cast<std::string, int, double, double>(&ODEPhaseBase::addLowerVarBound),
-          ODEPhaseBase_addLowerVarBound1);
-  obj.def("addLowerVarBound",
-          py::overload_cast<std::string, int, double>(&ODEPhaseBase::addLowerVarBound),
-          ODEPhaseBase_addLowerVarBound2);
-
-  obj.def("addUpperVarBound",
-          py::overload_cast<std::string, int, double, double>(&ODEPhaseBase::addUpperVarBound),
-          ODEPhaseBase_addUpperVarBound1);
-  obj.def("addUpperVarBound",
-          py::overload_cast<std::string, int, double>(&ODEPhaseBase::addUpperVarBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-
-  //////////////////////////////////////////////////////////////
-
-  obj.def("addLowerNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double, double>(&ODEPhaseBase::addLowerNormBound),
-          ODEPhaseBase_addUpperVarBound1);
-  obj.def("addLowerNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double>(&ODEPhaseBase::addLowerNormBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-  obj.def("addLowerNormBound",
-          py::overload_cast<std::string, VectorXi, double, double>(&ODEPhaseBase::addLowerNormBound),
-          ODEPhaseBase_addUpperVarBound1);
-  obj.def("addLowerNormBound",
-          py::overload_cast<std::string, VectorXi, double>(&ODEPhaseBase::addLowerNormBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-
-  obj.def("addUpperNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double, double>(&ODEPhaseBase::addUpperNormBound),
-          ODEPhaseBase_addUpperVarBound1);
-  obj.def("addUpperNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double>(&ODEPhaseBase::addUpperNormBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-  obj.def("addUpperNormBound",
-          py::overload_cast<std::string, VectorXi, double, double>(&ODEPhaseBase::addUpperNormBound),
-          ODEPhaseBase_addUpperVarBound1);
-  obj.def("addUpperNormBound",
-          py::overload_cast<std::string, VectorXi, double>(&ODEPhaseBase::addUpperNormBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-
-  obj.def("addLUNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double, double, double, double>(
-              &ODEPhaseBase::addLUNormBound),
-          ODEPhaseBase_addLUNormBound1);
-  obj.def(
-      "addLUNormBound",
-      py::overload_cast<PhaseRegionFlags, VectorXi, double, double, double>(&ODEPhaseBase::addLUNormBound),
-      ODEPhaseBase_addLUNormBound2);
-  obj.def("addLUNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double, double>(&ODEPhaseBase::addLUNormBound),
-          ODEPhaseBase_addLUNormBound3);
-
-  obj.def(
-      "addLUNormBound",
-      py::overload_cast<std::string, VectorXi, double, double, double, double>(&ODEPhaseBase::addLUNormBound),
-      ODEPhaseBase_addLUNormBound1);
-  obj.def("addLUNormBound",
-          py::overload_cast<std::string, VectorXi, double, double, double>(&ODEPhaseBase::addLUNormBound),
-          ODEPhaseBase_addLUNormBound2);
-  obj.def("addLUNormBound",
-          py::overload_cast<std::string, VectorXi, double, double>(&ODEPhaseBase::addLUNormBound),
-          ODEPhaseBase_addLUNormBound3);
-
-  //////////////////////////////////////////////////////////////
-
-  obj.def(
-      "addLowerSquaredNormBound",
-      py::overload_cast<PhaseRegionFlags, VectorXi, double, double>(&ODEPhaseBase::addLowerSquaredNormBound),
-      ODEPhaseBase_addUpperVarBound1);
-  obj.def("addLowerSquaredNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double>(&ODEPhaseBase::addLowerSquaredNormBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-  obj.def("addLowerSquaredNormBound",
-          py::overload_cast<std::string, VectorXi, double, double>(&ODEPhaseBase::addLowerSquaredNormBound),
-          ODEPhaseBase_addUpperVarBound1);
-  obj.def("addLowerSquaredNormBound",
-          py::overload_cast<std::string, VectorXi, double>(&ODEPhaseBase::addLowerSquaredNormBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-
-  obj.def(
-      "addUpperSquaredNormBound",
-      py::overload_cast<PhaseRegionFlags, VectorXi, double, double>(&ODEPhaseBase::addUpperSquaredNormBound),
-      ODEPhaseBase_addUpperVarBound1);
-  obj.def("addUpperSquaredNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double>(&ODEPhaseBase::addUpperSquaredNormBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-  obj.def("addUpperSquaredNormBound",
-          py::overload_cast<std::string, VectorXi, double, double>(&ODEPhaseBase::addUpperSquaredNormBound),
-          ODEPhaseBase_addUpperVarBound1);
-  obj.def("addUpperSquaredNormBound",
-          py::overload_cast<std::string, VectorXi, double>(&ODEPhaseBase::addUpperSquaredNormBound),
-          ODEPhaseBase_addUpperVarBound2);
-
-
-  obj.def("addLUSquaredNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double, double, double, double>(
-              &ODEPhaseBase::addLUSquaredNormBound),
-          ODEPhaseBase_addLUNormBound1);
-  obj.def("addLUSquaredNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double, double, double>(
-              &ODEPhaseBase::addLUSquaredNormBound),
-          ODEPhaseBase_addLUNormBound2);
-  obj.def("addLUSquaredNormBound",
-          py::overload_cast<PhaseRegionFlags, VectorXi, double, double>(&ODEPhaseBase::addLUSquaredNormBound),
-          ODEPhaseBase_addLUNormBound3);
-
-  obj.def("addLUSquaredNormBound",
-          py::overload_cast<std::string, VectorXi, double, double, double, double>(
-              &ODEPhaseBase::addLUSquaredNormBound),
-          ODEPhaseBase_addLUNormBound1);
-  obj.def(
-      "addLUSquaredNormBound",
-      py::overload_cast<std::string, VectorXi, double, double, double>(&ODEPhaseBase::addLUSquaredNormBound),
-      ODEPhaseBase_addLUNormBound2);
-  obj.def("addLUSquaredNormBound",
-          py::overload_cast<std::string, VectorXi, double, double>(&ODEPhaseBase::addLUSquaredNormBound),
-          ODEPhaseBase_addLUNormBound3);
-
-
-  //////////////////////////////////////////////////////////////
-
-  obj.def("addLowerFuncBound",
-          py::overload_cast<PhaseRegionFlags, ScalarFunctionalX, VectorXi, double, double>(
-              &ODEPhaseBase::addLowerFuncBound),
-          ODEPhaseBase_addLowerFuncBound1);
-  obj.def("addUpperFuncBound",
-          py::overload_cast<PhaseRegionFlags, ScalarFunctionalX, VectorXi, double, double>(
-              &ODEPhaseBase::addUpperFuncBound),
-          ODEPhaseBase_addUpperFuncBound2);
-
-  obj.def("addLowerFuncBound",
-          py::overload_cast<std::string, ScalarFunctionalX, VectorXi, double, double>(
-              &ODEPhaseBase::addLowerFuncBound),
-          ODEPhaseBase_addLowerFuncBound1);
-  obj.def("addUpperFuncBound",
-          py::overload_cast<std::string, ScalarFunctionalX, VectorXi, double, double>(
-              &ODEPhaseBase::addUpperFuncBound),
-          ODEPhaseBase_addUpperFuncBound2);
-
-
-  obj.def("addLowerFuncBound",
-          py::overload_cast<PhaseRegionFlags, ScalarFunctionalX, VectorXi, double>(
-              &ODEPhaseBase::addLowerFuncBound),
-          ODEPhaseBase_addLowerFuncBound1);
-  obj.def("addUpperFuncBound",
-          py::overload_cast<PhaseRegionFlags, ScalarFunctionalX, VectorXi, double>(
-              &ODEPhaseBase::addUpperFuncBound),
-          ODEPhaseBase_addUpperFuncBound2);
-
-  obj.def(
-      "addLowerFuncBound",
-      py::overload_cast<std::string, ScalarFunctionalX, VectorXi, double>(&ODEPhaseBase::addLowerFuncBound),
-      ODEPhaseBase_addLowerFuncBound1);
-  obj.def(
-      "addUpperFuncBound",
-      py::overload_cast<std::string, ScalarFunctionalX, VectorXi, double>(&ODEPhaseBase::addUpperFuncBound),
-      ODEPhaseBase_addUpperFuncBound2);
-
-
-  obj.def("addLUFuncBound",
-          py::overload_cast<PhaseRegionFlags, ScalarFunctionalX, VectorXi, double, double, double, double>(
-              &ODEPhaseBase::addLUFuncBound));
-  obj.def("addLUFuncBound",
-          py::overload_cast<PhaseRegionFlags, ScalarFunctionalX, VectorXi, double, double, double>(
-              &ODEPhaseBase::addLUFuncBound));
-  obj.def("addLUFuncBound",
-          py::overload_cast<PhaseRegionFlags, ScalarFunctionalX, VectorXi, double, double>(
-              &ODEPhaseBase::addLUFuncBound));
-
-  obj.def("addLUFuncBound",
-          py::overload_cast<std::string, ScalarFunctionalX, VectorXi, double, double, double, double>(
-              &ODEPhaseBase::addLUFuncBound));
-  obj.def("addLUFuncBound",
-          py::overload_cast<std::string, ScalarFunctionalX, VectorXi, double, double, double>(
-              &ODEPhaseBase::addLUFuncBound));
-  obj.def("addLUFuncBound",
-          py::overload_cast<std::string, ScalarFunctionalX, VectorXi, double, double>(
-              &ODEPhaseBase::addLUFuncBound));
-
-
-  //////////////////////////////////////////////////////////////
-
-
-  obj.def("addLowerDeltaVarBound",
-          py::overload_cast<PhaseRegionFlags, int, double, double>(&ODEPhaseBase::addLowerDeltaVarBound),
-          ODEPhaseBase_addLowerDeltaVarBound1);
-  obj.def("addLowerDeltaVarBound",
-          py::overload_cast<int, double, double>(&ODEPhaseBase::addLowerDeltaVarBound),
-          ODEPhaseBase_addLowerDeltaVarBound2);
-  obj.def("addLowerDeltaVarBound",
-          py::overload_cast<int, double>(&ODEPhaseBase::addLowerDeltaVarBound),
-          ODEPhaseBase_addLowerDeltaVarBound3);
-
-  obj.def("addLowerDeltaTimeBound",
-          py::overload_cast<double, double>(&ODEPhaseBase::addLowerDeltaTimeBound),
-          ODEPhaseBase_addLowerDeltaTimeBound1);
-  obj.def("addLowerDeltaTimeBound",
-          py::overload_cast<double>(&ODEPhaseBase::addLowerDeltaTimeBound),
-          ODEPhaseBase_addLowerDeltaTimeBound2);
-
-  obj.def("addUpperDeltaVarBound",
-          py::overload_cast<PhaseRegionFlags, int, double, double>(&ODEPhaseBase::addUpperDeltaVarBound),
-          ODEPhaseBase_addUpperDeltaVarBound1);
-  obj.def("addUpperDeltaVarBound",
-          py::overload_cast<int, double, double>(&ODEPhaseBase::addUpperDeltaVarBound),
-          ODEPhaseBase_addUpperDeltaVarBound2);
-  obj.def("addUpperDeltaVarBound",
-          py::overload_cast<int, double>(&ODEPhaseBase::addUpperDeltaVarBound),
-          ODEPhaseBase_addUpperDeltaVarBound3);
-  obj.def("addUpperDeltaTimeBound",
-          py::overload_cast<double, double>(&ODEPhaseBase::addUpperDeltaTimeBound),
-          ODEPhaseBase_addUpperDeltaTimeBound1);
-  obj.def("addUpperDeltaTimeBound",
-          py::overload_cast<double>(&ODEPhaseBase::addUpperDeltaTimeBound),
-          ODEPhaseBase_addUpperDeltaTimeBound2);
-
-  ////////////////////////////////////////////////////////////////////////////
-  obj.def("addStateObjective",
-          py::overload_cast<StateObjective>(&ODEPhaseBase::addStateObjective),
-          ODEPhaseBase_addStateObjective);
-
-  obj.def("addStateObjective",
-          py::overload_cast<PhaseRegionFlags, ScalarFunctionalX, VectorXi>(&ODEPhaseBase::addStateObjective),
-          ODEPhaseBase_addStateObjective);
-
-  obj.def("addStateObjective",
-          py::overload_cast<PhaseRegionFlags, ScalarFunctionalX, VectorXi, VectorXi, VectorXi>(
-              &ODEPhaseBase::addStateObjective),
-          ODEPhaseBase_addStateObjective);
-
-  obj.def("addStateObjective",
-          py::overload_cast<std::string, ScalarFunctionalX, VectorXi>(&ODEPhaseBase::addStateObjective),
-          ODEPhaseBase_addStateObjective);
-
-  obj.def("addStateObjective",
-          py::overload_cast<std::string, ScalarFunctionalX, VectorXi, VectorXi, VectorXi>(
-              &ODEPhaseBase::addStateObjective),
-          ODEPhaseBase_addStateObjective);
-
-
-  obj.def("addValueObjective",
-          py::overload_cast<PhaseRegionFlags, int, double>(&ODEPhaseBase::addValueObjective),
-          ODEPhaseBase_addValueObjective);
-
-  obj.def("addValueObjective",
-          py::overload_cast<std::string, int, double>(&ODEPhaseBase::addValueObjective),
-          ODEPhaseBase_addValueObjective);
-
-
-  obj.def("addDeltaVarObjective", &ODEPhaseBase::addDeltaVarObjective, ODEPhaseBase_addDeltaVarObjective);
-  obj.def("addDeltaTimeObjective", &ODEPhaseBase::addDeltaTimeObjective, ODEPhaseBase_addDeltaTimeObjective);
-
-  obj.def("addIntegralObjective",
-          py::overload_cast<StateObjective>(&ODEPhaseBase::addIntegralObjective),
-          ODEPhaseBase_addIntegralObjective1);
-  obj.def("addIntegralObjective",
-          py::overload_cast<ScalarFunctionalX, VectorXi>(&ODEPhaseBase::addIntegralObjective),
-          ODEPhaseBase_addIntegralObjective2);
-  obj.def(
-      "addIntegralObjective",
-      py::overload_cast<ScalarFunctionalX, VectorXi, VectorXi, VectorXi>(&ODEPhaseBase::addIntegralObjective),
-      ODEPhaseBase_addIntegralObjective2);
-  ///////////////////////////////////////////////////////////////////////////////
-  obj.def("addIntegralParamFunction",
-          py::overload_cast<StateObjective, int>(&ODEPhaseBase::addIntegralParamFunction),
-          ODEPhaseBase_addIntegralParamFunction1);
-  obj.def("addIntegralParamFunction",
-          py::overload_cast<ScalarFunctionalX, VectorXi, int>(&ODEPhaseBase::addIntegralParamFunction),
-          ODEPhaseBase_addIntegralParamFunction2);
-  obj.def("addIntegralParamFunction",
-          py::overload_cast<ScalarFunctionalX, VectorXi, VectorXi, VectorXi, int>(
-              &ODEPhaseBase::addIntegralParamFunction),
-          ODEPhaseBase_addIntegralParamFunction2);
-
-
-  ////////////////////////////////////////////////////
-  obj.def("getMeshInfo", &ODEPhaseBase::getMeshInfo);
-  obj.def("refineTrajAuto", &ODEPhaseBase::refineTrajAuto);
-  obj.def("calc_global_error", &ODEPhaseBase::calc_global_error);
-  obj.def("getMeshIters", &ODEPhaseBase::getMeshIters);
-
-
-  obj.def_readwrite("AdaptiveMesh", &ODEPhaseBase::AdaptiveMesh);
-
-  obj.def("setAdaptiveMesh", &ODEPhaseBase::setAdaptiveMesh, py::arg("AdaptiveMesh") = true);
-
-  obj.def("setMeshTol", &ODEPhaseBase::setMeshTol);
-  obj.def("setMeshRedFactor", &ODEPhaseBase::setMeshRedFactor);
-  obj.def("setMeshIncFactor", &ODEPhaseBase::setMeshIncFactor);
-  obj.def("setMeshErrFactor", &ODEPhaseBase::setMeshErrFactor);
-  obj.def("setMaxMeshIters", &ODEPhaseBase::setMaxMeshIters);
-  obj.def("setMinSegments", &ODEPhaseBase::setMinSegments);
-  obj.def("setMaxSegments", &ODEPhaseBase::setMaxSegments);
-  obj.def("setMeshErrorCriteria", &ODEPhaseBase::setMeshErrorCriteria);
-  obj.def("setMeshErrorEstimator", &ODEPhaseBase::setMeshErrorEstimator);
-
-
-  obj.def_readwrite("PrintMeshInfo", &ODEPhaseBase::PrintMeshInfo);
-  obj.def_readwrite("MaxMeshIters", &ODEPhaseBase::MaxMeshIters);
-  obj.def_readwrite("MeshTol", &ODEPhaseBase::MeshTol);
-  obj.def_readwrite("MeshErrorEstimator", &ODEPhaseBase::MeshErrorEstimator);
-  obj.def_readwrite("MeshErrorCriteria", &ODEPhaseBase::MeshErrorCriteria);
-
-
-  obj.def_readwrite("SolveOnlyFirst", &ODEPhaseBase::SolveOnlyFirst);
-  obj.def_readwrite("ForceOneMeshIter", &ODEPhaseBase::ForceOneMeshIter);
-  obj.def_readwrite("NewError", &ODEPhaseBase::NewError);
-
-
-  obj.def_readwrite("DetectControlSwitches", &ODEPhaseBase::DetectControlSwitches);
-  obj.def_readwrite("RelSwitchTol", &ODEPhaseBase::RelSwitchTol);
-  obj.def_readwrite("AbsSwitchTol", &ODEPhaseBase::AbsSwitchTol);
-  obj.def_readwrite("MeshAbortFlag", &ODEPhaseBase::MeshAbortFlag);
-
-
-  obj.def_readwrite("NumExtraSegs", &ODEPhaseBase::NumExtraSegs);
-  obj.def_readwrite("MeshRedFactor", &ODEPhaseBase::MeshRedFactor);
-  obj.def_readwrite("MeshIncFactor", &ODEPhaseBase::MeshIncFactor);
-  obj.def_readwrite("MeshErrFactor", &ODEPhaseBase::MeshErrFactor);
-  obj.def_readonly("MeshConverged", &ODEPhaseBase::MeshConverged);
-}
diff --git a/src/OptimalControl/ODEPhaseBase.h b/src/OptimalControl/ODEPhaseBase.h
index 97190c45..2054223c 100644
--- a/src/OptimalControl/ODEPhaseBase.h
+++ b/src/OptimalControl/ODEPhaseBase.h
@@ -11,6 +11,8 @@
 #include "StateFunction.h"
 #include "VectorFunctions/ASSET_VectorFunctions.h"
 #include "pch.h"
+#include "CommonFunctions/IOScaled.h"
+#include "InterfaceTypes.h"
 
 namespace ASSET {
 
@@ -34,7 +36,6 @@ namespace ASSET {
 
     friend OptimalControlProblem;
 
-    int calc_threads();
 
    protected:
     PhaseIndexer indexer;
@@ -79,9 +80,19 @@ namespace ASSET {
     int TranSpacingFuncIndices = 0;
     int ConstraintOrder = 0;
 
+    //////////////////////////
+
+    Eigen::VectorXd XtUPUnits;
+    Eigen::VectorXd SPUnits;
+
+    std::map<std::string, Eigen::VectorXi> SPidxs;
+
 
     ///////////////////////
    public:
+    bool AutoScaling = false;
+    bool SyncObjectiveScales = true;
+
     bool AdaptiveMesh = false;
     bool PrintMeshInfo = true;
     int MaxMeshIters = 10;
@@ -113,6 +124,13 @@ namespace ASSET {
     std::vector<MeshIterateInfo> MeshIters;
 
 
+    void setAutoScaling(bool autoscale) {
+        this->AutoScaling = autoscale;
+        this->resetTranscription();
+        this->invalidatePostOptInfo();
+    }
+
+
     void setAdaptiveMesh(bool amesh) {
       this->AdaptiveMesh = amesh;
     }
@@ -172,10 +190,18 @@ namespace ASSET {
       this->setXVars(Xv);
       this->setUVars(Uv);
       this->setPVars(Pv);
+
+      this->XtUPUnits = Eigen::VectorXd::Ones(this->XtUPVars());
+
     }
     virtual ~ODEPhaseBase() = default;
 
     //////////////////////////////////////////////////
+
+    virtual void setUnits(const Eigen::VectorXd& XtUPUnits_) = 0;
+    virtual void setUnits(const py::kwargs&);
+
+
     virtual void setControlMode(ControlModes m) {
       this->resetTranscription();
       this->invalidatePostOptInfo();
@@ -197,6 +223,8 @@ namespace ASSET {
     }
     virtual void setTranscriptionMode(TranscriptionModes m) = 0;
 
+
+
     void switchTranscriptionMode(TranscriptionModes m, VectorXd DBS, VectorXi DPB) {
       this->setTranscriptionMode(m);
       this->setTraj(this->ActiveTraj, DBS, DPB);
@@ -214,6 +242,33 @@ namespace ASSET {
     }
 
 
+    ///////////////////////////////////////////////////
+
+    void setStaticParamVgroups(std::map<std::string, Eigen::VectorXi> spidxs) {
+        this->SPidxs = spidxs;
+    }
+    void addStaticParamVgroups(std::map<std::string, Eigen::VectorXi> spidxs) {
+      for(auto& [key, value] : spidxs) {
+		this->SPidxs[key] = value;
+	  }
+    }
+    void addStaticParamVgroup(Eigen::VectorXi idx, std::string key) {
+        this->SPidxs[key] = idx;
+    }
+    void addStaticParamVgroup(int idx, std::string key) {
+        VectorXi tmp(1);
+        tmp << idx;
+        this->SPidxs[key] = tmp;
+    }
+
+    VectorXi getSPidx(std::string key) const {
+        if (SPidxs.count(key) == 0) {
+            throw std::invalid_argument(
+                fmt::format("No StaticParam variable index group with name: {0:} exists.", key));
+        }
+		return this->SPidxs.at(key);
+	}
+
     /////////////////////////////////////////////////
     template<class FuncMap>
     void removeFuncImpl(FuncMap& map, int index, const std::string& funcstr) {
@@ -240,418 +295,609 @@ namespace ASSET {
       return index;
     }
 
+   
 
-    /////////////////////////////////////////////////
-    int addEqualCon(StateConstraint con) {
-      return addFuncImpl(con, this->userEqualities, "Equality Constraint");
-    }
-    int addEqualCon(PhaseRegionFlags reg, VectorFunctionalX fun, VectorXi vars) {
-      return this->addEqualCon(StateConstraint(fun, reg, vars));
-    }
-    int addEqualCon(PhaseRegionFlags reg, VectorFunctionalX fun, VectorXi xv, VectorXi opv, VectorXi spv) {
-      return this->addEqualCon(StateConstraint(fun, reg, xv, opv, spv));
-    }
-
+    
+    PhaseRegionFlags getRegion(RegionType reg_t) const{
+        PhaseRegionFlags reg;
 
-    int addEqualCon(std::string reg, VectorFunctionalX fun, VectorXi vars) {
-      return this->addEqualCon(StateConstraint(fun, strto_PhaseRegionFlag(reg), vars));
-    }
-    int addEqualCon(std::string reg, VectorFunctionalX fun, VectorXi xv, VectorXi opv, VectorXi spv) {
-      return this->addEqualCon(StateConstraint(fun, strto_PhaseRegionFlag(reg), xv, opv, spv));
+        if (std::holds_alternative<PhaseRegionFlags>(reg_t)) {
+            reg = std::get<PhaseRegionFlags>(reg_t);
+        }
+        else if (std::holds_alternative<std::string>(reg_t)) {
+            reg = strto_PhaseRegionFlag(std::get<std::string>(reg_t));
+        }
+        return reg;
     }
 
 
-    int addDeltaVarEqualCon(PhaseRegionFlags reg, int var, double value, double scale);
-    int addDeltaVarEqualCon(int var, double value) {
-      return this->addDeltaVarEqualCon(PhaseRegionFlags::FrontandBack, var, value, 1.0);
-    }
-    int addDeltaVarEqualCon(int var, double value, double scale) {
-      return this->addDeltaVarEqualCon(PhaseRegionFlags::FrontandBack, var, value, scale);
-    }
+    VectorXi getXtUPVars(PhaseRegionFlags reg,VarIndexType XtUPvars_t) const {
 
-    int addDeltaTimeEqualCon(double value, double scale) {
-      return this->addDeltaVarEqualCon(this->TVar(), value, scale);
-    }
-    int addDeltaTimeEqualCon(double value) {
-      return this->addDeltaVarEqualCon(this->TVar(), value);
+        VectorXi XtUPvars;
+        
+        /////////////////////////////////////////////////
+        if (std::holds_alternative<int>(XtUPvars_t)) {
+            XtUPvars.resize(1);
+            XtUPvars[0] = std::get<int>(XtUPvars_t);
+        }
+        else if (std::holds_alternative<VectorXi>(XtUPvars_t)) {
+            XtUPvars = std::get<VectorXi>(XtUPvars_t);
+        }
+        else if (std::holds_alternative<std::string>(XtUPvars_t)) {
+            if (reg != StaticParams) {
+                XtUPvars = this->idx(std::get<std::string>(XtUPvars_t));
+            }
+            else {
+                XtUPvars = this->getSPidx(std::get<std::string>(XtUPvars_t));
+            }
+            if (reg == ODEParams) {
+                // Convert to 0 based index
+                for (int i = 0; i < XtUPvars.size(); i++) {
+                    XtUPvars[i] -= this->XtUVars();
+                }
+            }
+        }
+        else if (std::holds_alternative<std::vector<std::string>>(XtUPvars_t)) {
+
+            std::vector<VectorXi> varvec;
+            int size = 0;
+
+            auto tmpvars = std::get<std::vector<std::string>>(XtUPvars_t);
+
+            for (auto tmpv : tmpvars) {
+                if (reg != StaticParams) {
+                    varvec.push_back(this->idx(tmpv));
+                }
+                else {
+                    varvec.push_back(this->getSPidx(tmpv));
+                }
+
+                size += varvec.back().size();
+            }
+            XtUPvars.resize(size);
+
+            int next = 0;
+            for (auto varv : varvec) {
+                for (int i = 0; i < varv.size(); i++) {
+                    XtUPvars[next] = varv[i];
+                    next++;
+                }
+            }
+
+            if (reg == ODEParams) {
+                // Convert to 0 based index
+                for (int i = 0; i < XtUPvars.size(); i++) {
+                    XtUPvars[i] -= this->XtUVars();
+                }
+            }
+
+            
+        }
+        return XtUPvars;
     }
 
-    VectorXi addBoundaryValues(PhaseRegionFlags reg, VectorXi args, const VectorXd& value);
-    int addValueLock(PhaseRegionFlags reg, VectorXi args);
+    VectorXi getOPVars(PhaseRegionFlags reg, VarIndexType OPvars_t) const {
 
-    int addValueLock(std::string reg, VectorXi args) {
-      return this->addValueLock(strto_PhaseRegionFlag(reg), args);
-    }
+        VectorXi OPvars;
 
+        if (std::holds_alternative<int>(OPvars_t)) {
+            OPvars.resize(1);
+            OPvars[0] = std::get<int>(OPvars_t);
+        }
+        else if (std::holds_alternative<VectorXi>(OPvars_t)) {
+            OPvars = std::get<VectorXi>(OPvars_t);
+        }
+        else if (std::holds_alternative<std::string>(OPvars_t)) {
+            OPvars = this->idx(std::get<std::string>(OPvars_t));
 
-    int addBoundaryValue(PhaseRegionFlags reg, VectorXi args, const VectorXd& value);
-    int addBoundaryValue(std::string reg, VectorXi args, const VectorXd& value) {
-      return this->addBoundaryValue(strto_PhaseRegionFlag(reg), args, value);
-    }
+            for (int i = 0; i < OPvars.size(); i++) {
+                // Convert to 0 based index
+                OPvars[i] -= this->XtUVars();
+            }
+        }
+        else if (std::holds_alternative<std::vector<std::string>>(OPvars_t)) {
+            std::vector<VectorXi> varvec;
+            int size = 0;
+            auto tmpvars = std::get<std::vector<std::string>>(OPvars_t);
+            for (auto tmpv : tmpvars) {
+                varvec.push_back(this->idx(tmpv));
+                size += varvec.back().size();
+            }
+            OPvars.resize(size);
+
+            int next = 0;
+            for (auto varv : varvec) {
+                for (int i = 0; i < varv.size(); i++) {
+                    // Convert to 0 based index
+                    OPvars[next] = varv[i] - this->XtUVars();
+                    next++;
+                }
+            }
+        }
 
-    int addPeriodicityCon(VectorXi args);
-    //////////////////////////////////////////////////
-    //////////////////////////////////////////////////
-    int addInequalCon(StateConstraint con) {
-      return addFuncImpl(con, this->userInequalities, "Inequality Constraint");
-    }
-    int addInequalCon(PhaseRegionFlags reg, VectorFunctionalX fun, VectorXi vars) {
-      return this->addInequalCon(StateConstraint(fun, reg, vars));
-    }
-    int addInequalCon(PhaseRegionFlags reg, VectorFunctionalX fun, VectorXi xv, VectorXi opv, VectorXi spv) {
-      return this->addInequalCon(StateConstraint(fun, reg, xv, opv, spv));
+        return OPvars;
     }
 
-    int addInequalCon(std::string reg, VectorFunctionalX fun, VectorXi vars) {
-      return this->addInequalCon(StateConstraint(fun, strto_PhaseRegionFlag(reg), vars));
-    }
-    int addInequalCon(std::string reg, VectorFunctionalX fun, VectorXi xv, VectorXi opv, VectorXi spv) {
-      return this->addInequalCon(StateConstraint(fun, strto_PhaseRegionFlag(reg), xv, opv, spv));
-    }
+    VectorXi getSPVars(PhaseRegionFlags reg, VarIndexType SPvars_t) const {
 
-    ////////////////////////////////////////////////////
-    int addLUVarBound(
-        PhaseRegionFlags reg, int var, double lowerbound, double upperbound, double lbscale, double ubscale);
+        VectorXi SPvars;
 
+        if (std::holds_alternative<int>(SPvars_t)) {
+            SPvars.resize(1);
+            SPvars[0] = std::get<int>(SPvars_t);
+        }
+        else if (std::holds_alternative<VectorXi>(SPvars_t)) {
+            SPvars = std::get<VectorXi>(SPvars_t);
+        }
+        else if (std::holds_alternative<std::string>(SPvars_t)) {
+            SPvars = this->getSPidx(std::get<std::string>(SPvars_t));
+        }
+        else if (std::holds_alternative<std::vector<std::string>>(SPvars_t)) {
+            std::vector<VectorXi> varvec;
+            int size = 0;
+            auto tmpvars = std::get<std::vector<std::string>>(SPvars_t);
+            for (auto tmpv : tmpvars) {
+                varvec.push_back(this->getSPidx(tmpv));
+                size += varvec.back().size();
+            }
+            SPvars.resize(size);
+            int next = 0;
+            for (auto varv : varvec) {
+                for (int i = 0; i < varv.size(); i++) {
+                    SPvars[next] = varv[i];
+                    next++;
+                }
+            }
+        }
+        
 
-    int addLUVarBound(PhaseRegionFlags reg, int var, double lowerbound, double upperbound, double scale) {
-      return this->addLUVarBound(reg, var, lowerbound, upperbound, scale, scale);
+        return SPvars;
     }
 
 
-    Eigen::VectorXi addLUVarBounds(
-        PhaseRegionFlags reg, Eigen::VectorXi vars, double lowerbound, double upperbound, double scale) {
-      Eigen::VectorXi cnums(vars.size());
-      for (int i = 0; i < cnums.size(); i++) {
-        cnums[i] = this->addLUVarBound(reg, vars[i], lowerbound, upperbound, scale, scale);
-      }
 
-      return cnums;
-    }
+    template<class FuncHolder, class FuncType>
+    FuncHolder makeFuncImpl(RegionType reg_t,
+                     FuncType fun,
+                     VarIndexType XtUPvars_t,
+                     VarIndexType OPvars_t,
+                     VarIndexType SPvars_t,
+                     ScaleType scale_t) {
 
-    int addLUVarBound(PhaseRegionFlags reg, int var, double lowerbound, double upperbound) {
-      return this->addLUVarBound(reg, var, lowerbound, upperbound, 1.0);
-    }
 
-    int addLUVarBound(
-        std::string reg, int var, double lowerbound, double upperbound, double lbscale, double ubscale) {
-      return this->addLUVarBound(strto_PhaseRegionFlag(reg), var, lowerbound, upperbound, lbscale, ubscale);
-    }
+        PhaseRegionFlags reg = getRegion(reg_t);
+        FuncHolder func;
 
-    int addLUVarBound(std::string reg, int var, double lowerbound, double upperbound, double scale) {
-      return this->addLUVarBound(reg, var, lowerbound, upperbound, scale, scale);
-    }
+        if (std::holds_alternative<std::string>(XtUPvars_t)) {
+            std::string vars = std::get<std::string>(XtUPvars_t);
+            if (vars == "All" || vars == "all" ||vars=="XtUP") {
+                // Default case where the function has same inputs and order as ODE
+                VectorXi XtUPvars;
+                VectorXi OPvars;
+                VectorXi SPvars;
+                XtUPvars.setLinSpaced(this->XtUVars(), 0, this->XtUVars() - 1);
+                if (this->PVars() > 0) {
+                    OPvars.setLinSpaced(this->PVars(), 0, this->PVars() - 1);
+                }
+                func = FuncHolder(fun, reg, XtUPvars, OPvars, SPvars, scale_t);
+                return func; // return early
+            }
+        }
 
-    Eigen::VectorXi addLUVarBounds(
-        std::string reg, Eigen::VectorXi vars, double lowerbound, double upperbound, double scale) {
-      return addLUVarBounds(strto_PhaseRegionFlag(reg), vars, lowerbound, upperbound, scale);
-    }
+        VectorXi XtUPvars = this->getXtUPVars(reg,XtUPvars_t);
+        VectorXi OPvars;
+        VectorXi SPvars;
+        /////////////////////////////////////////////////
+        if (reg != ODEParams && reg != StaticParams) {
+            // If region is Params then the indices are held in XtUPvars_t and the others are emtpy
+            OPvars = this->getOPVars(reg, OPvars_t);
+            SPvars = this->getSPVars(reg, SPvars_t);
+            func = FuncHolder(fun, reg, XtUPvars, OPvars, SPvars, scale_t);
 
-    int addLUVarBound(std::string reg, int var, double lowerbound, double upperbound) {
-      return this->addLUVarBound(reg, var, lowerbound, upperbound, 1.0);
+        }
+        else {
+            func = FuncHolder(fun, reg, XtUPvars, scale_t);
+        }
+        
+        return func;
     }
-    ////////////////////////////////////////////////////
+   
 
 
-    int addLowerVarBound(PhaseRegionFlags reg, int var, double lowerbound, double lbscale);
-    int addLowerVarBound(std::string reg, int var, double lowerbound, double lbscale) {
-      return addLowerVarBound(strto_PhaseRegionFlag(reg), var, lowerbound, lbscale);
-    }
-
-    int addLowerVarBound(PhaseRegionFlags reg, int var, double lowerbound) {
-      return this->addLowerVarBound(reg, var, lowerbound, 1.0);
-    }
-    int addLowerVarBound(std::string reg, int var, double lowerbound) {
-      return this->addLowerVarBound(reg, var, lowerbound, 1.0);
+    
+    //////////////////////////////////////////////////
+    //////////////////////////////////////////////////
+    int addEqualCon(StateConstraint con) {
+        return addFuncImpl(con, this->userEqualities, "Equality Constraint");
     }
 
-    int addUpperVarBound(PhaseRegionFlags reg, int var, double upperbound, double ubscale);
-    int addUpperVarBound(std::string reg, int var, double upperbound, double ubscale) {
-      return addUpperVarBound(strto_PhaseRegionFlag(reg), var, upperbound, ubscale);
-    }
+    int addEqualCon(RegionType reg_t,
+        VectorFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        VarIndexType OPvars_t,
+        VarIndexType SPvars_t,
+        ScaleType scale_t) {
 
-    int addUpperVarBound(PhaseRegionFlags reg, int var, double upperbound) {
-      return this->addUpperVarBound(reg, var, upperbound, 1.0);
-    }
-    int addUpperVarBound(std::string reg, int var, double upperbound) {
-      return this->addUpperVarBound(reg, var, upperbound, 1.0);
+        auto con = makeFuncImpl<StateConstraint, VectorFunctionalX>(reg_t, fun, XtUPvars_t, OPvars_t, SPvars_t, scale_t);
+        return addFuncImpl(con, this->userEqualities, "Equality Constraint");
     }
 
-    ///////////////////////////////////////////////////
-    int addLUNormBound(PhaseRegionFlags reg,
-                       VectorXi vars,
-                       double lowerbound,
-                       double upperbound,
-                       double lbscale,
-                       double ubscale);
-    int addLUNormBound(std::string reg,
-                       VectorXi vars,
-                       double lowerbound,
-                       double upperbound,
-                       double lbscale,
-                       double ubscale) {
-      return addLUNormBound(strto_PhaseRegionFlag(reg), vars, lowerbound, upperbound, lbscale, ubscale);
-    }
+    int addEqualCon(RegionType reg_t,
+        VectorFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        ScaleType scale_t) {
 
-    int addLUNormBound(
-        PhaseRegionFlags reg, VectorXi vars, double lowerbound, double upperbound, double scale) {
-      return this->addLUNormBound(reg, vars, lowerbound, upperbound, scale, scale);
-    }
-    int addLUNormBound(PhaseRegionFlags reg, VectorXi vars, double lowerbound, double upperbound) {
-      return this->addLUNormBound(reg, vars, lowerbound, upperbound, 1.0);
-    }
-    int addLUNormBound(std::string reg, VectorXi vars, double lowerbound, double upperbound, double scale) {
-      return this->addLUNormBound(reg, vars, lowerbound, upperbound, scale, scale);
-    }
-    int addLUNormBound(std::string reg, VectorXi vars, double lowerbound, double upperbound) {
-      return this->addLUNormBound(reg, vars, lowerbound, upperbound, 1.0);
+        VectorXi empty;
+     
+        auto con = makeFuncImpl<StateConstraint, VectorFunctionalX>(reg_t, fun, XtUPvars_t, empty, empty, scale_t);
+        return addFuncImpl(con, this->userEqualities, "Equality Constraint");
     }
 
+    int addBoundaryValue(RegionType reg, VarIndexType args, const std::variant<double,VectorXd> & value, ScaleType scale_t);
+    int addDeltaVarEqualCon(VarIndexType var, double value, double scale, ScaleType scale_t);
+    int addDeltaTimeEqualCon(double value, double scale, ScaleType scale_t) {
+        return this->addDeltaVarEqualCon(this->TVar(), value, scale, scale_t);
 
-    int addLowerNormBound(PhaseRegionFlags reg, VectorXi vars, double lowerbound, double lbscale);
-    int addLowerNormBound(std::string reg, VectorXi vars, double lowerbound, double lbscale) {
-      return addLowerNormBound(strto_PhaseRegionFlag(reg), vars, lowerbound, lbscale);
     }
+    int addValueLock(RegionType reg, VarIndexType args, ScaleType scale_t);
+    int addPeriodicityCon(VarIndexType args, ScaleType scale_t);
 
-    int addLowerNormBound(PhaseRegionFlags reg, VectorXi vars, double lowerbound) {
-      return this->addLowerNormBound(reg, vars, lowerbound, 1.0);
-    }
-    int addLowerNormBound(std::string reg, VectorXi vars, double lowerbound) {
-      return this->addLowerNormBound(reg, vars, lowerbound, 1.0);
+    /////////////////////////////////////////////////
+    
+    //////////////////////////////////////////////////
+    //////////////////////////////////////////////////
+    int addInequalCon(StateConstraint con) {
+        return addFuncImpl(con, this->userInequalities, "Inequality Constraint");
     }
+    int addInequalCon(RegionType reg_t,
+        VectorFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        VarIndexType OPvars_t,
+        VarIndexType SPvars_t,
+        ScaleType scale_t) {
 
-    int addUpperNormBound(PhaseRegionFlags reg, VectorXi vars, double upperbound, double ubscale);
-    int addUpperNormBound(std::string reg, VectorXi vars, double upperbound, double ubscale) {
-      return addUpperNormBound(strto_PhaseRegionFlag(reg), vars, upperbound, ubscale);
-    }
-    int addUpperNormBound(PhaseRegionFlags reg, VectorXi vars, double upperbound) {
-      return this->addUpperNormBound(reg, vars, upperbound, 1.0);
-    }
-    int addUpperNormBound(std::string reg, VectorXi vars, double upperbound) {
-      return this->addUpperNormBound(reg, vars, upperbound, 1.0);
+        auto con = makeFuncImpl<StateConstraint, VectorFunctionalX>(reg_t, fun, XtUPvars_t, OPvars_t, SPvars_t, scale_t);
+        return addFuncImpl(con, this->userInequalities, "Inequality Constraint");
     }
 
-    ///////////////////////////////////////////////////
-    int addLUSquaredNormBound(PhaseRegionFlags reg,
-                              VectorXi vars,
-                              double lowerbound,
-                              double upperbound,
-                              double lbscale,
-                              double ubscale);
+    int addInequalCon(RegionType reg_t,
+        VectorFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        ScaleType scale_t) {
 
-    int addLUSquaredNormBound(std::string reg,
-                              VectorXi vars,
-                              double lowerbound,
-                              double upperbound,
-                              double lbscale,
-                              double ubscale) {
-      return addLUSquaredNormBound(
-          strto_PhaseRegionFlag(reg), vars, lowerbound, upperbound, lbscale, ubscale);
-    }
+        VectorXi empty;
 
-    int addLUSquaredNormBound(
-        PhaseRegionFlags reg, VectorXi vars, double lowerbound, double upperbound, double scale) {
-      return this->addLUSquaredNormBound(reg, vars, lowerbound, upperbound, scale, scale);
-    }
-    int addLUSquaredNormBound(PhaseRegionFlags reg, VectorXi vars, double lowerbound, double upperbound) {
-      return this->addLUSquaredNormBound(reg, vars, lowerbound, upperbound, 1.0);
-    }
-    int addLUSquaredNormBound(
-        std::string reg, VectorXi vars, double lowerbound, double upperbound, double scale) {
-      return this->addLUSquaredNormBound(reg, vars, lowerbound, upperbound, scale, scale);
-    }
-    int addLUSquaredNormBound(std::string reg, VectorXi vars, double lowerbound, double upperbound) {
-      return this->addLUSquaredNormBound(reg, vars, lowerbound, upperbound, 1.0);
+        auto con = makeFuncImpl<StateConstraint, VectorFunctionalX>(reg_t, fun, XtUPvars_t, empty, empty, scale_t);
+        return addFuncImpl(con, this->userInequalities, "Inequality Constraint");
     }
 
-    int addLowerSquaredNormBound(PhaseRegionFlags reg, VectorXi vars, double lowerbound, double lbscale);
-    int addLowerSquaredNormBound(std::string reg, VectorXi vars, double lowerbound, double lbscale) {
-      return addLowerSquaredNormBound(strto_PhaseRegionFlag(reg), vars, lowerbound, lbscale);
-    }
-    int addLowerSquaredNormBound(PhaseRegionFlags reg, VectorXi vars, double lowerbound) {
-      return this->addLowerSquaredNormBound(reg, vars, lowerbound, 1.0);
-    }
-    int addLowerSquaredNormBound(std::string reg, VectorXi vars, double lowerbound) {
-      return this->addLowerSquaredNormBound(reg, vars, lowerbound, 1.0);
-    }
+    ////////////////////////
+    int addLUVarBound(
+        RegionType reg, VarIndexType var, double lowerbound, double upperbound, double lbscale, double ubscale,
+        ScaleType scale_t);
 
-    int addUpperSquaredNormBound(PhaseRegionFlags reg, VectorXi vars, double upperbound, double ubscale);
-    int addUpperSquaredNormBound(std::string reg, VectorXi vars, double upperbound, double ubscale) {
-      return addUpperSquaredNormBound(strto_PhaseRegionFlag(reg), vars, upperbound, ubscale);
-    }
-    int addUpperSquaredNormBound(PhaseRegionFlags reg, VectorXi vars, double upperbound) {
-      return this->addUpperSquaredNormBound(reg, vars, upperbound, 1.0);
+    int addLUVarBound(
+        RegionType reg, VarIndexType var, double lowerbound, double upperbound, double scale,
+        ScaleType scale_t) {
+        return this->addLUVarBound(reg, var, lowerbound, upperbound, scale, scale, scale_t);
     }
-    int addUpperSquaredNormBound(std::string reg, VectorXi vars, double upperbound) {
-      return this->addUpperSquaredNormBound(reg, vars, upperbound, 1.0);
+    int addLUVarBound(
+        RegionType reg, VarIndexType var, double lowerbound, double upperbound,
+        ScaleType scale_t) {
+        return this->addLUVarBound(reg, var, lowerbound, upperbound, 1.0, 1.0, scale_t);
     }
 
+    int addLowerVarBound(
+        RegionType reg, VarIndexType var, double lowerbound, double lbscale,ScaleType scale_t);
 
-    ///////////////////////////////////////////////////
-    int addLowerFuncBound(
-        PhaseRegionFlags reg, ScalarFunctionalX func, VectorXi vars, double lowerbound, double lbscale);
-    int addLowerFuncBound(
-        std::string reg, ScalarFunctionalX func, VectorXi vars, double lowerbound, double lbscale) {
-      return addLowerFuncBound(strto_PhaseRegionFlag(reg), func, vars, lowerbound, lbscale);
-    }
+    int addUpperVarBound(
+        RegionType reg, VarIndexType var, double upperbound, double ubscale, ScaleType scale_t);
 
-    int addUpperFuncBound(
-        PhaseRegionFlags reg, ScalarFunctionalX func, VectorXi vars, double upperbound, double ubscale);
-    int addUpperFuncBound(
-        std::string reg, ScalarFunctionalX func, VectorXi vars, double upperbound, double ubscale) {
-      return addUpperFuncBound(strto_PhaseRegionFlag(reg), func, vars, upperbound, ubscale);
-    }
+    int addLUFuncBound(RegionType reg,
+        ScalarFunctionalX func,
+        VarIndexType XtUPvars,
+        VarIndexType OPvars,
+        VarIndexType SPvars,
+        double lowerbound,
+        double upperbound,
+        double lbscale,
+        double ubscale, ScaleType scale_t);
 
+    
 
-    int addLowerFuncBound(PhaseRegionFlags reg, ScalarFunctionalX func, VectorXi vars, double lowerbound) {
-      return this->addLowerFuncBound(reg, func, vars, lowerbound, 1.0);
-    }
-    int addUpperFuncBound(PhaseRegionFlags reg, ScalarFunctionalX func, VectorXi vars, double upperbound) {
-      return this->addUpperFuncBound(reg, func, vars, upperbound, 1.0);
+    int addLUFuncBound(RegionType reg,
+        ScalarFunctionalX func,
+        VarIndexType XtUPvars,
+        double lowerbound,
+        double upperbound,
+        double lbscale,
+        double ubscale, ScaleType scale_t) {
+
+        VectorXi empty;
+
+        return addLUFuncBound(reg, func, XtUPvars, empty, empty, lowerbound,upperbound, lbscale, ubscale, scale_t);
+    }
+
+    int addLUFuncBound(RegionType reg,
+        ScalarFunctionalX func,
+        VarIndexType XtUPvars,
+        VarIndexType OPvars,
+        VarIndexType SPvars,
+        double lowerbound,
+        double upperbound,
+        double scale,
+        ScaleType scale_t) {
+        return addLUFuncBound(reg, func, XtUPvars, OPvars, SPvars, lowerbound, upperbound, scale, scale, scale_t);
+    }
+
+    int addLUFuncBound(RegionType reg,
+        ScalarFunctionalX func,
+        VarIndexType XtUPvars,
+        double lowerbound,
+        double upperbound,
+        double scale,
+        ScaleType scale_t) {
+        VectorXi empty;
+        return addLUFuncBound(reg, func, XtUPvars, empty, empty, lowerbound, upperbound, scale, scale, scale_t);
+    }
+
+    int addLowerFuncBound(RegionType reg,
+        ScalarFunctionalX func,
+        VarIndexType XtUPvars,
+        VarIndexType OPvars,
+        VarIndexType SPvars,
+        double lowerbound,
+        double lbscale,
+        ScaleType scale_t);
+
+    int addLowerFuncBound(RegionType reg,
+        ScalarFunctionalX func,
+        VarIndexType XtUPvars,
+        double lowerbound,
+        double lbscale,
+        ScaleType scale_t) {
+        VectorXi empty;
+
+        return this->addLowerFuncBound(reg, func, XtUPvars, empty, empty, lowerbound, lbscale, scale_t);
+    }
+
+
+    int addUpperFuncBound(RegionType reg,
+        ScalarFunctionalX func,
+        VarIndexType XtUPvars,
+        VarIndexType OPvars,
+        VarIndexType SPvars,
+        double upperbound,
+        double ubscale,
+        ScaleType scale_t);
+
+    int addUpperFuncBound(RegionType reg,
+        ScalarFunctionalX func,
+        VarIndexType XtUPvars,
+        double upperbound,
+        double ubscale,
+        ScaleType scale_t) {
+        VectorXi empty;
+
+        return this->addUpperFuncBound(reg, func, XtUPvars, empty, empty, upperbound, ubscale, scale_t);
+    }
+
+    int addLUNormBound(RegionType reg,
+        VarIndexType XtUPvars,
+        double lowerbound,
+        double upperbound,
+        double lbscale,
+        double ubscale,
+        ScaleType scale_t);
+
+    int addLUNormBound(RegionType reg,
+        VarIndexType XtUPvars,
+        double lowerbound,
+        double upperbound,
+        double scale,
+        ScaleType scale_t) {
+        return this->addLUNormBound(reg, XtUPvars, lowerbound, upperbound, scale, scale, scale_t);
+    }
+
+    int addLUSquaredNormBound(RegionType reg,
+        VarIndexType XtUPvars,
+        double lowerbound,
+        double upperbound,
+        double lbscale,
+        double ubscale,
+        ScaleType scale_t);
+
+    int addLUSquaredNormBound(RegionType reg,
+        VarIndexType XtUPvars,
+        double lowerbound,
+        double upperbound,
+        double scale,
+        ScaleType scale_t) {
+        return this->addLUSquaredNormBound(reg, XtUPvars, lowerbound, upperbound, scale, scale, scale_t);
+    }
+
+
+    int addLowerNormBound(RegionType reg,
+        VarIndexType XtUPvars,
+        double lowerbound,
+        double lbscale,
+        ScaleType scale_t);
+
+    int addLowerSquaredNormBound(RegionType reg,
+        VarIndexType XtUPvars,
+        double lowerbound,
+        double lbscale,
+        ScaleType scale_t);
+
+    int addUpperNormBound(RegionType reg,
+        VarIndexType XtUPvars,
+        double upperbound,
+        double ubscale,
+        ScaleType scale_t);
+
+    int addUpperSquaredNormBound(RegionType reg,
+        VarIndexType XtUPvars,
+        double upperbound,
+        double ubscale,
+        ScaleType scale_t);
+    //
+    int addLowerDeltaVarBound(RegionType reg, VarIndexType var, double lowerbound, double lbscale,
+        ScaleType scale_t);
+    int addLowerDeltaVarBound(VarIndexType var, double lowerbound, double lbscale,
+        ScaleType scale_t) {
+        return this->addLowerDeltaVarBound(PhaseRegionFlags::FrontandBack, var, lowerbound, lbscale, scale_t);
+    }
+    int addLowerDeltaTimeBound(double lowerbound, double lbscale,
+        ScaleType scale_t) {
+        return this->addLowerDeltaVarBound(this->TVar(), lowerbound, lbscale, scale_t);
+    }
+    ///
+    int addUpperDeltaVarBound(RegionType reg, VarIndexType var, double upperbound, double ubscale,
+        ScaleType scale_t);
+    int addUpperDeltaVarBound(VarIndexType var, double upperbound, double ubscale,
+        ScaleType scale_t) {
+        return this->addUpperDeltaVarBound(PhaseRegionFlags::FrontandBack, var, upperbound, ubscale, scale_t);
+    }
+    int addUpperDeltaTimeBound(double upperbound, double ubscale,
+        ScaleType scale_t) {
+        return this->addUpperDeltaVarBound(this->TVar(), upperbound, ubscale, scale_t);
     }
 
-    int addLowerFuncBound(std::string reg, ScalarFunctionalX func, VectorXi vars, double lowerbound) {
-      return this->addLowerFuncBound(reg, func, vars, lowerbound, 1.0);
-    }
-    int addUpperFuncBound(std::string reg, ScalarFunctionalX func, VectorXi vars, double upperbound) {
-      return this->addUpperFuncBound(reg, func, vars, upperbound, 1.0);
-    }
+    ///////////////////////////////////////////////////////////////
+    
+    
 
+    ////////////////////////////////////////////////////
+    int addLUVarBound(
+        PhaseRegionFlags reg, int var, double lowerbound, double upperbound, double lbscale, double ubscale);
 
-    int addLUFuncBound(PhaseRegionFlags reg,
-                       ScalarFunctionalX func,
-                       VectorXi vars,
-                       double lowerbound,
-                       double upperbound,
-                       double lbscale,
-                       double ubscale);
 
-    int addLUFuncBound(std::string reg,
-                       ScalarFunctionalX func,
-                       VectorXi vars,
-                       double lowerbound,
-                       double upperbound,
-                       double lbscale,
-                       double ubscale) {
-      return addLUFuncBound(strto_PhaseRegionFlag(reg), func, vars, lowerbound, upperbound, lbscale, ubscale);
+    int addLUVarBound(PhaseRegionFlags reg, int var, double lowerbound, double upperbound, double scale) {
+      return this->addLUVarBound(reg, var, lowerbound, upperbound, scale, scale);
     }
 
-    int addLUFuncBound(PhaseRegionFlags reg,
-                       ScalarFunctionalX func,
-                       VectorXi vars,
-                       double lowerbound,
-                       double upperbound,
-                       double scale) {
-      return this->addLUFuncBound(reg, func, vars, lowerbound, upperbound, scale, scale);
-    }
-    int addLUFuncBound(
-        PhaseRegionFlags reg, ScalarFunctionalX func, VectorXi vars, double lowerbound, double upperbound) {
-      return this->addLUFuncBound(reg, func, vars, lowerbound, upperbound, 1.0, 1.0);
-    }
 
-    int addLUFuncBound(std::string reg,
-                       ScalarFunctionalX func,
-                       VectorXi vars,
-                       double lowerbound,
-                       double upperbound,
-                       double scale) {
-      return this->addLUFuncBound(reg, func, vars, lowerbound, upperbound, scale, scale);
-    }
-    int addLUFuncBound(
-        std::string reg, ScalarFunctionalX func, VectorXi vars, double lowerbound, double upperbound) {
-      return this->addLUFuncBound(reg, func, vars, lowerbound, upperbound, 1.0, 1.0);
-    }
+    Eigen::VectorXi addLUVarBounds(
+        PhaseRegionFlags reg, Eigen::VectorXi vars, double lowerbound, double upperbound, double scale) {
+      Eigen::VectorXi cnums(vars.size());
+      for (int i = 0; i < cnums.size(); i++) {
+        cnums[i] = this->addLUVarBound(reg, vars[i], lowerbound, upperbound, scale, scale);
+      }
 
-    ///////////////////////////////////////////////////////////////
-    int addLowerDeltaVarBound(PhaseRegionFlags reg, int var, double lowerbound, double lbscale);
-    int addLowerDeltaVarBound(int var, double lowerbound, double lbscale) {
-      return this->addLowerDeltaVarBound(PhaseRegionFlags::FrontandBack, var, lowerbound, lbscale);
-    }
-    int addLowerDeltaVarBound(int var, double lowerbound) {
-      return this->addLowerDeltaVarBound(PhaseRegionFlags::FrontandBack, var, lowerbound, 1.0);
+      return cnums;
     }
 
-    int addLowerDeltaTimeBound(double lowerbound, double lbscale) {
-      return this->addLowerDeltaVarBound(this->TVar(), lowerbound, lbscale);
-    }
-    int addLowerDeltaTimeBound(double lowerbound) {
-      return this->addLowerDeltaVarBound(this->TVar(), lowerbound, 1.0);
+    Eigen::VectorXi addLUVarBounds(
+        std::string reg, Eigen::VectorXi vars, double lowerbound, double upperbound, double scale) {
+      return addLUVarBounds(strto_PhaseRegionFlag(reg), vars, lowerbound, upperbound, scale);
     }
 
-    int addUpperDeltaVarBound(PhaseRegionFlags reg, int var, double upperbound, double ubscale);
-    int addUpperDeltaVarBound(int var, double upperbound, double ubscale) {
-      return this->addUpperDeltaVarBound(PhaseRegionFlags::FrontandBack, var, upperbound, ubscale);
-    }
-    int addUpperDeltaVarBound(int var, double upperbound) {
-      return this->addUpperDeltaVarBound(PhaseRegionFlags::FrontandBack, var, upperbound, 1.0);
-    }
+    
+    ////////////////////////////////////////////////////
 
-    int addUpperDeltaTimeBound(double upperbound, double ubscale) {
-      return this->addUpperDeltaVarBound(this->TVar(), upperbound, ubscale);
-    }
-    int addUpperDeltaTimeBound(double upperbound) {
-      return this->addUpperDeltaVarBound(this->TVar(), upperbound, 1.0);
-    }
 
+   
     //////////////////////////////////////////////////
     //////////////////////////////////////////////////
     //////////////////////////////////////////////////
     int addStateObjective(StateObjective obj) {
-      return addFuncImpl(obj, this->userStateObjectives, "State Objective");
-    }
-    int addStateObjective(PhaseRegionFlags reg, ScalarFunctionalX fun, VectorXi vars) {
-      return this->addStateObjective(StateObjective(fun, reg, vars));
-    }
-    int addStateObjective(
-        PhaseRegionFlags reg, ScalarFunctionalX fun, VectorXi xv, VectorXi opv, VectorXi spv) {
-      return this->addStateObjective(StateObjective(fun, reg, xv, opv, spv));
+        return addFuncImpl(obj, this->userStateObjectives, "State Objective");
     }
+    int addStateObjective(RegionType reg_t,
+        ScalarFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        VarIndexType OPvars_t,
+        VarIndexType SPvars_t,
+        ScaleType scale_t) {
 
-    int addStateObjective(std::string reg, ScalarFunctionalX fun, VectorXi vars) {
-      return this->addStateObjective(StateObjective(fun, strto_PhaseRegionFlag(reg), vars));
-    }
-    int addStateObjective(std::string reg, ScalarFunctionalX fun, VectorXi xv, VectorXi opv, VectorXi spv) {
-      return this->addStateObjective(StateObjective(fun, strto_PhaseRegionFlag(reg), xv, opv, spv));
+        auto con = makeFuncImpl<StateObjective, ScalarFunctionalX>(reg_t, fun, XtUPvars_t, OPvars_t, SPvars_t, scale_t);
+        return addFuncImpl(con, this->userStateObjectives, "State Objective");
     }
 
+    int addStateObjective(RegionType reg_t,
+        ScalarFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        ScaleType scale_t) {
 
-    int addValueObjective(PhaseRegionFlags reg, int var, double scale);
-    int addValueObjective(std::string reg, int var, double scale) {
-      return addValueObjective(strto_PhaseRegionFlag(reg), var, scale);
+        VectorXi empty;
+
+        auto con = makeFuncImpl<StateObjective, ScalarFunctionalX>(reg_t, fun, XtUPvars_t, empty, empty, scale_t);
+        return addFuncImpl(con, this->userStateObjectives, "State Objective");
+    }
+    int addValueObjective(RegionType reg, VarIndexType var, double scale,ScaleType scale_t);
+    int addDeltaVarObjective(VarIndexType var, double scale, ScaleType scale_t);
+    int addDeltaTimeObjective(double scale, ScaleType scale_t) {
+        return this->addDeltaVarObjective(this->TVar(), scale, scale_t);
     }
 
+    ///////////////////////////////////////////////
+    
+    
 
-    int addDeltaVarObjective(int var, double lbscale);
-    int addDeltaTimeObjective(double scale) {
-      return this->addDeltaVarObjective(this->TVar(), scale);
-    }
+
+    
     ///////////////////////////////////////////////////
     int addIntegralObjective(StateObjective obj) {
-      return addFuncImpl(obj, this->userIntegrands, "Integral Objective");
+        return addFuncImpl(obj, this->userIntegrands, "Integral Objective");
     }
-    int addIntegralObjective(ScalarFunctionalX fun, VectorXi vars) {
-      return this->addIntegralObjective(StateObjective(fun, Path, vars));
+
+    int addIntegralObjective(
+        ScalarFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        VarIndexType OPvars_t,
+        VarIndexType SPvars_t,
+        ScaleType scale_t) {
+
+        auto con = makeFuncImpl<StateObjective, ScalarFunctionalX>(Path, fun, XtUPvars_t, OPvars_t, SPvars_t, scale_t);
+        return addFuncImpl(con, this->userIntegrands, "Integral Objective");
     }
-    int addIntegralObjective(ScalarFunctionalX fun, VectorXi XtUvars, VectorXi OPvars, VectorXi SPvars) {
-      return this->addIntegralObjective(StateObjective(fun, Path, XtUvars, OPvars, SPvars));
+
+    int addIntegralObjective(
+        ScalarFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        ScaleType scale_t) {
+
+        VectorXi empty;
+
+        auto con = makeFuncImpl<StateObjective, ScalarFunctionalX>(Path, fun, XtUPvars_t, empty, empty, scale_t);
+        return addFuncImpl(con, this->userIntegrands, "Integral Objective");
     }
+
+
     ///////////////////////////////////////////////////
     int addIntegralParamFunction(StateObjective con, int pv) {
-      VectorXi epv(1);
-      epv[0] = pv;
-      int index = addFuncImpl(con, this->userParamIntegrands, "Integral Parameter Function");
-      this->userParamIntegrands[index].EXTVars = epv;
-      return index;
-    }
-    int addIntegralParamFunction(ScalarFunctionalX fun, VectorXi vars, int accum_parm) {
-      return this->addIntegralParamFunction(StateObjective(fun, Path, vars), accum_parm);
+        VectorXi epv(1);
+        epv[0] = pv;
+        int index = addFuncImpl(con, this->userParamIntegrands, "Integral Parameter Function");
+        this->userParamIntegrands[index].EXTVars = epv;
+        return index;
     }
 
     int addIntegralParamFunction(
-        ScalarFunctionalX fun, VectorXi XtUvars, VectorXi OPvars, VectorXi SPvars, int accum_parm) {
-      return this->addIntegralParamFunction(StateObjective(fun, Path, XtUvars, OPvars, SPvars), accum_parm);
+        ScalarFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        VarIndexType OPvars_t,
+        VarIndexType SPvars_t,
+        int accum_parm,
+        ScaleType scale_t) {
+
+        VectorXi epv(1);
+        epv[0] = accum_parm;
+
+        auto con = makeFuncImpl<StateObjective, ScalarFunctionalX>(Path, fun, XtUPvars_t, OPvars_t, SPvars_t, scale_t);
+        int index = addFuncImpl(con, this->userParamIntegrands, "Integral Parameter Function");
+        this->userParamIntegrands[index].EXTVars = epv;
+        return index;
     }
 
+    int addIntegralParamFunction(
+        ScalarFunctionalX fun,
+        VarIndexType XtUPvars_t,
+        int accum_parm,
+        ScaleType scale_t) {
+        VectorXi empty;
+        return addIntegralParamFunction(fun, XtUPvars_t, empty, empty, accum_parm, scale_t);
+    }
 
     /////////////////////////////////////////////////
 
@@ -680,6 +926,8 @@ namespace ASSET {
                          Eigen::VectorXi DPB,
                          bool LerpTraj);
 
+    void setTraj(const std::vector<Eigen::VectorXd>& mesh);
+
     void setTraj(const std::vector<Eigen::VectorXd>& mesh, Eigen::VectorXd DBS, Eigen::VectorXi DPB) {
       this->setTraj(mesh, DBS, DPB, false);
     }
@@ -712,18 +960,45 @@ namespace ASSET {
     void refineTrajAuto();
 
 
-    void setStaticParams(VectorXd parm) {
+    void setStaticParams(VectorXd parm,VectorXd units) {
+      if (units.size() != parm.size()) {
+          throw std::invalid_argument("Size of static parameter vector and scaling units vector must match");
+      }
+
       this->ActiveStaticParams = parm;
       this->numStatParams = parm.size();
       this->resetTranscription();
+      this->SPUnits = units;
     }
+    void setStaticParams(VectorXd parm) {
+        VectorXd units(parm.size());
+        units.setOnes();
+        return this->setStaticParams(parm, units);
+    }
+
+    void addStaticParams(VectorXd parm, VectorXd units) {
+	  if (this->numStatParams == 0) {
+		this->setStaticParams(parm,units);
+	  } else {
+          VectorXd parmstmp(this->ActiveStaticParams.size()+ parm.size());
+          parmstmp << this->ActiveStaticParams, parm;
+          VectorXd unitstmp(this->SPUnits.size() + units.size());
+          unitstmp << this->SPUnits, units;
+          this->setStaticParams(parmstmp,unitstmp);
+	  }
+	}
+	void addStaticParams(VectorXd parm) {
+		VectorXd units(parm.size());
+		units.setOnes();
+		return this->addStaticParams(parm, units);
+	}
+
+
     void subStaticParams(VectorXd parm) {
-      this->ActiveStaticParams = parm;
       if (this->numStatParams == parm.size()) {
-        // expected behavior
+          this->ActiveStaticParams = parm;
       } else {
-        this->numStatParams = parm.size();
-        this->resetTranscription();
+          this->setStaticParams(parm);
       }
     }
 
@@ -809,6 +1084,10 @@ namespace ASSET {
       int Gindex = this->userEqualities.at(index).GlobalIndex;
       return this->indexer.getFuncEqMultipliers(Gindex, this->ActiveEqCons);
     }
+    Eigen::VectorXd returnEqualConScales(int index) const {
+       return this->userEqualities.at(index).OutputScales;
+    }
+
 
     std::vector<Eigen::VectorXd> returnInequalConLmults(int index) const {
       if (!this->PostOptInfoValid) {
@@ -827,10 +1106,27 @@ namespace ASSET {
       int Gindex = this->userInequalities.at(index).GlobalIndex;
       return this->indexer.getFuncIqMultipliers(Gindex, this->ActiveIqCons);
     }
+    Eigen::VectorXd returnInequalConScales(int index) const {
+        return this->userInequalities.at(index).OutputScales;
+    }
 
     std::vector<Eigen::VectorXd> returnCostateTraj() const;
     std::vector<Eigen::VectorXd> returnTrajError() const;
 
+    Eigen::VectorXd returnIntegralObjectiveScales(int index) const {
+        return this->userIntegrands.at(index).OutputScales;
+    }
+    Eigen::VectorXd returnIntegralParamFunctionScales(int index) const {
+        return this->userParamIntegrands.at(index).OutputScales;
+    }
+    Eigen::VectorXd returnStateObjectiveScales(int index) const {
+        return this->userStateObjectives.at(index).OutputScales;
+    }
+    Eigen::VectorXd returnODEOutputScales() const {
+        VectorXd output_scales = XtUPUnits.head(this->XVars()).cwiseInverse() * this->XtUPUnits[this->XVars()];
+        return output_scales;
+    }
+
     /////////////////////////////////////////////////
    protected:
     virtual void transcribe_dynamics() = 0;
@@ -841,10 +1137,8 @@ namespace ASSET {
 
     void initIndexing() {
       this->indexer = PhaseIndexer(this->XVars(), this->UVars(), this->PVars(), this->numStatParams);
-      bool blockcon = false;
-      if (this->ControlMode == ControlModes::BlockConstant)
-        blockcon = true;
-      this->indexer.set_dimensions(this->numTranCardStates, this->numDefects, blockcon);
+      this->indexer.set_dimensions(this->numTranCardStates, this->numDefects, 
+          this->ControlMode == ControlModes::BlockConstant);
     }
     void check_functions(int pnum);
 
@@ -858,6 +1152,7 @@ namespace ASSET {
         case Params:
         case ODEParams:
         case StaticParams:
+        case InnerPath:
         case NodalPath: {
           int isize = func.XtUVars.size() + func.OPVars.size() + func.SPVars.size();
           if (irows != isize) {
@@ -896,6 +1191,16 @@ namespace ASSET {
       }
     }
 
+    Eigen::VectorXd get_input_scale(PhaseRegionFlags flag, VectorXi XtUV, VectorXi OPV, VectorXi SPV) const;
+
+    std::vector<Eigen::VectorXd> get_test_inputs(PhaseRegionFlags flag, VectorXi XtUV, VectorXi OPV, VectorXi SPV) const;
+
+    void calc_auto_scales();
+
+    std::vector<double> get_objective_scales();
+    void update_objective_scales(double scale);
+    
+
 
     static void check_lbscale(double lbscale) {
       if (lbscale <= 0.0) {
@@ -921,10 +1226,38 @@ namespace ASSET {
 
 
     Eigen::VectorXd makeSolverInput() const {
-      return this->indexer.makeSolverInput(this->ActiveTraj, this->ActiveStaticParams);
+
+        if (this->AutoScaling) {
+            auto ActiveTrajTmp = this->ActiveTraj;
+
+            for (auto& T : ActiveTrajTmp) {
+                T = T.cwiseQuotient(this->XtUPUnits);
+            }
+
+            VectorXd StaticParamsTmp;
+            if (this->ActiveStaticParams.size() > 0 && this->SPUnits.size()>0) {
+                StaticParamsTmp = this->ActiveStaticParams.cwiseQuotient(this->SPUnits);
+            }
+            return this->indexer.makeSolverInput(ActiveTrajTmp, StaticParamsTmp);
+
+      
+        } else {
+        return this->indexer.makeSolverInput(this->ActiveTraj, this->ActiveStaticParams);
+        }
     }
     void collectSolverOutput(const VectorXd& Vars) {
       this->indexer.collectSolverOutput(Vars, this->ActiveTraj, this->ActiveStaticParams);
+
+      if (this->AutoScaling) {
+          for (auto& T : this->ActiveTraj) {
+              T = T.cwiseProduct(this->XtUPUnits);
+          }
+          if (this->ActiveStaticParams.size() > 0 && this->SPUnits.size() > 0) {
+              this->ActiveStaticParams = this->ActiveStaticParams.cwiseProduct(this->SPUnits);
+          }
+
+      }
+
     }
     void collectSolverMultipliers(const VectorXd& EM, const VectorXd& IM) {
       this->MultipliersLoaded = true;
diff --git a/src/OptimalControl/ODEPhaseBaseBind.cpp b/src/OptimalControl/ODEPhaseBaseBind.cpp
new file mode 100644
index 00000000..4f6c16b9
--- /dev/null
+++ b/src/OptimalControl/ODEPhaseBaseBind.cpp
@@ -0,0 +1,724 @@
+#include "ODEPhaseBase.h"
+#include "PyDocString/OptimalControl/ODEPhaseBase_doc.h"
+
+void ASSET::ODEPhaseBase::Build(py::module& m) {
+    using namespace pybind11::literals;
+    using namespace doc;
+    auto obj =
+        py::class_<ODEPhaseBase, std::shared_ptr<ODEPhaseBase>, OptimizationProblemBase>(m, "PhaseInterface");
+    obj.doc() = "Base Class for All Optimal Control Phases";
+
+    obj.def("enable_vectorization", &ODEPhaseBase::enable_vectorization);
+
+    obj.def("setTraj",
+        py::overload_cast<const std::vector<Eigen::VectorXd>&, Eigen::VectorXd, Eigen::VectorXi>(
+            &ODEPhaseBase::setTraj),
+        ODEPhaseBase_setTraj1);
+
+    obj.def("setTraj",
+        py::overload_cast<const std::vector<Eigen::VectorXd>&, Eigen::VectorXd, Eigen::VectorXi, bool>(
+            &ODEPhaseBase::setTraj));
+
+    obj.def("setTraj",
+        py::overload_cast<const std::vector<Eigen::VectorXd>&, int>(&ODEPhaseBase::setTraj),
+        ODEPhaseBase_setTraj2);
+
+    obj.def("setTraj",
+        py::overload_cast<const std::vector<Eigen::VectorXd>&, int, bool>(&ODEPhaseBase::setTraj));
+
+    obj.def("setTraj",
+        py::overload_cast<const std::vector<Eigen::VectorXd>&>(&ODEPhaseBase::setTraj));
+
+
+    obj.def("switchTranscriptionMode",
+        py::overload_cast<TranscriptionModes, VectorXd, VectorXi>(&ODEPhaseBase::switchTranscriptionMode),
+        ODEPhaseBase_switchTranscriptionMode1);
+    obj.def("switchTranscriptionMode",
+        py::overload_cast<TranscriptionModes>(&ODEPhaseBase::switchTranscriptionMode),
+        ODEPhaseBase_switchTranscriptionMode2);
+
+
+    obj.def("switchTranscriptionMode",
+        py::overload_cast<std::string, VectorXd, VectorXi>(&ODEPhaseBase::switchTranscriptionMode),
+        ODEPhaseBase_switchTranscriptionMode1);
+    obj.def("switchTranscriptionMode",
+        py::overload_cast<std::string>(&ODEPhaseBase::switchTranscriptionMode),
+        ODEPhaseBase_switchTranscriptionMode2);
+
+
+    obj.def("transcribe", py::overload_cast<bool, bool>(&ODEPhaseBase::transcribe), ODEPhaseBase_transcribe);
+
+    obj.def("refineTrajManual",
+        py::overload_cast<int>(&ODEPhaseBase::refineTrajManual),
+        ODEPhaseBase_refineTrajManual1);
+    obj.def("refineTrajManual",
+        py::overload_cast<VectorXd, VectorXi>(&ODEPhaseBase::refineTrajManual),
+        ODEPhaseBase_refineTrajManual2);
+    obj.def("refineTrajEqual", &ODEPhaseBase::refineTrajEqual, ODEPhaseBase_refineTrajEqual);
+
+    obj.def("setStaticParams", py::overload_cast<VectorXd, VectorXd>(&ODEPhaseBase::setStaticParams), ODEPhaseBase_setStaticParams);
+    obj.def("setStaticParams", py::overload_cast<VectorXd>(&ODEPhaseBase::setStaticParams), ODEPhaseBase_setStaticParams);
+
+
+    obj.def("addStaticParams",
+        py::overload_cast<VectorXd, VectorXd>(&ODEPhaseBase::addStaticParams));
+    obj.def("addStaticParams",
+        py::overload_cast<VectorXd>(&ODEPhaseBase::addStaticParams));
+    obj.def("addStaticParamVgroups",
+        py::overload_cast<std::map<std::string, Eigen::VectorXi>>(&ODEPhaseBase::addStaticParamVgroups));
+    obj.def("setStaticParamVgroups",
+        py::overload_cast<std::map<std::string, Eigen::VectorXi>>(&ODEPhaseBase::setStaticParamVgroups));
+    obj.def("addStaticParamVgroup",
+        py::overload_cast<Eigen::VectorXi, std::string>(&ODEPhaseBase::addStaticParamVgroup));
+    obj.def("addStaticParamVgroup",
+        py::overload_cast<int, std::string>(&ODEPhaseBase::addStaticParamVgroup));
+
+
+    obj.def("setControlMode",
+        py::overload_cast<ControlModes>(&ODEPhaseBase::setControlMode),
+        ODEPhaseBase_setControlMode);
+    obj.def("setControlMode",
+        py::overload_cast<std::string>(&ODEPhaseBase::setControlMode),
+        ODEPhaseBase_setControlMode);
+
+    obj.def("setIntegralMode", &ODEPhaseBase::setIntegralMode, ODEPhaseBase_setIntegralMode);
+
+    obj.def("subStaticParams", &ODEPhaseBase::subStaticParams, ODEPhaseBase_subStaticParams);
+
+    obj.def("subVariables",
+        py::overload_cast<PhaseRegionFlags, VectorXi, VectorXd>(&ODEPhaseBase::subVariables),
+        ODEPhaseBase_subVariables);
+    obj.def("subVariable",
+        py::overload_cast<PhaseRegionFlags, int, double>(&ODEPhaseBase::subVariable),
+        ODEPhaseBase_subVariable);
+
+    obj.def("subVariables",
+        py::overload_cast<std::string, VectorXi, VectorXd>(&ODEPhaseBase::subVariables),
+        ODEPhaseBase_subVariables);
+    obj.def("subVariable",
+        py::overload_cast<std::string, int, double>(&ODEPhaseBase::subVariable),
+        ODEPhaseBase_subVariable);
+
+    obj.def("returnTraj", &ODEPhaseBase::returnTraj, ODEPhaseBase_returnTraj);
+    obj.def("returnTrajRange", &ODEPhaseBase::returnTrajRange, ODEPhaseBase_returnTrajRange);
+    obj.def("returnTrajRangeND", &ODEPhaseBase::returnTrajRangeND, ODEPhaseBase_returnTrajRangeND);
+    obj.def("returnTrajTable", &ODEPhaseBase::returnTrajTable);
+
+    obj.def("returnCostateTraj", &ODEPhaseBase::returnCostateTraj, ODEPhaseBase_returnCostateTraj);
+    obj.def("returnTrajError", &ODEPhaseBase::returnTrajError);
+
+    obj.def("returnUSplineConLmults", &ODEPhaseBase::returnUSplineConLmults);
+    obj.def("returnUSplineConVals", &ODEPhaseBase::returnUSplineConVals);
+
+
+    obj.def("returnEqualConLmults", &ODEPhaseBase::returnEqualConLmults, ODEPhaseBase_returnEqualConLmults);
+    obj.def("returnEqualConVals", &ODEPhaseBase::returnEqualConVals);
+    obj.def("returnEqualConScales", &ODEPhaseBase::returnEqualConScales);
+
+    obj.def(
+        "returnInequalConLmults", &ODEPhaseBase::returnInequalConLmults, ODEPhaseBase_returnInequalConLmults);
+    obj.def("returnInequalConVals", &ODEPhaseBase::returnInequalConVals);
+    obj.def("returnInequalConScales", &ODEPhaseBase::returnInequalConScales);
+
+    obj.def("returnIntegralObjectiveScales", &ODEPhaseBase::returnIntegralObjectiveScales);
+    obj.def("returnIntegralParamFunctionScales", &ODEPhaseBase::returnIntegralParamFunctionScales);
+    obj.def("returnStateObjectiveScales", &ODEPhaseBase::returnStateObjectiveScales);
+    obj.def("returnODEOutputScales", &ODEPhaseBase::returnODEOutputScales);
+
+
+    obj.def("returnStaticParams", &ODEPhaseBase::returnStaticParams, ODEPhaseBase_returnStaticParam);
+
+
+
+
+    obj.def("test_threads", &ODEPhaseBase::test_threads);
+
+    obj.def("removeEqualCon", &ODEPhaseBase::removeEqualCon, ODEPhaseBase_removeEqualCon);
+    obj.def("removeInequalCon", &ODEPhaseBase::removeInequalCon, ODEPhaseBase_removeInequalCon);
+    obj.def("removeStateObjective", &ODEPhaseBase::removeStateObjective, ODEPhaseBase_removeStateObjective);
+    obj.def("removeIntegralObjective",
+        &ODEPhaseBase::removeIntegralObjective,
+        ODEPhaseBase_removeIntegralObjective);
+    obj.def("removeIntegralParamFunction",
+        &ODEPhaseBase::removeIntegralParamFunction,
+        ODEPhaseBase_removeIntegralParamFunction);
+    ////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+    /////////////////////////////////////////////
+    ///// The New interface /////////////////////
+
+    obj.def("addEqualCon",
+        py::overload_cast<RegionType,
+        VectorFunctionalX,
+        VarIndexType,
+        VarIndexType,
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addEqualCon),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("XtUVars"),
+        py::arg("OPVars"),
+        py::arg("SPVars"),
+        py::arg("AutoScale") = std::string("auto"));
+
+    obj.def("addEqualCon",
+        py::overload_cast<RegionType,
+        VectorFunctionalX,
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addEqualCon),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("InputIndex"),
+        py::arg("AutoScale") = std::string("auto"));
+
+    obj.def("addBoundaryValue",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        const std::variant<double, VectorXd>&,
+        ScaleType>(&ODEPhaseBase::addBoundaryValue),
+        py::arg("PhaseRegion"),
+        py::arg("Index"),
+        py::arg("Value"),
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addDeltaVarEqualCon",
+        py::overload_cast<
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addDeltaVarEqualCon),
+        py::arg("var"),
+        py::arg("value"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addDeltaTimeEqualCon",
+        py::overload_cast<
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addDeltaTimeEqualCon),
+        py::arg("value"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addValueLock",
+        py::overload_cast<
+        RegionType,
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addValueLock),
+        py::arg("reg"),
+        py::arg("vars"),
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addPeriodicityCon",
+        py::overload_cast<
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addPeriodicityCon),
+        py::arg("vars"),
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+
+    //////////////////////////////////
+    /////// InequalCons
+    obj.def("addInequalCon",
+        py::overload_cast<RegionType,
+        VectorFunctionalX,
+        VarIndexType,
+        VarIndexType,
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addInequalCon),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("XtUVars"),
+        py::arg("OPVars"),
+        py::arg("SPVars"),
+        py::arg("AutoScale") = std::string("auto"));
+
+    obj.def("addInequalCon",
+        py::overload_cast<RegionType,
+        VectorFunctionalX,
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addInequalCon),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("InputIndex"),
+        py::arg("AutoScale") = std::string("auto"));
+
+
+
+    obj.def("addLUVarBound",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        double,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLUVarBound),
+        py::arg("PhaseRegion"),
+        py::arg("var"),
+        py::arg("lowerbound"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+    obj.def("addLowerVarBound",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLowerVarBound),
+        py::arg("PhaseRegion"),
+        py::arg("var"),
+        py::arg("lowerbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addUpperVarBound",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addUpperVarBound),
+        py::arg("PhaseRegion"),
+        py::arg("var"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+
+    obj.def("addLUFuncBound",
+        py::overload_cast<RegionType,
+        ScalarFunctionalX,
+        VarIndexType,
+        VarIndexType,
+        VarIndexType,
+        double,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLUFuncBound),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("XtUVars"),
+        py::arg("OPVars"),
+        py::arg("SPVars"),
+        py::arg("lowerbound"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addLUFuncBound",
+        py::overload_cast<RegionType,
+        ScalarFunctionalX,
+        VarIndexType,
+        double,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLUFuncBound),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("XtUPVars"),
+        py::arg("lowerbound"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+    //
+    obj.def("addLowerFuncBound",
+        py::overload_cast<RegionType,
+        ScalarFunctionalX,
+        VarIndexType,
+        VarIndexType,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLowerFuncBound),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("XtUVars"),
+        py::arg("OPVars"),
+        py::arg("SPVars"),
+        py::arg("lowerbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addLowerFuncBound",
+        py::overload_cast<RegionType,
+        ScalarFunctionalX,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLowerFuncBound),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("XtUPVars"),
+        py::arg("lowerbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addUpperFuncBound",
+        py::overload_cast<RegionType,
+        ScalarFunctionalX,
+        VarIndexType,
+        VarIndexType,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addUpperFuncBound),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("XtUVars"),
+        py::arg("OPVars"),
+        py::arg("SPVars"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addUpperFuncBound",
+        py::overload_cast<RegionType,
+        ScalarFunctionalX,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addUpperFuncBound),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("XtUPVars"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addLUNormBound",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        double,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLUNormBound),
+        py::arg("PhaseRegion"),
+        py::arg("XtUPVars"),
+        py::arg("lowerbound"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addLUSquaredNormBound",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        double,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLUSquaredNormBound),
+        py::arg("PhaseRegion"),
+        py::arg("XtUPVars"),
+        py::arg("lowerbound"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    //
+    obj.def("addLowerNormBound",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLowerNormBound),
+        py::arg("PhaseRegion"),
+        py::arg("XtUPVars"),
+        py::arg("lowerbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addLowerSquaredNormBound",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLowerSquaredNormBound),
+        py::arg("PhaseRegion"),
+        py::arg("XtUPVars"),
+        py::arg("lowerbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+    //
+    obj.def("addUpperNormBound",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addUpperNormBound),
+        py::arg("PhaseRegion"),
+        py::arg("XtUPVars"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+
+    obj.def("addUpperSquaredNormBound",
+        py::overload_cast<RegionType,
+        VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addUpperSquaredNormBound),
+        py::arg("PhaseRegion"),
+        py::arg("XtUPVars"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+    //
+    obj.def("addLowerDeltaVarBound",
+        py::overload_cast<VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLowerDeltaVarBound),
+        py::arg("Var"),
+        py::arg("lowerbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+    obj.def("addLowerDeltaTimeBound",
+        py::overload_cast<
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addLowerDeltaTimeBound),
+        py::arg("lowerbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+    //
+    obj.def("addUpperDeltaVarBound",
+        py::overload_cast<VarIndexType,
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addUpperDeltaVarBound),
+        py::arg("Var"),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+    obj.def("addUpperDeltaTimeBound",
+        py::overload_cast<
+        double,
+        double,
+        ScaleType>(&ODEPhaseBase::addUpperDeltaTimeBound),
+        py::arg("upperbound"),
+        py::arg("scale") = 1.0,
+        py::arg("AutoScale") = std::string("auto")
+    );
+    //////////////////////////////////
+    /////// StateObjectives /////////
+    obj.def("addStateObjective",
+        py::overload_cast<RegionType,
+        ScalarFunctionalX,
+        VarIndexType,
+        VarIndexType,
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addStateObjective),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("XtUVars"),
+        py::arg("OPVars"),
+        py::arg("SPVars"),
+        py::arg("AutoScale") = std::string("auto"));
+
+    obj.def("addStateObjective",
+        py::overload_cast<RegionType,
+        ScalarFunctionalX,
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addStateObjective),
+        py::arg("PhaseRegion"),
+        py::arg("Func"),
+        py::arg("InputIndex"),
+        py::arg("AutoScale") = std::string("auto"));
+
+
+    obj.def("addValueObjective",
+        py::overload_cast<
+        RegionType,
+        VarIndexType,
+        double,
+        ScaleType>(&ODEPhaseBase::addValueObjective),
+        py::arg("PhaseRegion"),
+        py::arg("Var"),
+        py::arg("scale"),
+        py::arg("AutoScale") = std::string("auto"));
+
+    obj.def("addDeltaVarObjective",
+        py::overload_cast<
+        VarIndexType,
+        double,
+        ScaleType>(&ODEPhaseBase::addDeltaVarObjective),
+        py::arg("Var"),
+        py::arg("scale"),
+        py::arg("AutoScale") = std::string("auto"));
+    obj.def("addDeltaTimeObjective",
+        py::overload_cast<
+        double,
+        ScaleType>(&ODEPhaseBase::addDeltaTimeObjective),
+        py::arg("Var"),
+        py::arg("AutoScale") = std::string("auto"));
+    //////////////////////////////////
+    /////// IntegralObjectives /////////
+    obj.def("addIntegralObjective",
+        py::overload_cast<
+        ScalarFunctionalX,
+        VarIndexType,
+        VarIndexType,
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addIntegralObjective),
+        py::arg("Func"),
+        py::arg("XtUVars"),
+        py::arg("OPVars"),
+        py::arg("SPVars"),
+        py::arg("AutoScale") = std::string("auto"));
+
+    obj.def("addIntegralObjective",
+        py::overload_cast<
+        ScalarFunctionalX,
+        VarIndexType,
+        ScaleType>(&ODEPhaseBase::addIntegralObjective),
+        py::arg("Func"),
+        py::arg("InputIndex"),
+        py::arg("AutoScale") = std::string("auto"));
+    //////////////////////////////////
+    /////// IntegralParamFunction /////////
+    obj.def("addIntegralParamFunction",
+        py::overload_cast<
+        ScalarFunctionalX,
+        VarIndexType,
+        VarIndexType,
+        VarIndexType,
+        int,
+        ScaleType>(&ODEPhaseBase::addIntegralParamFunction),
+        py::arg("Func"),
+        py::arg("XtUVars"),
+        py::arg("OPVars"),
+        py::arg("SPVars"),
+        py::arg("IntParam"),
+        py::arg("AutoScale") = std::string("auto"));
+
+    obj.def("addIntegralParamFunction",
+        py::overload_cast<
+        ScalarFunctionalX,
+        VarIndexType,
+        int,
+        ScaleType>(&ODEPhaseBase::addIntegralParamFunction),
+        py::arg("Func"),
+        py::arg("InputIndex"),
+        py::arg("IntParam"),
+        py::arg("AutoScale") = std::string("auto"));
+
+
+    ///////////////////////////////////////////////////////////////////
+
+
+    obj.def("addEqualCon",
+        py::overload_cast<StateConstraint>(&ODEPhaseBase::addEqualCon),
+        ODEPhaseBase_addEqualCon1);
+
+
+    ///////////////////////////////////////////////////////////////////////////////
+
+    obj.def("addInequalCon",
+        py::overload_cast<StateConstraint>(&ODEPhaseBase::addInequalCon),
+        ODEPhaseBase_addInequalCon1);
+    ////////////////////////////////////////////////////////////////////////////
+    obj.def("addLUVarBounds",
+        py::overload_cast<PhaseRegionFlags, Eigen::VectorXi, double, double, double>(
+            &ODEPhaseBase::addLUVarBounds),
+        ODEPhaseBase_addLUVarBounds);
+    obj.def("addLUVarBounds",
+        py::overload_cast<std::string, Eigen::VectorXi, double, double, double>(
+            &ODEPhaseBase::addLUVarBounds),
+        ODEPhaseBase_addLUVarBounds);
+
+    ////////////////////////////////////////////////////////////////////////////
+    obj.def("addStateObjective",
+        py::overload_cast<StateObjective>(&ODEPhaseBase::addStateObjective),
+        ODEPhaseBase_addStateObjective);
+
+    ////////////////////////////////////////////////////////////////////////////
+
+    obj.def("addIntegralObjective",
+        py::overload_cast<StateObjective>(&ODEPhaseBase::addIntegralObjective),
+        ODEPhaseBase_addIntegralObjective1);
+
+    ///////////////////////////////////////////////////////////////////////////////
+    obj.def("addIntegralParamFunction",
+        py::overload_cast<StateObjective, int>(&ODEPhaseBase::addIntegralParamFunction),
+        ODEPhaseBase_addIntegralParamFunction1);
+
+
+
+    ////////////////////////////////////////////////////
+    obj.def("getMeshInfo", &ODEPhaseBase::getMeshInfo);
+    obj.def("refineTrajAuto", &ODEPhaseBase::refineTrajAuto);
+    obj.def("calc_global_error", &ODEPhaseBase::calc_global_error);
+    obj.def("getMeshIters", &ODEPhaseBase::getMeshIters);
+
+
+    obj.def_readwrite("AdaptiveMesh", &ODEPhaseBase::AdaptiveMesh);
+    obj.def_readwrite("AutoScaling", &ODEPhaseBase::AutoScaling);
+    obj.def_readwrite("SyncObjectiveScales", &ODEPhaseBase::SyncObjectiveScales);
+
+
+
+    obj.def("setAutoScaling", &ODEPhaseBase::setAutoScaling, py::arg("AutoScaling") = true);
+
+    obj.def("setAdaptiveMesh", &ODEPhaseBase::setAdaptiveMesh, py::arg("AdaptiveMesh") = true);
+
+
+    obj.def("setUnits", py::overload_cast<const py::kwargs&>(&ODEPhaseBase::setUnits));
+    obj.def("setUnits", py::overload_cast<const Eigen::VectorXd&>(&ODEPhaseBase::setUnits));
+
+
+    obj.def("setMeshTol", &ODEPhaseBase::setMeshTol);
+    obj.def("setMeshRedFactor", &ODEPhaseBase::setMeshRedFactor);
+    obj.def("setMeshIncFactor", &ODEPhaseBase::setMeshIncFactor);
+    obj.def("setMeshErrFactor", &ODEPhaseBase::setMeshErrFactor);
+    obj.def("setMaxMeshIters", &ODEPhaseBase::setMaxMeshIters);
+    obj.def("setMinSegments", &ODEPhaseBase::setMinSegments);
+    obj.def("setMaxSegments", &ODEPhaseBase::setMaxSegments);
+    obj.def("setMeshErrorCriteria", &ODEPhaseBase::setMeshErrorCriteria);
+    obj.def("setMeshErrorEstimator", &ODEPhaseBase::setMeshErrorEstimator);
+
+
+    obj.def_readwrite("PrintMeshInfo", &ODEPhaseBase::PrintMeshInfo);
+    obj.def_readwrite("MaxMeshIters", &ODEPhaseBase::MaxMeshIters);
+    obj.def_readwrite("MeshTol", &ODEPhaseBase::MeshTol);
+    obj.def_readwrite("MeshErrorEstimator", &ODEPhaseBase::MeshErrorEstimator);
+    obj.def_readwrite("MeshErrorCriteria", &ODEPhaseBase::MeshErrorCriteria);
+
+
+    obj.def_readwrite("SolveOnlyFirst", &ODEPhaseBase::SolveOnlyFirst);
+    obj.def_readwrite("ForceOneMeshIter", &ODEPhaseBase::ForceOneMeshIter);
+    obj.def_readwrite("NewError", &ODEPhaseBase::NewError);
+
+
+    obj.def_readwrite("DetectControlSwitches", &ODEPhaseBase::DetectControlSwitches);
+    obj.def_readwrite("RelSwitchTol", &ODEPhaseBase::RelSwitchTol);
+    obj.def_readwrite("AbsSwitchTol", &ODEPhaseBase::AbsSwitchTol);
+    obj.def_readwrite("MeshAbortFlag", &ODEPhaseBase::MeshAbortFlag);
+
+
+    obj.def_readwrite("NumExtraSegs", &ODEPhaseBase::NumExtraSegs);
+    obj.def_readwrite("MeshRedFactor", &ODEPhaseBase::MeshRedFactor);
+    obj.def_readwrite("MeshIncFactor", &ODEPhaseBase::MeshIncFactor);
+    obj.def_readwrite("MeshErrFactor", &ODEPhaseBase::MeshErrFactor);
+    obj.def_readonly("MeshConverged", &ODEPhaseBase::MeshConverged);
+}
diff --git a/src/OptimalControl/ODESizes.h b/src/OptimalControl/ODESizes.h
index 61e0af8f..6d6a78e7 100644
--- a/src/OptimalControl/ODESizes.h
+++ b/src/OptimalControl/ODESizes.h
@@ -122,29 +122,80 @@ namespace ASSET {
   struct ODESize : ODEXUPVSizes<_XV, _UV, _PV> {
 
 
-    std::vector<int> Xidxs() const {
-      std::vector<int> idxs(this->XVars());
+    std::map<std::string, Eigen::VectorXi> _XtUPidxs;
+
+    void add_default_idxs() {
+       for (int i = 0; i < this->XVars(); i++) {
+           this->add_idx(std::to_string(i), this->Xidxs(i));
+       }
+       this->add_idx("t", this->XVars());
+       for (int i = 0; i < this->UVars(); i++) {
+           this->add_idx(std::to_string(i), this->Uidxs(i));
+       }
+       for (int i = 0; i < this->PVars(); i++) {
+           this->add_idx(std::to_string(i), this->Pidxs(i));
+       }
+    }
+
+    void add_idx(const std::string& name, const Eigen::VectorXi & idx) {
+       if (_XtUPidxs.count(name)) {
+        throw std::invalid_argument(fmt::format("Variable index group with name: {0:} already exists.", name));
+        }
+        if (idx.size() == 0) {
+        throw std::invalid_argument(fmt::format("Variable index group with name: {0:} has no elements.", name));
+        }
+        _XtUPidxs[name] = idx;
+    }
+
+    void add_idx(const std::string& name, int indx) {
+        Eigen::VectorXi idxv(1);
+        idxv[0] = indx;
+        this->add_idx(idxv);
+    }
+
+    Eigen::VectorXi idx(const std::string& name) const {
+        if (_XtUPidxs.count(name) == 0) {
+        throw std::invalid_argument(
+            fmt::format("No variable index group with name: {0:} exists.", name));
+        }
+        return _XtUPidxs.at(name);
+    }
+
+    void set_idxs(const std::map<std::string, Eigen::VectorXi>& idxs) {
+        this->_XtUPidxs = idxs;
+    }
+    std::map<std::string, Eigen::VectorXi> get_idxs() const {
+        return this->_XtUPidxs;
+    }
+
+    Eigen::VectorXi Xidxs() const {
+      Eigen::VectorXi idxs(this->XVars());
       std::iota(idxs.begin(), idxs.end(), 0);
       return idxs;
     }
-    std::vector<int> Xtidxs() const {
-      std::vector<int> idxs(this->XtVars());
+    Eigen::VectorXi Xtidxs() const {
+      Eigen::VectorXi idxs(this->XtVars());
       std::iota(idxs.begin(), idxs.end(), 0);
       return idxs;
     }
-    std::vector<int> XtUidxs() const {
-      std::vector<int> idxs(this->XtUVars());
+    Eigen::VectorXi XtUidxs() const {
+      Eigen::VectorXi idxs(this->XtUVars());
       std::iota(idxs.begin(), idxs.end(), 0);
       return idxs;
     }
-    std::vector<int> Uidxs() const {
-      std::vector<int> idxs(this->UVars());
+    Eigen::VectorXi Uidxs() const {
+      Eigen::VectorXi idxs(this->UVars());
       std::iota(idxs.begin(), idxs.end(), this->XtVars());
       return idxs;
     }
+    Eigen::VectorXi Pidxs() const {
+      Eigen::VectorXi idxs(this->UVars());
+      std::iota(idxs.begin(), idxs.end(), this->XtUVars());
+      return idxs;
+    }
 
 
-    std::vector<int> idxs_impl(const std::vector<int>& zidxs, const std::vector<int>& idxs) const {
+    Eigen::VectorXi idxs_impl(const Eigen::VectorXi& zidxs, const Eigen::VectorXi& idxs) const {
 
       auto minelem = *std::min_element(zidxs.begin(), zidxs.end());
       auto maxelem = *std::max_element(zidxs.begin(), zidxs.end());
@@ -153,28 +204,55 @@ namespace ASSET {
         throw std::invalid_argument("Indexing error in ODESizes idxs");
       }
 
-      std::vector<int> nidxs(zidxs.size());
+      Eigen::VectorXi nidxs(zidxs.size());
 
       for (int i = 0; i < zidxs.size(); i++) {
         nidxs[i] = idxs[zidxs[i]];
       }
 
-
       return nidxs;
     }
 
-    std::vector<int> Xidxs(const std::vector<int>& zidxs) const {
+    Eigen::VectorXi idxs_impl(int zidx, const Eigen::VectorXi& idxs) const {
+      Eigen::VectorXi zidxs(1);
+      zidxs[0] = zidx;
+      return this->idxs_impl(zidxs, idxs);
+    }
+
+
+
+    Eigen::VectorXi Xidxs(const Eigen::VectorXi& zidxs) const {
       return idxs_impl(zidxs, this->Xidxs());
     }
-    std::vector<int> Xtidxs(const std::vector<int>& zidxs) const {
+    Eigen::VectorXi Xtidxs(const Eigen::VectorXi& zidxs) const {
       return idxs_impl(zidxs, this->Xtidxs());
     }
-    std::vector<int> XtUidxs(const std::vector<int>& zidxs) const {
+    Eigen::VectorXi XtUidxs(const Eigen::VectorXi& zidxs) const {
       return idxs_impl(zidxs, this->XtUidxs());
     }
-    std::vector<int> Uidxs(const std::vector<int>& zidxs) const {
+    Eigen::VectorXi Uidxs(const Eigen::VectorXi& zidxs) const {
       return idxs_impl(zidxs, this->Uidxs());
     }
+    Eigen::VectorXi Pidxs(const Eigen::VectorXi& zidxs) const {
+      return idxs_impl(zidxs, this->Pidxs());
+    }
+
+
+    Eigen::VectorXi Xidxs(int zidxs) const {
+      return idxs_impl(zidxs, this->Xidxs());
+    }
+    Eigen::VectorXi Xtidxs(int zidxs) const {
+      return idxs_impl(zidxs, this->Xtidxs());
+    }
+    Eigen::VectorXi XtUidxs(int zidxs) const {
+      return idxs_impl(zidxs, this->XtUidxs());
+    }
+    Eigen::VectorXi Uidxs(int zidxs) const {
+      return idxs_impl(zidxs, this->Uidxs());
+    }
+    Eigen::VectorXi Pidxs(int zidxs) const {
+      return idxs_impl(zidxs, this->Pidxs());
+    }
 
 
     template<class Obj, class Derived>
@@ -191,17 +269,26 @@ namespace ASSET {
 
 
       obj.def("Xidxs", py::overload_cast<>(&Derived::Xidxs, py::const_));
-      obj.def("Xidxs", py::overload_cast<const std::vector<int>&>(&Derived::Xidxs, py::const_));
+      obj.def("Xidxs", py::overload_cast<const Eigen::VectorXi&>(&Derived::Xidxs, py::const_));
 
       obj.def("Xtidxs", py::overload_cast<>(&Derived::Xtidxs, py::const_));
-      obj.def("Xtidxs", py::overload_cast<const std::vector<int>&>(&Derived::Xtidxs, py::const_));
+      obj.def("Xtidxs", py::overload_cast<const Eigen::VectorXi&>(&Derived::Xtidxs, py::const_));
 
 
       obj.def("XtUidxs", py::overload_cast<>(&Derived::XtUidxs, py::const_));
-      obj.def("XtUidxs", py::overload_cast<const std::vector<int>&>(&Derived::XtUidxs, py::const_));
+      obj.def("XtUidxs", py::overload_cast<const Eigen::VectorXi&>(&Derived::XtUidxs, py::const_));
 
       obj.def("Uidxs", py::overload_cast<>(&Derived::Uidxs, py::const_));
-      obj.def("Uidxs", py::overload_cast<const std::vector<int>&>(&Derived::Uidxs, py::const_));
+      obj.def("Uidxs", py::overload_cast<const Eigen::VectorXi&>(&Derived::Uidxs, py::const_));
+
+      obj.def("add_idx",
+              py::overload_cast<const std::string & , const Eigen::VectorXi&>(&Derived::add_idx));
+      obj.def("get_idxs", &Derived::get_idxs);
+      obj.def("set_idxs", &Derived::set_idxs);
+      obj.def("idx", &Derived::idx);
+
+
+
     }
   };
 
diff --git a/src/OptimalControl/OptimalControlProblem.cpp b/src/OptimalControl/OptimalControlProblem.cpp
index 2d21524b..25101c0f 100644
--- a/src/OptimalControl/OptimalControlProblem.cpp
+++ b/src/OptimalControl/OptimalControlProblem.cpp
@@ -1,7 +1,117 @@
 #include "OptimalControlProblem.h"
+#include "AutoScalingUtils.h"
 
 #include "PyDocString/OptimalControl/OptimalControlProblem_doc.h"
 
+Eigen::VectorXd ASSET::OptimalControlProblem::get_input_scale(LinkFlags lflag, 
+    Eigen::Vector<PhaseRegionFlags, -1> regs,
+    std::vector<VectorXi> phases_to_link, 
+    std::vector<VectorXi> XtUVars, 
+    std::vector<VectorXi> OPVars, 
+    std::vector<VectorXi> SPVars, 
+    std::vector<VectorXi> LVars)
+{
+    
+
+    std::vector<VectorXd> scales;
+    int size = 0;
+
+    if (phases_to_link.size() > 0) {
+
+        for (int i = 0; i < phases_to_link[0].size(); i++) {
+
+            int pnum = phases_to_link[0][i];
+            auto flag = regs[i];
+            auto XtUV = XtUVars[i];
+            auto OPV = OPVars[i];
+            auto SPV = SPVars[i];
+
+            VectorXd scale = this->phases[pnum]->get_input_scale(flag, XtUV, OPV, SPV);
+            scales.push_back(scale);
+            size += scale.size();
+        }
+    }
+
+    if (LVars.size() > 0) {
+
+        VectorXd lscales(LVars[0].size());
+        for (int i = 0; i < LVars[0].size(); i++) {
+            lscales[i] = this->LPUnits[LVars[0][i]];
+            size++;
+        }
+        scales.push_back(lscales);
+    }
+
+    VectorXd input_scales(size);
+
+    int start = 0;
+
+    for (int i = 0; i < scales.size(); i++) {
+        size = scales[i].size();
+        input_scales.segment(start, size) = scales[i];
+        start += size;
+    }
+
+
+
+    return input_scales;
+}
+
+std::vector<Eigen::VectorXd> ASSET::OptimalControlProblem::get_test_inputs(LinkFlags lflag, 
+    Eigen::Vector<PhaseRegionFlags, -1> regs, 
+    std::vector<VectorXi> phases_to_link,
+    std::vector<VectorXi> XtUVars, 
+    std::vector<VectorXi> OPVars, std::vector<VectorXi> SPVars, std::vector<VectorXi> LVars)
+{
+
+    int nappl = std::max(phases_to_link.size(), LVars.size());
+
+    std::vector<Eigen::VectorXd> test_inputs;
+
+
+    for (int j = 0; j < nappl; j++) {
+        int size = 0;
+
+        std::vector<VectorXd> inputs;
+
+
+        for (int i = 0; i < phases_to_link[j].size(); i++) {
+
+            int pnum = phases_to_link[j][i];
+            auto flag = regs[i];
+            auto XtUV = XtUVars[i];
+            auto OPV = OPVars[i];
+            auto SPV = SPVars[i];
+
+            VectorXd input = this->phases[pnum]->get_test_inputs(flag, XtUV, OPV, SPV)[0];
+            inputs.push_back(input);
+            size += input.size();
+        }
+
+        VectorXd linput(LVars[j].size());
+        for (int i = 0; i < LVars[j].size(); i++) {
+            linput[i] = this->ActiveLinkParams[LVars[j][i]];
+            size++;
+        }
+        inputs.push_back(linput);
+
+        VectorXd test_input(size);
+
+        int start = 0;
+
+        for (int i = 0; i < inputs.size(); i++) {
+            size = inputs[i].size();
+            test_input.segment(start, size) = inputs[i];
+            start += size;
+        }
+
+        test_inputs.push_back(test_input);
+    }
+
+
+    return test_inputs;
+}
+
 void ASSET::OptimalControlProblem::transcribe_phases() {
 
 
@@ -45,6 +155,18 @@ void ASSET::OptimalControlProblem::transcribe_phases() {
 }
 
 void ASSET::OptimalControlProblem::check_functions() {
+    /*
+    Loops through all user defined functions and checks that they do not
+    reference non-existent variables. Should be run prior to any transcribing any
+    problem functions. First checks each phase, then all link functions.
+    */
+
+
+   for (int i = 0; i < this->phases.size(); i++) {
+        this->phases[i]->check_functions(i);
+   }
+
+
   auto CheckFunc = [&](std::string type, auto& func) {
     for (int i = 0; i < func.PhasesTolink.size(); i++) {
       for (int j = 0; j < func.PhasesTolink[i].size(); j++) {
@@ -136,13 +258,14 @@ void ASSET::OptimalControlProblem::check_functions() {
   std::string iq = "Link Inequality constraint";
   std::string obj = "Link Objective";
 
-
   for (auto& [key,f]: this->LinkEqualities)
     CheckFunc(eq, f);
   for (auto& [key, f]: this->LinkInequalities)
     CheckFunc(iq, f);
   for (auto& [key, f]: this->LinkObjectives)
     CheckFunc(obj, f);
+
+
 }
 
 void ASSET::OptimalControlProblem::transcribe_links() {
@@ -173,7 +296,24 @@ void ASSET::OptimalControlProblem::transcribe_links() {
                                             Eq.LinkParams,
                                             Eq.Func.ORows(),
                                             NextEq);
-    this->nlp->EqualityConstraints.emplace_back(ConstraintFunction(Eq.Func, VC[0], VC[1]));
+
+
+    auto Func = Eq.Func;
+
+    if (this->AutoScaling) {
+        VectorXd input_scales = this->get_input_scale(Eq.LinkFlag,
+            Eq.PhaseRegFlags,
+            Eq.PhasesTolink,
+            Eq.XtUVars,
+            Eq.OPVars,
+            Eq.SPVars,
+            Eq.LinkParams);
+        VectorXd output_scales(Func.ORows());
+        output_scales = Eq.OutputScales;
+        Func = IOScaled<decltype(Func)>(Eq.Func, input_scales, output_scales);
+    }
+
+    this->nlp->EqualityConstraints.emplace_back(ConstraintFunction(Func, VC[0], VC[1]));
     Eq.GlobalIndex = this->nlp->EqualityConstraints.size() - 1;
     this->numEqFuns++;
   }
@@ -187,7 +327,24 @@ void ASSET::OptimalControlProblem::transcribe_links() {
                                             Iq.LinkParams,
                                             Iq.Func.ORows(),
                                             NextIq);
-    this->nlp->InequalityConstraints.emplace_back(ConstraintFunction(Iq.Func, VC[0], VC[1]));
+
+    auto Func = Iq.Func;
+
+    if (this->AutoScaling) {
+        VectorXd input_scales = this->get_input_scale(Iq.LinkFlag,
+            Iq.PhaseRegFlags,
+            Iq.PhasesTolink,
+            Iq.XtUVars,
+            Iq.OPVars,
+            Iq.SPVars,
+            Iq.LinkParams);
+        VectorXd output_scales(Func.ORows());
+        output_scales = Iq.OutputScales;
+        Func = IOScaled<decltype(Func)>(Iq.Func, input_scales, output_scales);
+    }
+
+
+    this->nlp->InequalityConstraints.emplace_back(ConstraintFunction(Func, VC[0], VC[1]));
     Iq.GlobalIndex = this->nlp->InequalityConstraints.size() - 1;
     this->numIqFuns++;
   }
@@ -202,7 +359,24 @@ void ASSET::OptimalControlProblem::transcribe_links() {
                                             Ob.LinkParams,
                                             Ob.Func.ORows(),
                                             dummy);
-    this->nlp->Objectives.emplace_back(ObjectiveFunction(Ob.Func, VC[0]));
+
+    auto Func = Ob.Func;
+
+    if (this->AutoScaling) {
+        VectorXd input_scales = this->get_input_scale(Ob.LinkFlag,
+            Ob.PhaseRegFlags,
+            Ob.PhasesTolink,
+            Ob.XtUVars,
+            Ob.OPVars,
+            Ob.SPVars,
+            Ob.LinkParams);
+        VectorXd output_scales(Func.ORows());
+        output_scales = Ob.OutputScales;
+        Func = IOScaled<decltype(Func)>(Ob.Func, input_scales, output_scales);
+    }
+
+
+    this->nlp->Objectives.emplace_back(ObjectiveFunction(Func, VC[0]));
     Ob.GlobalIndex = this->nlp->Objectives.size() - 1;
 
     this->numObjFuns++;
@@ -212,9 +386,96 @@ void ASSET::OptimalControlProblem::transcribe_links() {
   this->numLinkIqCons = NextIq - this->numPhaseIqCons.sum();
 }
 
+void ASSET::OptimalControlProblem::calc_auto_scales()
+{
+    for (int i = 0; i < this->phases.size(); i++) {
+        this->phases[i]->calc_auto_scales();
+    }
+
+    auto calc_impl = [&](auto& funcmap) {
+        for (auto& [key, func] : funcmap) {
+            if (func.ScaleMode == "auto") {
+                VectorXd input_scales = this->get_input_scale(func.LinkFlag,
+                    func.PhaseRegFlags,
+                    func.PhasesTolink,
+                    func.XtUVars,
+                    func.OPVars,
+                    func.SPVars,
+                    func.LinkParams);
+                std::vector<VectorXd> test_inputs = this->get_test_inputs(func.LinkFlag,
+                    func.PhaseRegFlags,
+                    func.PhasesTolink,
+                    func.XtUVars,
+                    func.OPVars,
+                    func.SPVars,
+                    func.LinkParams);
+                VectorXd output_scales = calc_jacobian_row_scales(func.Func, input_scales, test_inputs, "norm", "mean");
+                func.OutputScales = output_scales;
+            }
+            else {
+
+
+            }
+
+        }
+    };
+
+
+    calc_impl(this->LinkEqualities);
+    calc_impl(this->LinkInequalities);
+    calc_impl(this->LinkObjectives);
+
+}
+
+std::vector<double> ASSET::OptimalControlProblem::get_objective_scales()
+{
+    std::vector<double> scales;
+    for (auto& [key, obj] : this->LinkObjectives) {
+        if (obj.ScaleMode == "auto") {
+            scales.push_back(obj.OutputScales[0]);
+        }
+    }
+    for (auto phase : this->phases) {
+        auto pscales = phase->get_objective_scales();
+
+        for (auto s : pscales) { 
+            scales.push_back(s); 
+        }
+        
+    }
+    return scales;
+}
+
+void ASSET::OptimalControlProblem::update_objective_scales(double scale)
+{
+    for (auto& [key, obj] : this->LinkObjectives) {
+        if (obj.ScaleMode == "auto") {
+            obj.OutputScales[0] = scale;
+        }
+    }
+    for (auto phase : this->phases) {
+        phase->update_objective_scales(scale);
+        
+    }
+}
+
 void ASSET::OptimalControlProblem::transcribe(bool showstats, bool showfuns) {
+
   this->nlp = std::make_shared<NonLinearProgram>(this->Threads);
+
   check_functions();
+  if (this->AutoScaling) {
+      this->calc_auto_scales();
+      if (this->SyncObjectiveScales) {
+          // Ensure that all objectives have same scale factor to preserve meaning of un-scaled problem
+          // Common scale is mean of all separate scale factors
+          auto objscales = this->get_objective_scales();
+          if (objscales.size() > 0) {
+              double meanobjscale = std::accumulate(objscales.begin(), objscales.end(), 0.0) / double(objscales.size());
+              this->update_objective_scales(meanobjscale);
+          }
+      }
+  }
 
   this->transcribe_phases();
   this->transcribe_links();
@@ -226,6 +487,8 @@ void ASSET::OptimalControlProblem::transcribe(bool showstats, bool showfuns) {
     this->print_stats(showfuns);
   this->nlp->make_NLP(this->numProbVars, this->numProbEqCons, this->numProbIqCons);
   this->optimizer->setNLP(this->nlp);
+
+  //////DO NOT GET RID OF THIS!!!!!!//
   this->doTranscription = false;
 }
 
@@ -500,844 +763,3 @@ std::array<Eigen::MatrixXi, 2> ASSET::OptimalControlProblem::make_link_Vindex_Ci
   return std::array<Eigen::MatrixXi, 2> {Vindex, Cindex};
 }
 
-void ASSET::OptimalControlProblem::Build(py::module& m) {
-  using namespace doc;
-  auto obj =
-      py::class_<OptimalControlProblem, std::shared_ptr<OptimalControlProblem>, OptimizationProblemBase>(
-          m, "OptimalControlProblem");
-  obj.def(py::init<>());
-
-  //////////////////
-  obj.def("addLinkEqualCon",
-          py::overload_cast<LinkConstraint>(&OptimalControlProblem::addLinkEqualCon),
-          OptimalControlProblem_addLinkEqualCon1);
-
-  ////////////////////////////
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            RegVec,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            std::vector<std::string>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            LinkFlags,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            std::string,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
-
-  /// ///
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            RegVec,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            std::vector<std::string>,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            LinkFlags,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            std::string,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
-
-  ////////////////////////////
-
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            RegVec,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon),
-          OptimalControlProblem_addLinkEqualCon2);
-
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            RegVec,
-                            std::vector<std::vector<std::shared_ptr<ODEPhaseBase>>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon),
-          OptimalControlProblem_addLinkEqualCon2);
-
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX, LinkFlags, std::vector<VectorXi>, VectorXi>(
-              &OptimalControlProblem::addLinkEqualCon),
-          OptimalControlProblem_addLinkEqualCon2);
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX, LinkFlags, std::vector<std::vector<PhasePtr>>, VectorXi>(
-              &OptimalControlProblem::addLinkEqualCon),
-          OptimalControlProblem_addLinkEqualCon2);
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX, std::string, std::vector<VectorXi>, VectorXi>(
-              &OptimalControlProblem::addLinkEqualCon),
-          OptimalControlProblem_addLinkEqualCon2);
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX, std::string, std::vector<std::vector<PhasePtr>>, VectorXi>(
-              &OptimalControlProblem::addLinkEqualCon),
-          OptimalControlProblem_addLinkEqualCon2);
-
-
-  obj.def("addForwardLinkEqualCon",
-          py::overload_cast<int, int, VectorXi>(&OptimalControlProblem::addForwardLinkEqualCon),
-          OptimalControlProblem_addForwardLinkEqualCon);
-
-
-  obj.def("addForwardLinkEqualCon",
-          py::overload_cast<PhasePtr, PhasePtr, VectorXi>(&OptimalControlProblem::addForwardLinkEqualCon),
-          OptimalControlProblem_addForwardLinkEqualCon);
-
-
-  obj.def("addDirectLinkEqualCon",
-          py::overload_cast<LinkFlags, int, VectorXi, int, VectorXi>(
-              &OptimalControlProblem::addDirectLinkEqualCon),
-          OptimalControlProblem_addDirectLinkEqualCon);
-
-  obj.def(
-      "addDirectLinkEqualCon",
-      py::overload_cast<VectorFunctionalX, int, PhaseRegionFlags, VectorXi, int, PhaseRegionFlags, VectorXi>(
-          &OptimalControlProblem::addDirectLinkEqualCon),
-      OptimalControlProblem_addDirectLinkEqualCon);
-  obj.def("addDirectLinkEqualCon",
-          py::overload_cast<int, PhaseRegionFlags, VectorXi, int, PhaseRegionFlags, VectorXi>(
-              &OptimalControlProblem::addDirectLinkEqualCon),
-          OptimalControlProblem_addDirectLinkEqualCon);
-
-  obj.def("addDirectLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            PhasePtr,
-                            PhaseRegionFlags,
-                            VectorXi,
-                            PhasePtr,
-                            PhaseRegionFlags,
-                            VectorXi>(&OptimalControlProblem::addDirectLinkEqualCon),
-          OptimalControlProblem_addDirectLinkEqualCon);
-
-  obj.def("addDirectLinkEqualCon",
-          py::overload_cast<PhasePtr, PhaseRegionFlags, VectorXi, PhasePtr, PhaseRegionFlags, VectorXi>(
-              &OptimalControlProblem::addDirectLinkEqualCon),
-          OptimalControlProblem_addDirectLinkEqualCon);
-
-  //
-  obj.def("addDirectLinkEqualCon",
-          py::overload_cast<VectorFunctionalX, int, std::string, VectorXi, int, std::string, VectorXi>(
-              &OptimalControlProblem::addDirectLinkEqualCon),
-          OptimalControlProblem_addDirectLinkEqualCon);
-  obj.def("addDirectLinkEqualCon",
-          py::overload_cast<int, std::string, VectorXi, int, std::string, VectorXi>(
-              &OptimalControlProblem::addDirectLinkEqualCon),
-          OptimalControlProblem_addDirectLinkEqualCon);
-
-  obj.def(
-      "addDirectLinkEqualCon",
-      py::overload_cast<VectorFunctionalX, PhasePtr, std::string, VectorXi, PhasePtr, std::string, VectorXi>(
-          &OptimalControlProblem::addDirectLinkEqualCon),
-      OptimalControlProblem_addDirectLinkEqualCon);
-
-  obj.def("addDirectLinkEqualCon",
-          py::overload_cast<PhasePtr, std::string, VectorXi, PhasePtr, std::string, VectorXi>(
-              &OptimalControlProblem::addDirectLinkEqualCon),
-          OptimalControlProblem_addDirectLinkEqualCon);
-
-
-  //////////////////////////////////////////////////
-  //////////////////////////////////////////////////
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<LinkConstraint>(&OptimalControlProblem::addLinkInequalCon),
-          OptimalControlProblem_addLinkInequalCon);
-
-  ////////////////////////////
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            RegVec,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            std::vector<std::string>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            LinkFlags,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            std::string,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-
-  /// ///
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            RegVec,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            std::vector<std::string>,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            LinkFlags,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            std::string,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-
-  ////////////////////////////
-
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX, LinkFlags, std::vector<VectorXi>, VectorXi>(
-              &OptimalControlProblem::addLinkInequalCon),
-          OptimalControlProblem_addLinkInequalCon);
-
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX, LinkFlags, std::vector<std::vector<PhasePtr>>, VectorXi>(
-              &OptimalControlProblem::addLinkInequalCon),
-          OptimalControlProblem_addLinkInequalCon);
-
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX, std::string, std::vector<VectorXi>, VectorXi>(
-              &OptimalControlProblem::addLinkInequalCon),
-          OptimalControlProblem_addLinkInequalCon);
-
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX, std::string, std::vector<std::vector<PhasePtr>>, VectorXi>(
-              &OptimalControlProblem::addLinkInequalCon),
-          OptimalControlProblem_addLinkInequalCon);
-
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            RegVec,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            RegVec,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
-
-
-  ////////////////////////////////////////
-  obj.def("addLinkObjective",
-          py::overload_cast<LinkObjective>(&OptimalControlProblem::addLinkObjective),
-          OptimalControlProblem_addLinkObjective);
-
-
-  ////////////////////////////
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            RegVec,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            std::vector<std::string>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            LinkFlags,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            std::string,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
-
-  /// ///
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            RegVec,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            std::vector<std::string>,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            LinkFlags,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            std::string,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
-
-  ////////////////////////////
-
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX, LinkFlags, std::vector<VectorXi>, VectorXi>(
-              &OptimalControlProblem::addLinkObjective),
-          OptimalControlProblem_addLinkObjective);
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX, LinkFlags, std::vector<std::vector<PhasePtr>>, VectorXi>(
-              &OptimalControlProblem::addLinkObjective),
-          OptimalControlProblem_addLinkObjective);
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX, std::string, std::vector<VectorXi>, VectorXi>(
-              &OptimalControlProblem::addLinkObjective),
-          OptimalControlProblem_addLinkObjective);
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX, std::string, std::vector<std::vector<PhasePtr>>, VectorXi>(
-              &OptimalControlProblem::addLinkObjective),
-          OptimalControlProblem_addLinkObjective);
-
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            RegVec,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective),
-          OptimalControlProblem_addLinkObjective);
-
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            RegVec,
-                            std::vector<std::vector<PhasePtr>>,
-                            std::vector<VectorXi>,
-                            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective),
-          OptimalControlProblem_addLinkObjective);
-
-
-  //////////////////////////////////////////////////////////////////////////////
-
-  obj.def("addLinkParamEqualCon",
-          py::overload_cast<VectorFunctionalX, std::vector<VectorXi>>(
-              &OptimalControlProblem::addLinkParamEqualCon),
-          OptimalControlProblem_addLinkParamEqualCon1);
-  obj.def("addLinkParamEqualCon",
-          py::overload_cast<VectorFunctionalX, VectorXi>(&OptimalControlProblem::addLinkParamEqualCon),
-          OptimalControlProblem_addLinkParamEqualCon2);
-  obj.def("addLinkParamInequalCon",
-          py::overload_cast<VectorFunctionalX, std::vector<VectorXi>>(
-              &OptimalControlProblem::addLinkParamInequalCon),
-          OptimalControlProblem_addLinkParamInequalCon1);
-  obj.def("addLinkParamInequalCon",
-          py::overload_cast<VectorFunctionalX, VectorXi>(&OptimalControlProblem::addLinkParamInequalCon),
-          OptimalControlProblem_addLinkParamInequalCon2);
-  obj.def("addLinkParamObjective",
-          py::overload_cast<ScalarFunctionalX, std::vector<VectorXi>>(
-              &OptimalControlProblem::addLinkParamObjective),
-          OptimalControlProblem_addLinkParamObjective1);
-  obj.def("addLinkParamObjective",
-          py::overload_cast<ScalarFunctionalX, VectorXi>(&OptimalControlProblem::addLinkParamObjective),
-          OptimalControlProblem_addLinkParamObjective2);
-
-  //////////////////////////////////////////////////////////////////////////////
-
-  obj.def("removeLinkEqualCon",
-          &OptimalControlProblem::removeLinkEqualCon,
-          OptimalControlProblem_removeLinkEqualCon);
-  obj.def("removeLinkInequalCon",
-          &OptimalControlProblem::removeLinkInequalCon,
-          OptimalControlProblem_removeLinkEqualCon);
-  obj.def("removeLinkObjective",
-          &OptimalControlProblem::removeLinkObjective,
-          OptimalControlProblem_removeLinkObjective);
-
-  obj.def("addPhase",
-          py::overload_cast<PhasePtr>(&OptimalControlProblem::addPhase),
-          OptimalControlProblem_addPhase);
-
-
-  obj.def("addPhases", &OptimalControlProblem::addPhases);
-
-  obj.def("getPhaseNum", py::overload_cast<PhasePtr>(&OptimalControlProblem::getPhaseNum));
-
-
-  obj.def("removePhase", &OptimalControlProblem::removePhase, OptimalControlProblem_removePhase);
-  obj.def("Phase", &OptimalControlProblem::Phase, OptimalControlProblem_Phase);
-  obj.def("setLinkParams", &OptimalControlProblem::setLinkParams, OptimalControlProblem_setLinkParams);
-  obj.def(
-      "returnLinkParams", &OptimalControlProblem::returnLinkParams, OptimalControlProblem_returnLinkParams);
-
-
-  obj.def("transcribe",
-          py::overload_cast<bool, bool>(&OptimalControlProblem::transcribe),
-          OptimalControlProblem_transcribe);
-
-  obj.def_readonly("Phases", &OptimalControlProblem::phases, OptimalControlProblem_Phases);
-
-
-  ///////////////////////////////////////////////////////////////
-  /////////////// New Link Interface/////////////////////////////
-
-  ///
-  /// EqualCons
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX, std::vector<PhaseIndexPack>, VectorXi>(
-              &OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX, std::vector<PhaseIndexPackPtr>, VectorXi>(
-              &OptimalControlProblem::addLinkEqualCon));
-
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX, std::vector<PhaseIndexPack>, VectorXi>(
-              &OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            VectorXi>(&OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            VectorXi>(&OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkEqualCon));
-
-  obj.def("addLinkEqualCon",
-          py::overload_cast<VectorFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkEqualCon));
-
-
-  ///
-  /// InequalCons
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX, std::vector<PhaseIndexPack>, VectorXi>(
-              &OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX, std::vector<PhaseIndexPackPtr>, VectorXi>(
-              &OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX, std::vector<PhaseIndexPack>, VectorXi>(
-              &OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            VectorXi>(&OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            VectorXi>(&OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkInequalCon));
-
-  obj.def("addLinkInequalCon",
-          py::overload_cast<VectorFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkInequalCon));
-
-
-  ///
-  /// Objectives
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX, std::vector<PhaseIndexPack>, VectorXi>(
-              &OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX, std::vector<PhaseIndexPackPtr>, VectorXi>(
-              &OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX, std::vector<PhaseIndexPack>, VectorXi>(
-              &OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            VectorXi>(&OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            VectorXi>(&OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            int,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkObjective));
-
-  obj.def("addLinkObjective",
-          py::overload_cast<ScalarFunctionalX,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            PhasePtr,
-                            std::string,
-                            Eigen::VectorXi,
-                            Eigen::VectorXi>(&OptimalControlProblem::addLinkObjective));
-
-
-  ///////////////////////
-  obj.def("returnLinkEqualConVals", &OptimalControlProblem::returnLinkEqualConVals);
-  obj.def("returnLinkEqualConLmults", &OptimalControlProblem::returnLinkEqualConLmults);
-
-  obj.def("returnLinkInequalConVals", &OptimalControlProblem::returnLinkInequalConVals);
-  obj.def("returnLinkInequalConLmults", &OptimalControlProblem::returnLinkInequalConLmults);
-
-
-  ///////////////////////
-
-  obj.def_readwrite("AdaptiveMesh", &OptimalControlProblem::AdaptiveMesh);
-  obj.def_readwrite("PrintMeshInfo", &OptimalControlProblem::PrintMeshInfo);
-  obj.def_readwrite("MaxMeshIters", &OptimalControlProblem::MaxMeshIters);
-  obj.def_readonly("MeshConverged", &OptimalControlProblem::MeshConverged);
-  obj.def_readwrite("SolveOnlyFirst", &OptimalControlProblem::SolveOnlyFirst);
-
-  obj.def("setAdaptiveMesh",
-          &OptimalControlProblem::setAdaptiveMesh,
-          py::arg("AdaptiveMesh") = true,
-          py::arg("ApplyToPhases") = true);
-  obj.def("setMeshTol", &OptimalControlProblem::setMeshTol);
-  obj.def("setMeshRedFactor", &OptimalControlProblem::setMeshRedFactor);
-  obj.def("setMeshIncFactor", &OptimalControlProblem::setMeshIncFactor);
-  obj.def("setMeshErrFactor", &OptimalControlProblem::setMeshErrFactor);
-  obj.def("setMaxMeshIters", &OptimalControlProblem::setMaxMeshIters);
-  obj.def("setMinSegments", &OptimalControlProblem::setMinSegments);
-  obj.def("setMaxSegments", &OptimalControlProblem::setMaxSegments);
-  obj.def("setMeshErrorCriteria", &OptimalControlProblem::setMeshErrorCriteria);
-  obj.def("setMeshErrorEstimator", &OptimalControlProblem::setMeshErrorEstimator);
-}
diff --git a/src/OptimalControl/OptimalControlProblem.h b/src/OptimalControl/OptimalControlProblem.h
index 55badfa0..cfdd6494 100644
--- a/src/OptimalControl/OptimalControlProblem.h
+++ b/src/OptimalControl/OptimalControlProblem.h
@@ -3,6 +3,7 @@
 #include "LinkFunction.h"
 #include "ODEPhaseBase.h"
 #include "pch.h"
+#include "InterfaceTypes.h"
 
 namespace ASSET {
 
@@ -29,7 +30,12 @@ namespace ASSET {
     using PhaseIndexPackPtr =
         std::tuple<PhasePtr, std::string, Eigen::VectorXi, Eigen::VectorXi, Eigen::VectorXi>;
 
+    
+    using PhaseRefType = std::variant<int, PhasePtr, std::string>;
+
+    using PhasePack = std::tuple<PhaseRefType, RegionType, VarIndexType, VarIndexType, VarIndexType>;
 
+   
     std::vector<PhasePtr> phases;
     std::vector<std::string> phase_names;
 
@@ -44,10 +50,56 @@ namespace ASSET {
     std::map<int, LinkObjective>  LinkObjectives;
 
     VectorXd ActiveLinkParams;
-    void setLinkParams(VectorXd lp) {
-      this->ActiveLinkParams = lp;
-      this->numLinkParams = lp.size();
+    Eigen::VectorXd LPUnits;
+    bool AutoScaling = false;
+    bool SyncObjectiveScales = true;
+
+    std::map<std::string, Eigen::VectorXi> LPidxs;
+
+
+    void setLinkParams(VectorXd parm, VectorXd units) {
+        if (units.size() != parm.size()) {
+            throw std::invalid_argument("Size of link parameter vector and scaling units vector must match");
+        }
+
+        this->ActiveLinkParams = parm;
+        this->numLinkParams = parm.size();
+        this->resetTranscription();
+        this->LPUnits = units;
+    }
+    void setLinkParams(VectorXd parm) {
+        VectorXd units(parm.size());
+        units.setOnes();
+        return this->setLinkParams(parm, units);
+    }
+
+
+    void setLinkParamVgroups(std::map<std::string, Eigen::VectorXi> lpidxs) {
+        this->LPidxs = lpidxs;
+    }
+    void addLinkParamVgroups(std::map<std::string, Eigen::VectorXi> lpidxs) {
+        for (auto& [key, value] : lpidxs) {
+            this->LPidxs[key] = value;
+        }
     }
+    void addLinkParamVgroup(Eigen::VectorXi idx, std::string key) {
+        this->LPidxs[key] = idx;
+    }
+    void addLinkParamVgroup(int idx, std::string key) {
+        VectorXi tmp(1);
+        tmp << idx;
+        this->LPidxs[key] = tmp;
+    }
+    VectorXi getLPidx(std::string key) const {
+        if (LPidxs.count(key) == 0) {
+            throw std::invalid_argument(
+                fmt::format("No LinkParam variable index group with name: {0:} exists.", key));
+        }
+        return this->LPidxs.at(key);
+    }
+
+
+
     VectorXd returnLinkParams() {
       return this->ActiveLinkParams;
     }
@@ -98,6 +150,18 @@ namespace ASSET {
 
     bool MeshConverged = false;
 
+
+    void setAutoScaling(bool autoscale, bool applytophases) {
+        this->AutoScaling = autoscale;
+        if (applytophases) {
+            for (auto phase : this->phases) {
+                phase->setAutoScaling(autoscale);
+            }
+        }
+        this->resetTranscription();
+        this->invalidatePostOptInfo();
+    }
+
     void setAdaptiveMesh(bool amesh, bool applytophases) {
       this->AdaptiveMesh = amesh;
       if (applytophases) {
@@ -209,6 +273,22 @@ namespace ASSET {
       return int(ptrit - phases.begin());
     }
 
+    int getPhaseNum(PhaseRefType phase_t) {
+        int phasenum;
+
+        if (std::holds_alternative<int>(phase_t)) {
+            phasenum = std::get<int>(phase_t);
+        }
+        else if (std::holds_alternative<PhasePtr>(phase_t)) {
+            phasenum = this->getPhaseNum(std::get<PhasePtr>(phase_t));
+        }
+        else if (std::holds_alternative<std::string>(phase_t)) {
+            phasenum = this->getPhaseNum(std::get<std::string>(phase_t));
+        }
+        return phasenum;
+    }
+
+
     std::vector<VectorXi> ptl_from_phase_names(std::vector<std::vector<std::string>> ptlnamevec) {
       std::vector<VectorXi> ptl;
       for (auto& appl: ptlnamevec) {
@@ -247,150 +327,178 @@ namespace ASSET {
       return this->phases[ith];
     }
 
-    /////////////////////////////////////////////////
+    
 
-    template<class FuncType, class PackType, class OutType>
-    OutType makeLinkFunc(FuncType f, std::vector<PackType> packs, VectorXi lv) {
+   
 
-      int npacks = packs.size();
-      std::vector<Eigen::VectorXi> PTL;
-      VectorXi phasenums(npacks);
-      Eigen::Matrix<PhaseRegionFlags, -1, 1> RegFlags(npacks);
-      std::vector<Eigen::VectorXi> xtvs(npacks);
-      std::vector<Eigen::VectorXi> opvs(npacks);
-      std::vector<Eigen::VectorXi> spvs(npacks);
-
-      for (int i = 0; i < npacks; i++) {
-        if constexpr (std::is_same<PackType, PhaseIndexPack>::value) {
-          phasenums[i] = std::get<0>(packs[i]);
-        } else {
-          phasenums[i] = this->getPhaseNum(std::get<0>(packs[i]));
+    VectorXi getLPVars(VarIndexType LPvars_t) const {
+
+        VectorXi LPvars;
+
+        if (std::holds_alternative<int>(LPvars_t)) {
+            LPvars.resize(1);
+            LPvars[0] = std::get<int>(LPvars_t);
+        }
+        else if (std::holds_alternative<VectorXi>(LPvars_t)) {
+            LPvars = std::get<VectorXi>(LPvars_t);
+        }
+        else if (std::holds_alternative<std::string>(LPvars_t)) {
+            LPvars = this->getLPidx(std::get<std::string>(LPvars_t));
+        }
+        else if (std::holds_alternative<std::vector<std::string>>(LPvars_t)) {
+            std::vector<VectorXi> varvec;
+            int size = 0;
+            auto tmpvars = std::get<std::vector<std::string>>(LPvars_t);
+            for (auto tmpv : tmpvars) {
+                varvec.push_back(this->getLPidx(tmpv));
+                size += varvec.back().size();
+            }
+            LPvars.resize(size);
+            int next = 0;
+            for (auto varv : varvec) {
+                for (int i = 0; i < varv.size(); i++) {
+                    LPvars[next] = varv[i];
+                    next++;
+                }
+            }
         }
 
-        RegFlags[i] = strto_PhaseRegionFlag(std::get<1>(packs[i]));
-        xtvs[i] = std::get<2>(packs[i]);
-        opvs[i] = std::get<3>(packs[i]);
-        spvs[i] = std::get<4>(packs[i]);
-      }
+        return LPvars;
+    }
 
-      PTL.push_back(phasenums);
-      std::vector<Eigen::VectorXi> lvs;
-      lvs.push_back(lv);
-      return OutType(f, RegFlags, PTL, xtvs, opvs, spvs, lvs);
-    }
-    template<class FuncType, class PhaseType, class OutType>
-    OutType makeLinkFunc(FuncType f,
-                         PhaseType phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         Eigen::VectorXi opv0,
-                         Eigen::VectorXi spv0,
-                         PhaseType phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1,
-                         Eigen::VectorXi opv1,
-                         Eigen::VectorXi spv1,
-                         VectorXi lv) {
-
-      auto pack0 = std::tuple {phase0, reg0, xtv0, opv0, spv0};
-      auto pack1 = std::tuple {phase1, reg1, xtv1, opv1, spv1};
-      auto packs = std::vector {pack0, pack1};
-      return this->makeLinkFunc<FuncType, decltype(pack0), OutType>(f, packs, lv);
-    }
-
-    template<class FuncType, class PhaseType, class OutType>
-    OutType makeLinkFunc(FuncType f,
-                         PhaseType phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         Eigen::VectorXi opv0,
-                         Eigen::VectorXi spv0,
-                         PhaseType phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1,
-                         Eigen::VectorXi opv1,
-                         Eigen::VectorXi spv1) {
-
-      auto pack0 = std::tuple {phase0, reg0, xtv0, opv0, spv0};
-      auto pack1 = std::tuple {phase1, reg1, xtv1, opv1, spv1};
-      auto packs = std::vector {pack0, pack1};
-      Eigen::VectorXi lv;
-      lv.resize(0);
-      return this->makeLinkFunc<FuncType, decltype(pack0), OutType>(f, packs, lv);
-    }
-
-    template<class FuncType, class PhaseType, class OutType>
-    OutType makeLinkFunc(FuncType f,
-                         PhaseType phase0,
-                         std::string reg0,
-                         Eigen::VectorXi v0,
-                         PhaseType phase1,
-                         std::string reg1,
-                         Eigen::VectorXi v1,
-                         Eigen::VectorXi lv) {
-
-      Eigen::VectorXi xtv0, opv0, spv0, xtv1, opv1, spv1;
-
-      strto_PhaseRegionFlag(reg0);
-      strto_PhaseRegionFlag(reg1);
-
-      if (reg0 == "ODEParams")
-        opv0 = v0;
-      else if (reg0 == "StaticParams")
-        spv0 = v0;
-      else
-        xtv0 = v0;
 
-      if (reg1 == "ODEParams")
-        opv1 = v1;
-      else if (reg1 == "StaticParams")
-        spv1 = v1;
-      else
-        xtv1 = v1;
+    template<class FuncHolder, class FuncType>
+    FuncHolder makeFuncImpl(FuncType fun, std::vector<PhasePack> packs, VarIndexType lv, ScaleType scale_t) {
+
+        int npacks = packs.size();
+        std::vector<Eigen::VectorXi> PTL;
+        VectorXi phasenums(npacks);
+        Eigen::Matrix<PhaseRegionFlags, -1, 1> RegFlags(npacks);
+        std::vector<Eigen::VectorXi> xtvs(npacks);
+        std::vector<Eigen::VectorXi> opvs(npacks);
+        std::vector<Eigen::VectorXi> spvs(npacks);
+
 
+        for (int i = 0; i < npacks; i++) {
+
+            auto phase_t = std::get<0>(packs[i]); // Name of phase, either phaseptr,int, or string
+            int phasenum = getPhaseNum(phase_t);
+
+            if (phasenum < 0 || phasenum >= this->phases.size()) {
+                throw std::invalid_argument(fmt::format("Function references non - existent phase : {0:}\n",phasenum));
+            }
+
+
+            phasenums[i] = phasenum;
+
+            RegFlags[i] = this->phases[phasenum]->getRegion(std::get<1>(packs[i]));
+            xtvs[i] = this->phases[phasenum]->getXtUPVars(RegFlags[i],std::get<2>(packs[i]));
+            opvs[i] = this->phases[phasenum]->getOPVars(RegFlags[i], std::get<3>(packs[i]));
+            spvs[i] = this->phases[phasenum]->getSPVars(RegFlags[i], std::get<4>(packs[i]));
+        }
+
+        std::vector<Eigen::VectorXi> lvs;
+        lvs.push_back(getLPVars(lv));
+        PTL.push_back(phasenums);
+
+        auto func = FuncHolder(fun, RegFlags, PTL, xtvs, opvs, spvs, lvs, scale_t);
+        return func;
 
-      auto pack0 = std::tuple {phase0, reg0, xtv0, opv0, spv0};
-      auto pack1 = std::tuple {phase1, reg1, xtv1, opv1, spv1};
-      auto packs = std::vector {pack0, pack1};
-      return this->makeLinkFunc<FuncType, decltype(pack0), OutType>(f, packs, lv);
     }
-    template<class FuncType, class PhaseType, class OutType>
-    OutType makeLinkFunc(FuncType f,
-                         PhaseType phase0,
-                         std::string reg0,
-                         Eigen::VectorXi v0,
-                         PhaseType phase1,
-                         std::string reg1,
-                         Eigen::VectorXi v1) {
 
-      Eigen::VectorXi xtv0, opv0, spv0, xtv1, opv1, spv1;
 
-      strto_PhaseRegionFlag(reg0);
-      strto_PhaseRegionFlag(reg1);
+    template<class FuncHolder, class FuncType>
+    FuncHolder makeFuncImpl(FuncType fun, PhaseRefType p0, RegionType reg0, VarIndexType XtUV0, VarIndexType OPV0, VarIndexType SPV0,
+        PhaseRefType p1, RegionType reg1, VarIndexType XtUV1, VarIndexType OPV1, VarIndexType SPV1,
+        VarIndexType lv, ScaleType scale_t) {
 
-      if (reg0 == "ODEParams")
-        opv0 = v0;
-      else if (reg0 == "StaticParams")
-        spv0 = v0;
-      else
-        xtv0 = v0;
+        auto pack0 = PhasePack{ p0,reg0,XtUV0,OPV0,SPV0 };
+        auto pack1 = PhasePack{ p1,reg1,XtUV1,OPV1,SPV1 };
 
-      if (reg1 == "ODEParams")
-        opv1 = v1;
-      else if (reg1 == "StaticParams")
-        spv1 = v1;
-      else
-        xtv1 = v1;
+        auto packs = std::vector{ pack0,pack1 };
+        return makeFuncImpl<FuncHolder, FuncType>(fun, packs, lv, scale_t);
+    }
+
+    template<class FuncHolder, class FuncType>
+    FuncHolder makeFuncImpl(FuncType fun, 
+        PhaseRefType p0, RegionType reg0_t, VarIndexType v0,
+        PhaseRefType p1, RegionType reg1_t, VarIndexType v1, 
+        VarIndexType lv, ScaleType scale_t) {
+
+        VarIndexType xtv0, opv0, spv0, xtv1, opv1, spv1;
+
+        xtv0 = VectorXi();
+        opv0 = VectorXi();
+        spv0 = VectorXi();
+
+        xtv1 = VectorXi();
+        opv1 = VectorXi();
+        spv1 = VectorXi();
+
+
+        PhaseRegionFlags reg0,reg1;
+
+
+        if (std::holds_alternative<PhaseRegionFlags>(reg0_t)) {
+            reg0 = std::get<PhaseRegionFlags>(reg0_t);
+        }
+        else if (std::holds_alternative<std::string>(reg0_t)) {
+            reg0 = strto_PhaseRegionFlag(std::get<std::string>(reg0_t));
+        }
+
+        if (std::holds_alternative<PhaseRegionFlags>(reg1_t)) {
+            reg1 = std::get<PhaseRegionFlags>(reg1_t);
+        }
+        else if (std::holds_alternative<std::string>(reg1_t)) {
+            reg1 = strto_PhaseRegionFlag(std::get<std::string>(reg1_t));
+        }
 
 
-      auto pack0 = std::tuple {phase0, reg0, xtv0, opv0, spv0};
-      auto pack1 = std::tuple {phase1, reg1, xtv1, opv1, spv1};
-      auto packs = std::vector {pack0, pack1};
-      Eigen::VectorXi lv;
-      lv.resize(0);
-      return this->makeLinkFunc<FuncType, decltype(pack0), OutType>(f, packs, lv);
+        
+
+        if (reg0 == ODEParams)
+            opv0 = v0;
+        else if (reg0 == StaticParams)
+            spv0 = v0;
+        else
+            xtv0 = v0;
+
+        if (reg1 == ODEParams)
+            opv1 = v1;
+        else if (reg1 == StaticParams)
+            spv1 = v1;
+        else
+            xtv1 = v1;
+
+
+        auto pack0 = PhasePack{ p0, reg0, xtv0, opv0, spv0 };
+        auto pack1 = PhasePack{ p1, reg1, xtv1, opv1, spv1 };
+        auto packs = std::vector{ pack0, pack1 };
+
+        return makeFuncImpl<FuncHolder, FuncType>(fun, packs, lv, scale_t);
+    }
+
+
+    template<class FuncHolder, class FuncType>
+    FuncHolder makeFuncImpl(FuncType fun,
+        PhaseRefType p0, RegionType reg0_t, VarIndexType v0,
+        PhaseRefType p1, RegionType reg1_t, VarIndexType v1,
+        ScaleType scale_t) {
+
+        VectorXi empty;
+
+        return makeFuncImpl<FuncHolder, FuncType>(fun,
+            p0, reg0_t, v0,
+            p1, reg1_t, v1,
+            empty, scale_t);
     }
 
+
+    /////////////////////////////////////////////////
+
+
+    
+
     template<class FuncMap>
     void removeFuncImpl(FuncMap& map, int index, const std::string& funcstr) {
       this->resetTranscription();
@@ -426,132 +534,153 @@ namespace ASSET {
 
 
     /////////////// THE NEW EQUALCON INTERFACE//////////////////////////////
+    
 
-    int addLinkEqualCon(VectorFunctionalX lc, std::vector<PhaseIndexPack> packs, VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhaseIndexPack, LinkConstraint>(lc, packs, lv);
-      return this->addLinkEqualCon(Func);
-    }
-    int addLinkEqualCon(VectorFunctionalX lc, std::vector<PhaseIndexPackPtr> packs, VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhaseIndexPackPtr, LinkConstraint>(lc, packs, lv);
-      return this->addLinkEqualCon(Func);
+    int addLinkEqualCon(VectorFunctionalX lc, std::vector<PhasePack> packs, VarIndexType lv, ScaleType scale_t) {
+        auto Func = this->makeFuncImpl<LinkConstraint, VectorFunctionalX>(lc, packs, lv,scale_t);
+        return addFuncImpl(Func, this->LinkEqualities, "Link Equality Constraint");
     }
 
+    int addLinkEqualCon(VectorFunctionalX lc, 
+        PhaseRefType p0, RegionType reg0, VarIndexType XtUV0, VarIndexType OPV0, VarIndexType SPV0,
+        PhaseRefType p1, RegionType reg1, VarIndexType XtUV1, VarIndexType OPV1, VarIndexType SPV1,
+        VarIndexType lv, ScaleType scale_t) {
 
-    int addLinkEqualCon(VectorFunctionalX lc,
-                        int phase0,
-                        std::string reg0,
-                        Eigen::VectorXi xtv0,
-                        Eigen::VectorXi opv0,
-                        Eigen::VectorXi spv0,
-                        int phase1,
-                        std::string reg1,
-                        Eigen::VectorXi xtv1,
-                        Eigen::VectorXi opv1,
-                        Eigen::VectorXi spv1,
-                        VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, int, LinkConstraint>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1, lv);
-      return this->addLinkEqualCon(Func);
-    }
-    int addLinkEqualCon(VectorFunctionalX lc,
-                        PhasePtr phase0,
-                        std::string reg0,
-                        Eigen::VectorXi xtv0,
-                        Eigen::VectorXi opv0,
-                        Eigen::VectorXi spv0,
-                        PhasePtr phase1,
-                        std::string reg1,
-                        Eigen::VectorXi xtv1,
-                        Eigen::VectorXi opv1,
-                        Eigen::VectorXi spv1,
-                        VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhasePtr, LinkConstraint>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1, lv);
-      return this->addLinkEqualCon(Func);
+        auto Func = this->makeFuncImpl<LinkConstraint, VectorFunctionalX>(lc,
+             p0,  reg0,  XtUV0,  OPV0,  SPV0,
+             p1,  reg1,  XtUV1,  OPV1,  SPV1,
+             lv,  scale_t);
+        return addFuncImpl(Func, this->LinkEqualities, "Link Equality Constraint");
     }
 
-
     int addLinkEqualCon(VectorFunctionalX lc,
-                        int phase0,
-                        std::string reg0,
-                        Eigen::VectorXi xtv0,
-                        Eigen::VectorXi opv0,
-                        Eigen::VectorXi spv0,
-                        int phase1,
-                        std::string reg1,
-                        Eigen::VectorXi xtv1,
-                        Eigen::VectorXi opv1,
-                        Eigen::VectorXi spv1) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, int, LinkConstraint>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1);
-      return this->addLinkEqualCon(Func);
+        PhaseRefType p0, RegionType reg0, VarIndexType v0,
+        PhaseRefType p1, RegionType reg1, VarIndexType v1,
+        VarIndexType lv, ScaleType scale_t) {
+
+        auto Func = this->makeFuncImpl<LinkConstraint, VectorFunctionalX>(lc,
+            p0, reg0, v0, 
+            p1, reg1, v1, 
+            lv, scale_t);
+        return addFuncImpl(Func, this->LinkEqualities, "Link Equality Constraint");
     }
     int addLinkEqualCon(VectorFunctionalX lc,
-                        PhasePtr phase0,
-                        std::string reg0,
-                        Eigen::VectorXi xtv0,
-                        Eigen::VectorXi opv0,
-                        Eigen::VectorXi spv0,
-                        PhasePtr phase1,
-                        std::string reg1,
-                        Eigen::VectorXi xtv1,
-                        Eigen::VectorXi opv1,
-                        Eigen::VectorXi spv1) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhasePtr, LinkConstraint>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1);
-      return this->addLinkEqualCon(Func);
+        PhaseRefType p0, RegionType reg0, VarIndexType v0,
+        PhaseRefType p1, RegionType reg1, VarIndexType v1,
+         ScaleType scale_t) {
+
+        auto Func = this->makeFuncImpl<LinkConstraint, VectorFunctionalX>(lc,
+            p0, reg0, v0,
+            p1, reg1, v1,
+           scale_t);
+        return addFuncImpl(Func, this->LinkEqualities, "Link Equality Constraint");
     }
 
+    std::vector<int> 
+        addForwardLinkEqualCon(PhaseRefType iphase_t, PhaseRefType fphase_t, VarIndexType vars,
+            ScaleType scale_t) {
 
-    int addLinkEqualCon(VectorFunctionalX lc,
-                        int phase0,
-                        std::string reg0,
-                        Eigen::VectorXi xtv0,
-                        int phase1,
-                        std::string reg1,
-                        Eigen::VectorXi xtv1,
-                        Eigen::VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, int, LinkConstraint>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1, lv);
-      return this->addLinkEqualCon(Func);
+        int iphase = getPhaseNum(iphase_t);
+        int fphase = getPhaseNum(fphase_t);
+
+
+
+        if (iphase < 0)
+            iphase = (this->phases.size() + iphase);
+        if (fphase < 0)
+            fphase = (this->phases.size() + fphase);
+
+        if (iphase < 0 || iphase >= this->phases.size()) {
+            throw std::invalid_argument(fmt::format("Link Equality constraint references non-existent phase:{0:}\n",iphase));
+        }
+
+        int vsize = this->phases[iphase]->getXtUPVars(Front, vars).size();
+
+        auto args = Arguments<-1>(2 * vsize);
+        auto func = args.head<-1>(vsize) - args.tail<-1>(vsize);
+
+        std::vector<int> idxs;
+        for (int i = iphase; i < fphase; i++) {
+            
+            int idx = this->addLinkEqualCon(func, i, "Last", vars, i + 1, "First", vars,scale_t);
+
+            idxs.push_back(idx);
+        }
+        
+        return idxs;
     }
-    int addLinkEqualCon(VectorFunctionalX lc,
-                        PhasePtr phase0,
-                        std::string reg0,
-                        Eigen::VectorXi xtv0,
-                        PhasePtr phase1,
-                        std::string reg1,
-                        Eigen::VectorXi xtv1,
-                        Eigen::VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhasePtr, LinkConstraint>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1, lv);
-      return this->addLinkEqualCon(Func);
+
+
+    int addDirectLinkEqualCon(PhaseRefType p0, RegionType reg0_t, VarIndexType v0,
+        PhaseRefType p1, RegionType reg1_t, VarIndexType v1,
+        ScaleType scale_t) {
+
+        int phase = getPhaseNum(p0);
+
+        if (phase < 0)
+            phase = (this->phases.size() + phase);
+        
+        if (phase < 0 || phase >= this->phases.size()) {
+            throw std::invalid_argument(fmt::format("Link Equality constraint references non-existent phase:{0:}\n", phase));
+        }
+
+        PhaseRegionFlags reg0 = get_PhaseRegion(reg0_t);
+
+        int vsize = this->phases[phase]->getXtUPVars(reg0, v0).size();
+
+        auto args = Arguments<-1>(2 * vsize);
+        auto func = args.head<-1>(vsize) - args.tail<-1>(vsize);
+
+        return this->addLinkEqualCon(func, p0, reg0_t, v0, p1, reg1_t, v1, scale_t);
+
     }
 
+    std::vector<int> addParamLinkEqualCon(PhaseRefType iphase_t, PhaseRefType fphase_t, RegionType reg0_t, VarIndexType vars, ScaleType scale_t) {
+
+        PhaseRegionFlags reg0 = get_PhaseRegion(reg0_t);
+
+        if (reg0 != ODEParams && reg0 != StaticParams) {
+            throw std::invalid_argument("Phase Region must be ODEParams or StaticParams");
+        }
+
+        int iphase = getPhaseNum(iphase_t);
+        int fphase = getPhaseNum(fphase_t);
+
+        if (iphase < 0)
+            iphase = (this->phases.size() + iphase);
+        if (fphase < 0)
+            fphase = (this->phases.size() + fphase);
+
+        if (iphase < 0 || iphase >= this->phases.size()) {
+            throw std::invalid_argument(fmt::format("Link Equality constraint references non-existent phase:{0:}\n", iphase));
+        }
+
+        std::vector<int> idxs;
+        for (int i = iphase; i < fphase; i++) {
+
+            int idx = this->addDirectLinkEqualCon(i, reg0, vars, i + 1, reg0, vars, scale_t);
+
+            idxs.push_back(idx);
+        }
+
+        return idxs;
 
-    int addLinkEqualCon(VectorFunctionalX lc,
-                        int phase0,
-                        std::string reg0,
-                        Eigen::VectorXi xtv0,
-                        int phase1,
-                        std::string reg1,
-                        Eigen::VectorXi xtv1) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, int, LinkConstraint>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1);
-      return this->addLinkEqualCon(Func);
     }
-    int addLinkEqualCon(VectorFunctionalX lc,
-                        PhasePtr phase0,
-                        std::string reg0,
-                        Eigen::VectorXi xtv0,
-                        PhasePtr phase1,
-                        std::string reg1,
-                        Eigen::VectorXi xtv1) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhasePtr, LinkConstraint>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1);
-      return this->addLinkEqualCon(Func);
+
+    
+    int addLinkParamEqualCon(VectorFunctionalX lc, std::vector<VectorXi> lpvs,ScaleType scale_t) {
+        std::vector<Eigen::VectorXi> empty;
+        return this->addLinkEqualCon(
+            LinkConstraint(lc, LinkFlags::LinkParams, empty, empty, empty, empty, lpvs, scale_t));
+    }
+    int addLinkParamEqualCon(VectorFunctionalX lc, VectorXi lpv, ScaleType scale_t) {
+        std::vector<Eigen::VectorXi> lpvs;
+        lpvs.push_back(lpv);
+        return this->addLinkParamEqualCon(lc, lpvs,scale_t);
     }
 
+    //////////////////////////////////////////////////////////////
+
 
     int addLinkEqualCon(VectorFunctionalX lc,
                         RegVec regs,
@@ -863,139 +992,60 @@ namespace ASSET {
     ////////////////////////////////////////////////////////////////////
 
     int addLinkInequalCon(LinkConstraint lc) {
-      return addFuncImpl(lc, this->LinkInequalities, "Link Inequality Constrain");
+      return addFuncImpl(lc, this->LinkInequalities, "Link Inequality Constraint");
     }
 
-
-    
-    /////////////// THE NEW INEQUALCON INTERFACE//////////////////////////////
-
-    int addLinkInequalCon(VectorFunctionalX lc, std::vector<PhaseIndexPack> packs, VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhaseIndexPack, LinkConstraint>(lc, packs, lv);
-      return this->addLinkInequalCon(Func);
+    int addLinkInequalCon(VectorFunctionalX lc, std::vector<PhasePack> packs, VarIndexType lv, ScaleType scale_t) {
+        auto Func = this->makeFuncImpl<LinkConstraint, VectorFunctionalX>(lc, packs, lv, scale_t);
+        return addFuncImpl(Func, this->LinkInequalities, "Link Inequality Constraint");
     }
-    int addLinkInequalCon(VectorFunctionalX lc, std::vector<PhaseIndexPackPtr> packs, VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhaseIndexPackPtr, LinkConstraint>(lc, packs, lv);
-      return this->addLinkInequalCon(Func);
-    }
-
 
     int addLinkInequalCon(VectorFunctionalX lc,
-                          int phase0,
-                          std::string reg0,
-                          Eigen::VectorXi xtv0,
-                          Eigen::VectorXi opv0,
-                          Eigen::VectorXi spv0,
-                          int phase1,
-                          std::string reg1,
-                          Eigen::VectorXi xtv1,
-                          Eigen::VectorXi opv1,
-                          Eigen::VectorXi spv1,
-                          VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, int, LinkConstraint>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1, lv);
-      return this->addLinkInequalCon(Func);
-    }
-    int addLinkInequalCon(VectorFunctionalX lc,
-                          PhasePtr phase0,
-                          std::string reg0,
-                          Eigen::VectorXi xtv0,
-                          Eigen::VectorXi opv0,
-                          Eigen::VectorXi spv0,
-                          PhasePtr phase1,
-                          std::string reg1,
-                          Eigen::VectorXi xtv1,
-                          Eigen::VectorXi opv1,
-                          Eigen::VectorXi spv1,
-                          VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhasePtr, LinkConstraint>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1, lv);
-      return this->addLinkInequalCon(Func);
-    }
-
+        PhaseRefType p0, RegionType reg0, VarIndexType XtUV0, VarIndexType OPV0, VarIndexType SPV0,
+        PhaseRefType p1, RegionType reg1, VarIndexType XtUV1, VarIndexType OPV1, VarIndexType SPV1,
+        VarIndexType lv, ScaleType scale_t) {
 
-    int addLinkInequalCon(VectorFunctionalX lc,
-                          int phase0,
-                          std::string reg0,
-                          Eigen::VectorXi xtv0,
-                          Eigen::VectorXi opv0,
-                          Eigen::VectorXi spv0,
-                          int phase1,
-                          std::string reg1,
-                          Eigen::VectorXi xtv1,
-                          Eigen::VectorXi opv1,
-                          Eigen::VectorXi spv1) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, int, LinkConstraint>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1);
-      return this->addLinkInequalCon(Func);
-    }
-    int addLinkInequalCon(VectorFunctionalX lc,
-                          PhasePtr phase0,
-                          std::string reg0,
-                          Eigen::VectorXi xtv0,
-                          Eigen::VectorXi opv0,
-                          Eigen::VectorXi spv0,
-                          PhasePtr phase1,
-                          std::string reg1,
-                          Eigen::VectorXi xtv1,
-                          Eigen::VectorXi opv1,
-                          Eigen::VectorXi spv1) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhasePtr, LinkConstraint>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1);
-      return this->addLinkInequalCon(Func);
+        auto Func = this->makeFuncImpl<LinkConstraint, VectorFunctionalX>(lc,
+            p0, reg0, XtUV0, OPV0, SPV0,
+            p1, reg1, XtUV1, OPV1, SPV1,
+            lv, scale_t);
+        return addFuncImpl(Func, this->LinkInequalities, "Link Inequality Constraint");
     }
 
-
     int addLinkInequalCon(VectorFunctionalX lc,
-                          int phase0,
-                          std::string reg0,
-                          Eigen::VectorXi xtv0,
-                          int phase1,
-                          std::string reg1,
-                          Eigen::VectorXi xtv1,
-                          Eigen::VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, int, LinkConstraint>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1, lv);
-      return this->addLinkInequalCon(Func);
+        PhaseRefType p0, RegionType reg0, VarIndexType v0,
+        PhaseRefType p1, RegionType reg1, VarIndexType v1,
+        VarIndexType lv, ScaleType scale_t) {
+
+        auto Func = this->makeFuncImpl<LinkConstraint, VectorFunctionalX>(lc,
+            p0, reg0, v0,
+            p1, reg1, v1,
+            lv, scale_t);
+        return addFuncImpl(Func, this->LinkInequalities, "Link Inequality Constraint");
     }
     int addLinkInequalCon(VectorFunctionalX lc,
-                          PhasePtr phase0,
-                          std::string reg0,
-                          Eigen::VectorXi xtv0,
-                          PhasePtr phase1,
-                          std::string reg1,
-                          Eigen::VectorXi xtv1,
-                          Eigen::VectorXi lv) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhasePtr, LinkConstraint>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1, lv);
-      return this->addLinkInequalCon(Func);
-    }
-
+        PhaseRefType p0, RegionType reg0, VarIndexType v0,
+        PhaseRefType p1, RegionType reg1, VarIndexType v1,
+        ScaleType scale_t) {
 
-    int addLinkInequalCon(VectorFunctionalX lc,
-                          int phase0,
-                          std::string reg0,
-                          Eigen::VectorXi xtv0,
-                          int phase1,
-                          std::string reg1,
-                          Eigen::VectorXi xtv1) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, int, LinkConstraint>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1);
-      return this->addLinkInequalCon(Func);
+        auto Func = this->makeFuncImpl<LinkConstraint, VectorFunctionalX>(lc,
+            p0, reg0, v0,
+            p1, reg1, v1,
+            scale_t);
+        return addFuncImpl(Func, this->LinkInequalities, "Link Inequality Constraint");
     }
-    int addLinkInequalCon(VectorFunctionalX lc,
-                          PhasePtr phase0,
-                          std::string reg0,
-                          Eigen::VectorXi xtv0,
-                          PhasePtr phase1,
-                          std::string reg1,
-                          Eigen::VectorXi xtv1) {
-      auto Func = this->makeLinkFunc<VectorFunctionalX, PhasePtr, LinkConstraint>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1);
-      return this->addLinkInequalCon(Func);
+    int addLinkParamInequalCon(VectorFunctionalX lc, std::vector<VectorXi> lpvs, ScaleType scale_t) {
+        std::vector<Eigen::VectorXi> empty;
+        return this->addLinkInequalCon(
+            LinkConstraint(lc, LinkFlags::LinkParams, empty, empty, empty, empty, lpvs, scale_t));
+    }
+    int addLinkParamInequalCon(VectorFunctionalX lc, VectorXi lpv, ScaleType scale_t) {
+        std::vector<Eigen::VectorXi> lpvs;
+        lpvs.push_back(lpv);
+        return this->addLinkParamInequalCon(lc, lpvs, scale_t);
     }
 
-
+    
     //////////////////////////
 
 
@@ -1152,131 +1202,57 @@ namespace ASSET {
     int addLinkObjective(LinkObjective lc) {
       return addFuncImpl(lc, this->LinkObjectives, "Link Objective");
     }
-    /////////////// THE NEW INEQUALCON INTERFACE//////////////////////////////
-
-    int addLinkObjective(ScalarFunctionalX lc, std::vector<PhaseIndexPack> packs, VectorXi lv) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, PhaseIndexPack, LinkObjective>(lc, packs, lv);
-      return this->addLinkObjective(Func);
-    }
-    int addLinkObjective(ScalarFunctionalX lc, std::vector<PhaseIndexPackPtr> packs, VectorXi lv) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, PhaseIndexPackPtr, LinkObjective>(lc, packs, lv);
-      return this->addLinkObjective(Func);
-    }
 
+    
 
-    int addLinkObjective(ScalarFunctionalX lc,
-                         int phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         Eigen::VectorXi opv0,
-                         Eigen::VectorXi spv0,
-                         int phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1,
-                         Eigen::VectorXi opv1,
-                         Eigen::VectorXi spv1,
-                         VectorXi lv) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, int, LinkObjective>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1, lv);
-      return this->addLinkObjective(Func);
-    }
-    int addLinkObjective(ScalarFunctionalX lc,
-                         PhasePtr phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         Eigen::VectorXi opv0,
-                         Eigen::VectorXi spv0,
-                         PhasePtr phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1,
-                         Eigen::VectorXi opv1,
-                         Eigen::VectorXi spv1,
-                         VectorXi lv) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, PhasePtr, LinkObjective>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1, lv);
-      return this->addLinkObjective(Func);
+    int addLinkObjective(ScalarFunctionalX lc, std::vector<PhasePack> packs, VarIndexType lv, ScaleType scale_t) {
+        auto Func = this->makeFuncImpl<LinkObjective, ScalarFunctionalX>(lc, packs, lv, scale_t);
+        return addFuncImpl(Func, this->LinkObjectives, "Link Objective");
     }
 
-
-    int addLinkObjective(ScalarFunctionalX lc,
-                         int phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         Eigen::VectorXi opv0,
-                         Eigen::VectorXi spv0,
-                         int phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1,
-                         Eigen::VectorXi opv1,
-                         Eigen::VectorXi spv1) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, int, LinkObjective>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1);
-      return this->addLinkObjective(Func);
-    }
     int addLinkObjective(ScalarFunctionalX lc,
-                         PhasePtr phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         Eigen::VectorXi opv0,
-                         Eigen::VectorXi spv0,
-                         PhasePtr phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1,
-                         Eigen::VectorXi opv1,
-                         Eigen::VectorXi spv1) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, PhasePtr, LinkObjective>(
-          lc, phase0, reg0, xtv0, opv0, spv0, phase1, reg1, xtv1, opv1, spv1);
-      return this->addLinkObjective(Func);
-    }
+        PhaseRefType p0, RegionType reg0, VarIndexType XtUV0, VarIndexType OPV0, VarIndexType SPV0,
+        PhaseRefType p1, RegionType reg1, VarIndexType XtUV1, VarIndexType OPV1, VarIndexType SPV1,
+        VarIndexType lv, ScaleType scale_t) {
 
+        auto Func = this->makeFuncImpl<LinkObjective, ScalarFunctionalX>(lc,
+            p0, reg0, XtUV0, OPV0, SPV0,
+            p1, reg1, XtUV1, OPV1, SPV1,
+            lv, scale_t);
+        return addFuncImpl(Func, this->LinkObjectives, "Link Objective");
+    }
 
     int addLinkObjective(ScalarFunctionalX lc,
-                         int phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         int phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1,
-                         Eigen::VectorXi lv) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, int, LinkObjective>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1, lv);
-      return this->addLinkObjective(Func);
+        PhaseRefType p0, RegionType reg0, VarIndexType v0,
+        PhaseRefType p1, RegionType reg1, VarIndexType v1,
+        VarIndexType lv, ScaleType scale_t) {
+
+        auto Func = this->makeFuncImpl<LinkObjective, ScalarFunctionalX>(lc,
+            p0, reg0, v0,
+            p1, reg1, v1,
+            lv, scale_t);
+        return addFuncImpl(Func, this->LinkObjectives, "Link Objective");
     }
     int addLinkObjective(ScalarFunctionalX lc,
-                         PhasePtr phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         PhasePtr phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1,
-                         Eigen::VectorXi lv) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, PhasePtr, LinkObjective>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1, lv);
-      return this->addLinkObjective(Func);
-    }
-
+        PhaseRefType p0, RegionType reg0, VarIndexType v0,
+        PhaseRefType p1, RegionType reg1, VarIndexType v1,
+        ScaleType scale_t) {
 
-    int addLinkObjective(ScalarFunctionalX lc,
-                         int phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         int phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, int, LinkObjective>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1);
-      return this->addLinkObjective(Func);
+        auto Func = this->makeFuncImpl<LinkObjective, ScalarFunctionalX>(lc,
+            p0, reg0, v0,
+            p1, reg1, v1,
+            scale_t);
+        return addFuncImpl(Func, this->LinkObjectives, "Link Objective");
     }
-    int addLinkObjective(ScalarFunctionalX lc,
-                         PhasePtr phase0,
-                         std::string reg0,
-                         Eigen::VectorXi xtv0,
-                         PhasePtr phase1,
-                         std::string reg1,
-                         Eigen::VectorXi xtv1) {
-      auto Func = this->makeLinkFunc<ScalarFunctionalX, PhasePtr, LinkObjective>(
-          lc, phase0, reg0, xtv0, phase1, reg1, xtv1);
-      return this->addLinkObjective(Func);
+    int addLinkParamObjective(ScalarFunctionalX lc, std::vector<VectorXi> lpvs, ScaleType scale_t) {
+        std::vector<Eigen::VectorXi> empty;
+        return this->addLinkObjective(
+            LinkObjective(lc, LinkFlags::LinkParams, empty, empty, empty, empty, lpvs, scale_t));
+    }
+    int addLinkParamObjective(ScalarFunctionalX lc, VectorXi lpv, ScaleType scale_t) {
+        std::vector<Eigen::VectorXi> lpvs;
+        lpvs.push_back(lpv);
+        return this->addLinkParamObjective(lc, lpvs, scale_t);
     }
 
 
@@ -1547,10 +1523,36 @@ namespace ASSET {
       return Allvals;
     }
 
-
-
+    Eigen::VectorXd returnLinkEqualConScales(int index) {
+        return this->LinkEqualities.at(index).OutputScales;
+    }
+    Eigen::VectorXd returnLinkInequalConScales(int index) {
+        return this->LinkInequalities.at(index).OutputScales;
+    }
+    Eigen::VectorXd returnLinkObjectiveScales(int index) {
+        return this->LinkObjectives.at(index).OutputScales;
+    }
     
     ///////////////////////////////////////////////////
+
+    Eigen::VectorXd get_input_scale(LinkFlags lflag, Eigen::Vector<PhaseRegionFlags, -1> regs,
+        std::vector<VectorXi> phases_to_link,
+        std::vector<VectorXi> XtUVars,
+        std::vector<VectorXi> OPVars,
+        std::vector<VectorXi> SPVars,
+        std::vector<VectorXi> LVars
+    );
+
+    std::vector<Eigen::VectorXd> get_test_inputs(LinkFlags lflag, Eigen::Vector<PhaseRegionFlags, -1> regs,
+        std::vector<VectorXi> phases_to_link,
+        std::vector<VectorXi> XtUVars,
+        std::vector<VectorXi> OPVars,
+        std::vector<VectorXi> SPVars,
+        std::vector<VectorXi> LVars
+    );
+
+
+
     void checkTranscriptions() {
       for (int i = 0; i < this->phases.size(); i++) {
         if (this->phases[i]->doTranscription) {
@@ -1706,6 +1708,12 @@ namespace ASSET {
 
     void transcribe_links();
 
+    void calc_auto_scales();
+
+    std::vector<double> get_objective_scales();
+    void update_objective_scales(double scale);
+
+
     void transcribe(bool showstats, bool showfuns);
 
     void transcribe() {
@@ -1798,7 +1806,12 @@ namespace ASSET {
           Start = this->numPhaseVars.segment(0, i).sum();
         Vars.segment(Start, this->numPhaseVars[i]) = this->phases[i]->makeSolverInput();
       }
-      Vars.tail(this->numLinkParams) = this->ActiveLinkParams;
+      if (this->AutoScaling  && this->LPUnits.size()>0) {
+          Vars.tail(this->numLinkParams) = this->ActiveLinkParams.cwiseQuotient(this->LPUnits);
+      }
+      else {
+          Vars.tail(this->numLinkParams) = this->ActiveLinkParams;
+      }
 
       return Vars;
     }
@@ -1810,7 +1823,12 @@ namespace ASSET {
           Start = this->numPhaseVars.segment(0, i).sum();
         this->phases[i]->collectSolverOutput(Vars.segment(Start, this->numPhaseVars[i]));
       }
-      this->ActiveLinkParams = Vars.tail(this->numLinkParams);
+      if (this->AutoScaling && this->LPUnits.size() > 0) {
+          this->ActiveLinkParams = Vars.tail(this->numLinkParams).cwiseProduct(this->LPUnits);
+      }
+      else {
+          this->ActiveLinkParams = Vars.tail(this->numLinkParams);
+      }
     }
     void collectSolverMultipliers(const VectorXd& EM, const VectorXd& IM) {
       this->MultipliersLoaded = true;
@@ -1875,6 +1893,10 @@ namespace ASSET {
 
 
     static void Build(py::module& m);
+    static void BuildNewLinkIterface(py::class_<OptimalControlProblem, std::shared_ptr<OptimalControlProblem>, OptimizationProblemBase>&);
+    static void BuildOldLinkIterface(py::class_<OptimalControlProblem, std::shared_ptr<OptimalControlProblem>, OptimizationProblemBase>&);
+
+
   };
 
 }  // namespace ASSET
diff --git a/src/OptimalControl/OptimalControlProblemBind.cpp b/src/OptimalControl/OptimalControlProblemBind.cpp
new file mode 100644
index 00000000..a1402c7f
--- /dev/null
+++ b/src/OptimalControl/OptimalControlProblemBind.cpp
@@ -0,0 +1,882 @@
+#include "OptimalControlProblem.h"
+#include "PyDocString/OptimalControl/OptimalControlProblem_doc.h"
+
+void ASSET::OptimalControlProblem::Build(py::module& m) {
+    using namespace doc;
+    auto obj =
+        py::class_<OptimalControlProblem, std::shared_ptr<OptimalControlProblem>, OptimizationProblemBase>(
+            m, "OptimalControlProblem");
+    obj.def(py::init<>());
+
+
+
+    BuildNewLinkIterface(obj);
+    BuildOldLinkIterface(obj);
+
+
+    //////////////////////////////////////////////////////////////////////////////
+
+    obj.def("addLinkParamEqualCon",
+        py::overload_cast<VectorFunctionalX, std::vector<VectorXi>>(
+            &OptimalControlProblem::addLinkParamEqualCon),
+        OptimalControlProblem_addLinkParamEqualCon1);
+    obj.def("addLinkParamEqualCon",
+        py::overload_cast<VectorFunctionalX, VectorXi>(&OptimalControlProblem::addLinkParamEqualCon),
+        OptimalControlProblem_addLinkParamEqualCon2);
+    obj.def("addLinkParamInequalCon",
+        py::overload_cast<VectorFunctionalX, std::vector<VectorXi>>(
+            &OptimalControlProblem::addLinkParamInequalCon),
+        OptimalControlProblem_addLinkParamInequalCon1);
+    obj.def("addLinkParamInequalCon",
+        py::overload_cast<VectorFunctionalX, VectorXi>(&OptimalControlProblem::addLinkParamInequalCon),
+        OptimalControlProblem_addLinkParamInequalCon2);
+    obj.def("addLinkParamObjective",
+        py::overload_cast<ScalarFunctionalX, std::vector<VectorXi>>(
+            &OptimalControlProblem::addLinkParamObjective),
+        OptimalControlProblem_addLinkParamObjective1);
+    obj.def("addLinkParamObjective",
+        py::overload_cast<ScalarFunctionalX, VectorXi>(&OptimalControlProblem::addLinkParamObjective),
+        OptimalControlProblem_addLinkParamObjective2);
+
+    //////////////////////////////////////////////////////////////////////////////
+
+    obj.def("removeLinkEqualCon",
+        &OptimalControlProblem::removeLinkEqualCon,
+        OptimalControlProblem_removeLinkEqualCon);
+    obj.def("removeLinkInequalCon",
+        &OptimalControlProblem::removeLinkInequalCon,
+        OptimalControlProblem_removeLinkEqualCon);
+    obj.def("removeLinkObjective",
+        &OptimalControlProblem::removeLinkObjective,
+        OptimalControlProblem_removeLinkObjective);
+
+    obj.def("addPhase",
+        py::overload_cast<PhasePtr>(&OptimalControlProblem::addPhase),
+        OptimalControlProblem_addPhase);
+
+
+    obj.def("addPhases", &OptimalControlProblem::addPhases);
+
+    obj.def("getPhaseNum", py::overload_cast<PhasePtr>(&OptimalControlProblem::getPhaseNum));
+
+
+    obj.def("removePhase", &OptimalControlProblem::removePhase, OptimalControlProblem_removePhase);
+    obj.def("Phase", &OptimalControlProblem::Phase, OptimalControlProblem_Phase);
+
+
+    obj.def("setLinkParams", py::overload_cast<VectorXd, VectorXd>(&OptimalControlProblem::setLinkParams));
+    obj.def("setLinkParams", py::overload_cast<VectorXd>(&OptimalControlProblem::setLinkParams), OptimalControlProblem_setLinkParams);
+
+    obj.def("addLinkParamVgroups",
+        py::overload_cast<std::map<std::string, Eigen::VectorXi>>(&OptimalControlProblem::addLinkParamVgroups));
+    obj.def("setLinkParamVgroups",
+        py::overload_cast<std::map<std::string, Eigen::VectorXi>>(&OptimalControlProblem::setLinkParamVgroups));
+    obj.def("addLinkParamVgroup",
+        py::overload_cast<Eigen::VectorXi, std::string>(&OptimalControlProblem::addLinkParamVgroup));
+    obj.def("addLinkParamVgroup",
+        py::overload_cast<int, std::string>(&OptimalControlProblem::addLinkParamVgroup));
+
+
+    obj.def(
+        "returnLinkParams", &OptimalControlProblem::returnLinkParams, OptimalControlProblem_returnLinkParams);
+
+
+    obj.def("transcribe",
+        py::overload_cast<bool, bool>(&OptimalControlProblem::transcribe),
+        OptimalControlProblem_transcribe);
+
+    obj.def_readonly("Phases", &OptimalControlProblem::phases, OptimalControlProblem_Phases);
+
+
+    ///////////////////////
+    obj.def("returnLinkEqualConVals", &OptimalControlProblem::returnLinkEqualConVals);
+    obj.def("returnLinkEqualConLmults", &OptimalControlProblem::returnLinkEqualConLmults);
+
+    obj.def("returnLinkInequalConVals", &OptimalControlProblem::returnLinkInequalConVals);
+    obj.def("returnLinkInequalConLmults", &OptimalControlProblem::returnLinkInequalConLmults);
+
+    obj.def("returnLinkEqualConScales", &OptimalControlProblem::returnLinkEqualConScales);
+    obj.def("returnLinkInequalConScales", &OptimalControlProblem::returnLinkInequalConScales);
+    obj.def("returnLinkObjectiveScales", &OptimalControlProblem::returnLinkObjectiveScales);
+
+
+    ///////////////////////
+
+    obj.def_readwrite("AutoScaling", &OptimalControlProblem::AutoScaling);
+    obj.def_readwrite("SyncObjectiveScales", &OptimalControlProblem::SyncObjectiveScales);
+
+
+    obj.def_readwrite("AdaptiveMesh", &OptimalControlProblem::AdaptiveMesh);
+    obj.def_readwrite("PrintMeshInfo", &OptimalControlProblem::PrintMeshInfo);
+    obj.def_readwrite("MaxMeshIters", &OptimalControlProblem::MaxMeshIters);
+    obj.def_readonly("MeshConverged", &OptimalControlProblem::MeshConverged);
+    obj.def_readwrite("SolveOnlyFirst", &OptimalControlProblem::SolveOnlyFirst);
+
+    obj.def_readwrite("MeshAbortFlag", &OptimalControlProblem::MeshAbortFlag);
+
+
+    obj.def("setAdaptiveMesh",
+        &OptimalControlProblem::setAdaptiveMesh,
+        py::arg("AdaptiveMesh") = true,
+        py::arg("ApplyToPhases") = true);
+    obj.def("setAutoScaling",
+        &OptimalControlProblem::setAutoScaling,
+        py::arg("AutoScaling") = true,
+        py::arg("ApplyToPhases") = true);
+
+
+    obj.def("setMeshTol", &OptimalControlProblem::setMeshTol);
+    obj.def("setMeshRedFactor", &OptimalControlProblem::setMeshRedFactor);
+    obj.def("setMeshIncFactor", &OptimalControlProblem::setMeshIncFactor);
+    obj.def("setMeshErrFactor", &OptimalControlProblem::setMeshErrFactor);
+    obj.def("setMaxMeshIters", &OptimalControlProblem::setMaxMeshIters);
+    obj.def("setMinSegments", &OptimalControlProblem::setMinSegments);
+    obj.def("setMaxSegments", &OptimalControlProblem::setMaxSegments);
+    obj.def("setMeshErrorCriteria", &OptimalControlProblem::setMeshErrorCriteria);
+    obj.def("setMeshErrorEstimator", &OptimalControlProblem::setMeshErrorEstimator);
+}
+
+void ASSET::OptimalControlProblem::BuildNewLinkIterface(py::class_<OptimalControlProblem, std::shared_ptr<OptimalControlProblem>, OptimizationProblemBase>& obj)
+{
+
+    //////////// EqualCons////////////////////////////////////////
+    {
+        obj.def("addLinkEqualCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            std::vector<PhasePack>,
+            VarIndexType,
+            ScaleType>(&OptimalControlProblem::addLinkEqualCon),
+            py::arg("func"),
+            py::arg("phasepack"),
+            py::arg("linkparams") = VectorXi(),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+
+
+        obj.def("addLinkEqualCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            VarIndexType,
+            VarIndexType,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            VarIndexType,
+            VarIndexType,
+            VarIndexType,
+            ScaleType> (&OptimalControlProblem::addLinkEqualCon),
+            py::arg("func"),
+            py::arg("phase0"),
+            py::arg("reg0"),
+            py::arg("XtUVars0"),
+            py::arg("OPVars0"),
+            py::arg("SPVars0"),
+            py::arg("phase1"),
+            py::arg("reg1"),
+            py::arg("XtUVars1"),
+            py::arg("OPVars1"),
+            py::arg("SPVars1"),
+            py::arg("linkparams") = VectorXi(),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+
+
+        obj.def("addLinkEqualCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            VarIndexType,
+            ScaleType>(&OptimalControlProblem::addLinkEqualCon),
+            py::arg("func"),
+            py::arg("phase0"),
+            py::arg("reg0"),
+            py::arg("v0"),
+            py::arg("phase1"),
+            py::arg("reg1"),
+            py::arg("v1"),
+            py::arg("linkparams") = VectorXi(),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+        obj.def("addLinkParamEqualCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            std::vector<VectorXi>, ScaleType>(
+                &OptimalControlProblem::addLinkParamEqualCon),
+            py::arg("func"),
+            py::arg("LinkParms"),
+            py::arg("AutoScale") = std::string("auto")
+        );
+        obj.def("addLinkParamEqualCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            VectorXi,
+            ScaleType>(
+                &OptimalControlProblem::addLinkParamEqualCon),
+            py::arg("func"),
+            py::arg("LinkParms"),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+
+        obj.def("addForwardLinkEqualCon",
+            py::overload_cast<
+            PhaseRefType,
+            PhaseRefType,
+            VarIndexType,
+            ScaleType>(&OptimalControlProblem::addForwardLinkEqualCon),
+            py::arg("phase0"),
+            py::arg("phase1"),
+            py::arg("vars"),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+        obj.def("addParamLinkEqualCon",
+            py::overload_cast<
+            PhaseRefType,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            ScaleType>(&OptimalControlProblem::addParamLinkEqualCon),
+            py::arg("phase0"),
+            py::arg("phase1"),
+            py::arg("reg0"),
+            py::arg("vars"),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+
+        obj.def("addDirectLinkEqualCon",
+            py::overload_cast<
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            ScaleType>(&OptimalControlProblem::addDirectLinkEqualCon),
+            py::arg("phase0"),
+            py::arg("reg0"),
+            py::arg("v0"),
+            py::arg("phase1"),
+            py::arg("reg1"),
+            py::arg("v1"),
+            py::arg("AutoScale") = std::string("auto")
+        );
+    }
+
+
+    //////////// InequalCons////////////////////////////////////////
+    {
+        obj.def("addLinkInequalCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            std::vector<PhasePack>,
+            VarIndexType,
+            ScaleType>(&OptimalControlProblem::addLinkInequalCon),
+            py::arg("func"),
+            py::arg("phasepack"),
+            py::arg("linkparams") = VectorXi(),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            VarIndexType,
+            VarIndexType,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            VarIndexType,
+            VarIndexType,
+            VarIndexType,
+            ScaleType> (&OptimalControlProblem::addLinkInequalCon),
+            py::arg("func"),
+            py::arg("phase0"),
+            py::arg("reg0"),
+            py::arg("XtUVars0"),
+            py::arg("OPVars0"),
+            py::arg("SPVars0"),
+            py::arg("phase1"),
+            py::arg("reg1"),
+            py::arg("XtUVars1"),
+            py::arg("OPVars1"),
+            py::arg("SPVars1"),
+            py::arg("linkparams") = VectorXi(),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            VarIndexType,
+            ScaleType>(&OptimalControlProblem::addLinkInequalCon),
+            py::arg("func"),
+            py::arg("phase0"),
+            py::arg("reg0"),
+            py::arg("v0"),
+            py::arg("phase1"),
+            py::arg("reg1"),
+            py::arg("v1"),
+            py::arg("linkparams") = VectorXi(),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+        obj.def("addLinkParamInequalCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            std::vector<VectorXi>, ScaleType>(
+                &OptimalControlProblem::addLinkParamInequalCon),
+            py::arg("func"),
+            py::arg("LinkParms"),
+            py::arg("AutoScale") = std::string("auto")
+        );
+        obj.def("addLinkParamInequalCon",
+            py::overload_cast<
+            VectorFunctionalX,
+            VectorXi,
+            ScaleType>(
+                &OptimalControlProblem::addLinkParamInequalCon),
+            py::arg("func"),
+            py::arg("LinkParms"),
+            py::arg("AutoScale") = std::string("auto")
+        );
+    }
+    //////////// Objectives ////////////////////////////////////////
+    {
+        obj.def("addLinkObjective",
+            py::overload_cast<
+            ScalarFunctionalX,
+            std::vector<PhasePack>,
+            VarIndexType,
+            ScaleType>(&OptimalControlProblem::addLinkObjective),
+            py::arg("func"),
+            py::arg("phasepack"),
+            py::arg("linkparams") = VectorXi(),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+
+
+        obj.def("addLinkObjective",
+            py::overload_cast<
+            ScalarFunctionalX,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            VarIndexType,
+            VarIndexType,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            VarIndexType,
+            VarIndexType,
+            VarIndexType,
+            ScaleType> (&OptimalControlProblem::addLinkObjective),
+            py::arg("func"),
+            py::arg("phase0"),
+            py::arg("reg0"),
+            py::arg("XtUVars0"),
+            py::arg("OPVars0"),
+            py::arg("SPVars0"),
+            py::arg("phase1"),
+            py::arg("reg1"),
+            py::arg("XtUVars1"),
+            py::arg("OPVars1"),
+            py::arg("SPVars1"),
+            py::arg("linkparams") = VectorXi(),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+
+
+        obj.def("addLinkObjective",
+            py::overload_cast<
+            ScalarFunctionalX,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            PhaseRefType,
+            RegionType,
+            VarIndexType,
+            VarIndexType,
+            ScaleType>(&OptimalControlProblem::addLinkObjective),
+            py::arg("func"),
+            py::arg("phase0"),
+            py::arg("reg0"),
+            py::arg("v0"),
+            py::arg("phase1"),
+            py::arg("reg1"),
+            py::arg("v1"),
+            py::arg("linkparams") = VectorXi(),
+            py::arg("AutoScale") = std::string("auto")
+        );
+
+        obj.def("addLinkParamObjective",
+            py::overload_cast<
+            ScalarFunctionalX,
+            std::vector<VectorXi>, ScaleType>(
+                &OptimalControlProblem::addLinkParamObjective),
+            py::arg("func"),
+            py::arg("LinkParms"),
+            py::arg("AutoScale") = std::string("auto")
+        );
+        obj.def("addLinkParamObjective",
+            py::overload_cast<
+            ScalarFunctionalX,
+            VectorXi,
+            ScaleType>(
+                &OptimalControlProblem::addLinkParamObjective),
+            py::arg("func"),
+            py::arg("LinkParms"),
+            py::arg("AutoScale") = std::string("auto")
+        );
+    }
+
+
+}
+
+void ASSET::OptimalControlProblem::BuildOldLinkIterface(py::class_<OptimalControlProblem, std::shared_ptr<OptimalControlProblem>, OptimizationProblemBase>& obj)
+{
+    using namespace doc;
+
+    {
+        ////////////////// Legacy EqualCons//////////
+        obj.def("addLinkEqualCon",
+            py::overload_cast<LinkConstraint>(&OptimalControlProblem::addLinkEqualCon),
+            OptimalControlProblem_addLinkEqualCon1);
+
+
+
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            RegVec,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            std::vector<std::string>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
+
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            LinkFlags,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            std::string,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
+
+        /// ///
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            RegVec,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            std::vector<std::string>,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
+
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            LinkFlags,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            std::string,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon));
+
+        ////////////////////////////
+
+
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            RegVec,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon),
+            OptimalControlProblem_addLinkEqualCon2);
+
+
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            RegVec,
+            std::vector<std::vector<std::shared_ptr<ODEPhaseBase>>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkEqualCon),
+            OptimalControlProblem_addLinkEqualCon2);
+
+
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX, LinkFlags, std::vector<VectorXi>, VectorXi>(
+                &OptimalControlProblem::addLinkEqualCon),
+            OptimalControlProblem_addLinkEqualCon2);
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX, LinkFlags, std::vector<std::vector<PhasePtr>>, VectorXi>(
+                &OptimalControlProblem::addLinkEqualCon),
+            OptimalControlProblem_addLinkEqualCon2);
+
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX, std::string, std::vector<VectorXi>, VectorXi>(
+                &OptimalControlProblem::addLinkEqualCon),
+            OptimalControlProblem_addLinkEqualCon2);
+        obj.def("addLinkEqualCon",
+            py::overload_cast<VectorFunctionalX, std::string, std::vector<std::vector<PhasePtr>>, VectorXi>(
+                &OptimalControlProblem::addLinkEqualCon),
+            OptimalControlProblem_addLinkEqualCon2);
+
+
+        obj.def("addForwardLinkEqualCon",
+            py::overload_cast<int, int, VectorXi>(&OptimalControlProblem::addForwardLinkEqualCon),
+            OptimalControlProblem_addForwardLinkEqualCon);
+
+
+        obj.def("addForwardLinkEqualCon",
+            py::overload_cast<PhasePtr, PhasePtr, VectorXi>(&OptimalControlProblem::addForwardLinkEqualCon),
+            OptimalControlProblem_addForwardLinkEqualCon);
+
+
+        obj.def("addDirectLinkEqualCon",
+            py::overload_cast<LinkFlags, int, VectorXi, int, VectorXi>(
+                &OptimalControlProblem::addDirectLinkEqualCon),
+            OptimalControlProblem_addDirectLinkEqualCon);
+
+        obj.def(
+            "addDirectLinkEqualCon",
+            py::overload_cast<VectorFunctionalX, int, PhaseRegionFlags, VectorXi, int, PhaseRegionFlags, VectorXi>(
+                &OptimalControlProblem::addDirectLinkEqualCon),
+            OptimalControlProblem_addDirectLinkEqualCon);
+
+
+
+        obj.def("addDirectLinkEqualCon",
+            py::overload_cast<VectorFunctionalX,
+            PhasePtr,
+            PhaseRegionFlags,
+            VectorXi,
+            PhasePtr,
+            PhaseRegionFlags,
+            VectorXi>(&OptimalControlProblem::addDirectLinkEqualCon),
+            OptimalControlProblem_addDirectLinkEqualCon);
+
+
+
+
+        //
+        obj.def("addDirectLinkEqualCon",
+            py::overload_cast<VectorFunctionalX, int, std::string, VectorXi, int, std::string, VectorXi>(
+                &OptimalControlProblem::addDirectLinkEqualCon),
+            OptimalControlProblem_addDirectLinkEqualCon);
+
+
+
+        obj.def(
+            "addDirectLinkEqualCon",
+            py::overload_cast<VectorFunctionalX, PhasePtr, std::string, VectorXi, PhasePtr, std::string, VectorXi>(
+                &OptimalControlProblem::addDirectLinkEqualCon),
+            OptimalControlProblem_addDirectLinkEqualCon);
+
+
+
+
+    }
+
+    //////////////////Legacy  InequalCons//////////
+    {
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<LinkConstraint>(&OptimalControlProblem::addLinkInequalCon),
+            OptimalControlProblem_addLinkInequalCon);
+
+        ////////////////////////////
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            RegVec,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            std::vector<std::string>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            LinkFlags,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            std::string,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+
+        /// ///
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            RegVec,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            std::vector<std::string>,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            LinkFlags,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            std::string,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+
+        ////////////////////////////
+
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX, LinkFlags, std::vector<VectorXi>, VectorXi>(
+                &OptimalControlProblem::addLinkInequalCon),
+            OptimalControlProblem_addLinkInequalCon);
+
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX, LinkFlags, std::vector<std::vector<PhasePtr>>, VectorXi>(
+                &OptimalControlProblem::addLinkInequalCon),
+            OptimalControlProblem_addLinkInequalCon);
+
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX, std::string, std::vector<VectorXi>, VectorXi>(
+                &OptimalControlProblem::addLinkInequalCon),
+            OptimalControlProblem_addLinkInequalCon);
+
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX, std::string, std::vector<std::vector<PhasePtr>>, VectorXi>(
+                &OptimalControlProblem::addLinkInequalCon),
+            OptimalControlProblem_addLinkInequalCon);
+
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            RegVec,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+
+
+        obj.def("addLinkInequalCon",
+            py::overload_cast<VectorFunctionalX,
+            RegVec,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkInequalCon));
+
+
+        ////////////////////////////////////////
+        obj.def("addLinkObjective",
+            py::overload_cast<LinkObjective>(&OptimalControlProblem::addLinkObjective),
+            OptimalControlProblem_addLinkObjective);
+
+    }
+
+    {
+        //////////////////Legacy LinkObjectives//////////
+
+
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            RegVec,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            std::vector<std::string>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
+
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            LinkFlags,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            std::string,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
+
+        /// ///
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            RegVec,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            std::vector<std::string>,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
+
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            LinkFlags,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            std::string,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective));
+
+        ////////////////////////////
+
+
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX, LinkFlags, std::vector<VectorXi>, VectorXi>(
+                &OptimalControlProblem::addLinkObjective),
+            OptimalControlProblem_addLinkObjective);
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX, LinkFlags, std::vector<std::vector<PhasePtr>>, VectorXi>(
+                &OptimalControlProblem::addLinkObjective),
+            OptimalControlProblem_addLinkObjective);
+
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX, std::string, std::vector<VectorXi>, VectorXi>(
+                &OptimalControlProblem::addLinkObjective),
+            OptimalControlProblem_addLinkObjective);
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX, std::string, std::vector<std::vector<PhasePtr>>, VectorXi>(
+                &OptimalControlProblem::addLinkObjective),
+            OptimalControlProblem_addLinkObjective);
+
+
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            RegVec,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective),
+            OptimalControlProblem_addLinkObjective);
+
+
+        obj.def("addLinkObjective",
+            py::overload_cast<ScalarFunctionalX,
+            RegVec,
+            std::vector<std::vector<PhasePtr>>,
+            std::vector<VectorXi>,
+            std::vector<VectorXi>>(&OptimalControlProblem::addLinkObjective),
+            OptimalControlProblem_addLinkObjective);
+
+    }
+
+}
diff --git a/src/OptimalControl/StateFunction.h b/src/OptimalControl/StateFunction.h
index d356b2f8..d1bb9a50 100644
--- a/src/OptimalControl/StateFunction.h
+++ b/src/OptimalControl/StateFunction.h
@@ -2,6 +2,7 @@
 
 #include "OptimalControlFlags.h"
 #include "pch.h"
+#include "InterfaceTypes.h"
 
 namespace ASSET {
 
@@ -15,6 +16,11 @@ namespace ASSET {
 
     Eigen::VectorXi EXTVars;  // dirty i know
 
+    std::string ScaleMode ="auto"; // auto,custom,none
+    bool ScalesSet = false;
+    Eigen::VectorXd OutputScales;
+    
+
     int StorageIndex = 0;
     int PhaseLocalIndex = 0;
     int GlobalIndex = 0;
@@ -26,9 +32,23 @@ namespace ASSET {
       this->XtUVars = xtuv;
       this->OPVars = opv;
       this->SPVars = spv;
+      this->OutputScales = Eigen::VectorXd::Ones(this->Func.ORows());
+    }
+
+    StateFunction(
+        FuncType f, PhaseRegionFlags Reg, Eigen::VectorXi xtuv, Eigen::VectorXi opv, Eigen::VectorXi spv, ScaleType scale_t):
+        StateFunction(f,Reg,xtuv,opv,spv){
+       
+        auto [ScaleMode, ScalesSet, OutputScales] = get_scale_info(this->Func.ORows(), scale_t);
+        this->OutputScales = OutputScales;
+        this->ScaleMode = ScaleMode;
+        this->ScalesSet = ScalesSet;
     }
+
+
     StateFunction(FuncType f, PhaseRegionFlags Reg, Eigen::VectorXi xtuv) {
       this->Func = f;
+      this->OutputScales = Eigen::VectorXd::Ones(this->Func.ORows());
 
       switch (Reg) {
         case PhaseRegionFlags::ODEParams: {
@@ -54,11 +74,24 @@ namespace ASSET {
         }
       }
     }
+    StateFunction(FuncType f, PhaseRegionFlags Reg, Eigen::VectorXi xtuv, ScaleType scale_t):
+        StateFunction(f,Reg,xtuv){
+        auto [ScaleMode, ScalesSet, OutputScales] = get_scale_info(this->Func.ORows(), scale_t);
+        this->OutputScales = OutputScales;
+        this->ScaleMode = ScaleMode;
+        this->ScalesSet = ScalesSet;
+    }
+
+
+
+
     StateFunction(
         FuncType f, PhaseRegionFlags Reg, Eigen::VectorXi xtuv, PhaseRegionFlags ParReg, Eigen::VectorXi pv) {
       this->Func = f;
       this->RegionFlag = Reg;
       this->XtUVars = xtuv;
+      this->OutputScales = Eigen::VectorXd::Ones(this->Func.ORows());
+
       switch (ParReg) {
         case PhaseRegionFlags::ODEParams: {
           this->OPVars = pv;
diff --git a/src/Solvers/PardisoInterface.h b/src/Solvers/PardisoInterface.h
index f0d2a8d7..bde412f9 100644
--- a/src/Solvers/PardisoInterface.h
+++ b/src/Solvers/PardisoInterface.h
@@ -5,9 +5,18 @@
 
   #include <Eigen/src/Core/util/DisableStupidWarnings.h>
   #include <mkl_pardiso.h>
-
   #include <Eigen/SparseCore>
 
+/*
+The classes in this file are directly based on the PardisoSupport module from Eigen 3.4 and 
+are subject to Eigen's MPL2 license, which can be found in the notices folder of the GitHub repository.
+Changes include the addition of several member functions to access internal PARDISO parameters, and additional
+methods to perform phases of the factorization without making unnecessary copies of the input matrix.
+Note that the MPL2 license is only applied to this particular file and not the rest of the project,
+as per the MPL2 license.
+ 
+*/
+
 namespace Eigen {
 
   template<typename _MatrixType>
diff --git a/src/VectorFunctions/ASSET_VectorFunctions.cpp b/src/VectorFunctions/ASSET_VectorFunctions.cpp
index a7fc5778..4e81099d 100644
--- a/src/VectorFunctions/ASSET_VectorFunctions.cpp
+++ b/src/VectorFunctions/ASSET_VectorFunctions.cpp
@@ -6,6 +6,8 @@
 #include "CommonFunctions/InterpTable4D.h"
 
 #include "Utils/fmtlib.h"
+#include "CommonFunctions/IOScaled.h"
+
 
 namespace ASSET {}  // namespace ASSET
 
@@ -46,6 +48,9 @@ void ASSET::VectorFunctionBuild(FunctionRegistry& reg, py::module& m) {
   reg.Build_Register<Constant<-1, 1>>(mod, "ConstantScalar");
 
 
+  reg.Build_Register<IOScaled<Gen>>(mod, "IOScaled");
+
+
   InterpTable1DBuild(mod);
   InterpTable2DBuild(mod);
   InterpTable3DBuild(mod);
diff --git a/src/VectorFunctions/CommonFunctions/IOScaled.h b/src/VectorFunctions/CommonFunctions/IOScaled.h
new file mode 100644
index 00000000..ce2d7504
--- /dev/null
+++ b/src/VectorFunctions/CommonFunctions/IOScaled.h
@@ -0,0 +1,159 @@
+#pragma once
+
+#include "VectorFunction.h"
+
+namespace ASSET {
+
+  template<class Func>
+  struct IOScaled : VectorFunction<IOScaled<Func>, Func::IRC, Func::ORC, Analytic> {
+    using Base = VectorFunction<IOScaled<Func>, Func::IRC, Func::ORC, Analytic> ;
+    using Base::compute;
+    DENSE_FUNCTION_BASE_TYPES(Base);
+    Func func;
+    using INPUT_DOMAIN = typename Func::INPUT_DOMAIN;
+    static const bool IsLinearFunction = Func::IsLinearFunction;
+    static const bool IsVectorizable = Func::IsVectorizable;
+
+    Input<double> input_scales;
+    Output<double> output_scales;
+
+    IOScaled() { }
+
+    IOScaled(Func f, const Input<double>& input_scales, const Output<double>& output_scales)
+        : func(std::move(f)) {
+      this->setIORows(this->func.IRows(), this->func.ORows());
+      this->set_input_domain(this->IRows(), {this->func.input_domain()});
+
+      this->input_scales = input_scales;
+      this->output_scales = output_scales;
+
+      this->EnableVectorization = this->func.EnableVectorization;
+    }
+    
+    static void Build(py::module& m, const char* name) {
+      auto obj = py::class_<IOScaled<Func>>(m, name);
+      obj.def(py::init<Func , const Input<double> & , const Output<double> & >());
+      Base::DenseBaseBuild(obj);
+    }
+
+
+    template<class InType, class OutType>
+    inline void compute_impl(ConstVectorBaseRef<InType> x, ConstVectorBaseRef<OutType> fx_) const {
+      typedef typename InType::Scalar Scalar;
+      VectorBaseRef<OutType> fx = fx_.const_cast_derived();
+
+      auto Impl = [&](auto& x_scaled) {
+            for (int i = 0; i < this->IRows(); i++) {
+                 x_scaled[i] = this->input_scales[i] * x[i];
+            }
+
+        this->func.compute(x_scaled, fx);
+        
+        for (int i = 0; i < this->ORows(); i++) {
+                 fx[i] *= this->output_scales[i];
+        }
+      };
+
+
+      MemoryManager::allocate_run(this->IRows(),
+                                  Impl, TempSpec<Input<Scalar>>(this->IRows(), 1));
+
+
+    }
+    template<class InType, class OutType, class JacType>
+    inline void compute_jacobian_impl(ConstVectorBaseRef<InType> x,
+                                      ConstVectorBaseRef<OutType> fx_,
+                                      ConstMatrixBaseRef<JacType> jx_) const {
+
+
+     typedef typename InType::Scalar Scalar;
+     VectorBaseRef<OutType> fx = fx_.const_cast_derived();
+     MatrixBaseRef<JacType> jx = jx_.const_cast_derived();
+
+
+      auto Impl = [&](auto& x_scaled) {
+        for (int i = 0; i < this->IRows(); i++) {
+          x_scaled[i] = this->input_scales[i] * x[i];
+        }
+
+      this->func.compute_jacobian(x_scaled, fx, jx);
+
+        for (int i = 0; i < this->ORows(); i++) {
+          fx[i] *= this->output_scales[i];
+          jx.row(i) *= Scalar(this->output_scales[i]);
+        }
+        for (int i = 0; i < this->IRows(); i++) {
+          jx.col(i) *= Scalar(this->input_scales[i]);
+        }
+
+      };
+
+      MemoryManager::allocate_run(this->IRows(), Impl, TempSpec<Input<Scalar>>(this->IRows(), 1));
+
+    }
+
+
+    template<class InType,
+             class OutType,
+             class JacType,
+             class AdjGradType,
+             class AdjHessType,
+             class AdjVarType>
+    inline void compute_jacobian_adjointgradient_adjointhessian_impl(
+        ConstVectorBaseRef<InType> x,
+        ConstVectorBaseRef<OutType> fx_,
+        ConstMatrixBaseRef<JacType> jx_,
+        ConstVectorBaseRef<AdjGradType> adjgrad_,
+        ConstMatrixBaseRef<AdjHessType> adjhess_,
+        ConstVectorBaseRef<AdjVarType> adjvars) const {
+
+        typedef typename InType::Scalar Scalar;
+        VectorBaseRef<OutType> fx = fx_.const_cast_derived();
+        MatrixBaseRef<JacType> jx = jx_.const_cast_derived();
+        VectorBaseRef<AdjGradType> adjgrad = adjgrad_.const_cast_derived();
+        MatrixBaseRef<AdjHessType> adjhess = adjhess_.const_cast_derived();
+
+
+        auto Impl = [&](auto& x_scaled,auto & l_scaled) {
+        for (int i = 0; i < this->IRows(); i++) {
+          x_scaled[i] = this->input_scales[i] * x[i];
+        }
+        for (int i = 0; i < this->ORows(); i++) {
+          l_scaled[i] = this->output_scales[i] * adjvars[i];
+        }
+        this->func.compute_jacobian_adjointgradient_adjointhessian(
+            x_scaled, fx, jx, adjgrad, adjhess, l_scaled);
+
+
+
+        for (int i = 0; i < this->ORows(); i++) {
+          fx[i] *= this->output_scales[i];
+          jx.row(i) *= Scalar(this->output_scales[i]);
+        }
+        for (int i = 0; i < this->IRows(); i++) {
+          jx.col(i) *= Scalar(this->input_scales[i]);
+          adjhess.col(i) *= Scalar(this->input_scales[i]);
+          adjgrad[i] *=this->input_scales[i];
+        }
+
+        for (int i = 0; i < this->IRows(); i++) {
+            adjhess.row(i) *= Scalar(this->input_scales[i]);
+        }
+
+      };
+
+      MemoryManager::allocate_run(this->IRows(),
+                                    Impl,
+                                    TempSpec<Input<Scalar>>(this->IRows(), 1),
+                                    TempSpec<Output<Scalar>>(this->ORows(), 1));
+
+
+
+
+    }
+
+   
+    
+  };
+
+}  // namespace ASSET
\ No newline at end of file
diff --git a/src/VectorFunctions/DenseFunctionBase.h b/src/VectorFunctions/DenseFunctionBase.h
index 0bfa6496..ba291f81 100644
--- a/src/VectorFunctions/DenseFunctionBase.h
+++ b/src/VectorFunctions/DenseFunctionBase.h
@@ -1060,11 +1060,11 @@ namespace ASSET {
 
     /*
      * Counts the number of non-zero elements in the lower triangle of the hessian and
-     * the jacobian of derived and represents the number of elements that will be explicilty scattered
+     * the Jacobian of derived and represents the number of elements that will be explicilty scattered
      * into the KKT matrix of an optimization problem on each function call.
      * For dense functions, in almost all cases they are all non-zero, or are assumed non-zero even if
      * they are not. However, this method will account for explicit non-zeros if the HessianElemIsNonZero ex.
-     * is explicity overloaded by derived. The sum of the non-zeros here is used when interfacing whith the
+     * is explicitly overloaded by derived. The sum of the non-zeros here is used when interfacing whith the
      * optimizer throutght the wrapper classes ConstraintFunction and ObjectiveFunction.
      */
     int numKKTEles(bool dojac, bool dohess) const {
@@ -1088,7 +1088,7 @@ namespace ASSET {
     }
 
     /*
-     * This function is responsibe for determing the target locations of the jacobian and hessian elements
+     * This function is responsible for determining the target locations of the jacobian and hessian elements
      * of derived inside of the KKT matrix of an optimization problem when it is used as a constraint. Meta
      * data containing the locations of the input variables and constraint row for each call of derived are
      * held in the Solver indexing data struct.
@@ -1539,6 +1539,10 @@ namespace ASSET {
       obj.def("IRows", &Derived::IRows);
       obj.def("ORows", &Derived::ORows);
       obj.def("name", &Derived::name);
+
+      obj.def("input_domain", &Derived::input_domain);
+      obj.def("is_linear", &Derived::is_linear);
+
       obj.def("compute", [](const Derived& func, ConstEigenRef<Input<double>> x) {
         if (x.size() != func.IRows())
           throw std::invalid_argument("Incorrectly sized input to function");
diff --git a/src/VectorFunctions/IndexingData.h b/src/VectorFunctions/IndexingData.h
index 303d9fbe..88842377 100644
--- a/src/VectorFunctions/IndexingData.h
+++ b/src/VectorFunctions/IndexingData.h
@@ -46,19 +46,19 @@ namespace ASSET {
     MatrixXi Vindex;
 
     /// <summary>
-    /// Matrix whose columns containt the ordered constraint output indices for the correspoding
+    /// Matrix whose columns constraint the ordered constraint output indices for the corresponding
     /// column in Vindex. This matrix is empy for objective functions.
     /// </summary>
     MatrixXi Cindex;
 
     /// <summary>
-    /// Each element indicates whether the correspoding indices in Vindex are sorted
+    /// Each element indicates whether the corresponding indices in Vindex are sorted
     /// and contigous (ie: 10,11,12...)
     /// </summary>
     Eigen::Matrix<ParsedIOFlags, -1, 1> VindexContinuity;
 
     /// <summary>
-    /// Each element indicates whether the correspoding indices in Cindex are sorted
+    /// Each element indicates whether the corresponding indices in Cindex are sorted
     /// and contigous (ie: 10,11,12...)
     /// </summary>
     Eigen::Matrix<ParsedIOFlags, -1, 1> CindexContinuity;
diff --git a/src/VectorFunctions/VectorFunctionTypeErasure/GenericFunction.h b/src/VectorFunctions/VectorFunctionTypeErasure/GenericFunction.h
index f8c4fdae..e6814686 100644
--- a/src/VectorFunctions/VectorFunctionTypeErasure/GenericFunction.h
+++ b/src/VectorFunctions/VectorFunctionTypeErasure/GenericFunction.h
@@ -396,8 +396,7 @@ namespace ASSET {
         obj.def(py::init(&Derived::PyCopy<GenericFunction<IR, 1>>));
       }
 
-      obj.def("input_domain", &Derived::input_domain);
-      obj.def("is_linear", &Derived::is_linear);
+     
       obj.def("SuperTest", &Derived::SuperTest);
       obj.def("SpeedTest", &Derived::SpeedTest);
 


Test patch:
diff --git a/asset_asrl/test/test_AutoScaling/__init__.py b/asset_asrl/test/test_AutoScaling/__init__.py
new file mode 100644
index 00000000..7398c240
--- /dev/null
+++ b/asset_asrl/test/test_AutoScaling/__init__.py
@@ -0,0 +1,3 @@
+import unittest
+if __name__ == '__main__':
+    unittest.main()
diff --git a/asset_asrl/test/test_AutoScaling/test_ObjScaling.py b/asset_asrl/test/test_AutoScaling/test_ObjScaling.py
new file mode 100644
index 00000000..2b7e84e4
--- /dev/null
+++ b/asset_asrl/test/test_AutoScaling/test_ObjScaling.py
@@ -0,0 +1,254 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import asset_asrl as ast
+import asset_asrl.VectorFunctions as vf
+import asset_asrl.OptimalControl as oc
+from asset_asrl.VectorFunctions import Arguments as Args
+import unittest
+
+
+
+class ODE(oc.ODEBase):
+    def __init__(self):
+        
+        XVars =1
+        UVars =1
+        
+        args = oc.ODEArguments(XVars,UVars)
+        x = args.XVar(0)
+        u = args.UVar(0)
+        xdot = .5*x + u
+        super().__init__(xdot,XVars,UVars)
+
+    class obj(vf.ScalarFunction):
+        def __init__(self):
+            x,u = Args(2).tolist()
+            obj = u*u + x*u + 1.25*x**2
+            super().__init__(obj)
+            
+###############################################################################
+class test_ObjScaling(unittest.TestCase):
+    '''
+    Testing to see whether we get same solution with and without autoscaling
+    '''
+    @classmethod
+    def setUpClass(self):
+        
+        self.FinalState = 0.3185865574270634
+        self.MaxStateError = .001
+        
+        
+        
+        
+    def problem_impl(self,tmode,cmode,mtol):
+        
+        ode = ODE()
+        
+       
+        nsegs = 20
+        
+        
+        iscale = np.pi
+        vscale = np.e
+        
+        xstar = 2.11
+        tstar = 3.3
+        ustar = 5.1
+        pstar = 8.2
+        
+        
+        x0 = 1.0
+        t0 = 0.0
+        tf = 1.0
+        u0 = .4
+        
+        TrajIG = [[x0,t,u0] for t in np.linspace(t0,tf,100)]
+        
+        
+        #### Ground Truth, No AutoScaling #################
+        phase = ode.phase(tmode,TrajIG,nsegs)
+        phase.setAdaptiveMesh(True)
+        phase.setControlMode(cmode)
+        phase.setMeshTol(mtol)
+
+        phase.addBoundaryValue("Front",[0,1],[x0,t0])
+        phase.addBoundaryValue("Back", [1],  [tf])
+        phase.addIntegralObjective(ODE.obj()*iscale,[0,2])
+        phase.addValueObjective("Back",0,vscale)
+        
+        phase.setThreads(1,1)
+        phase.optimizer.CNRMode =True
+        phase.optimizer.set_QPOrderingMode("MINDEG")
+        
+        if __name__ != "__main__":
+            phase.optimizer.PrintLevel = 3
+            phase.PrintMeshInfo = False
+            
+        phase.optimize()
+        
+        xf1 = phase.returnTraj()[-1][0]
+        ###################################################
+        
+        #### AutoScaling, Value + Integral ###############
+        phase = ode.phase(tmode,TrajIG,nsegs)
+        phase.setAdaptiveMesh(True)
+        phase.setMeshTol(mtol)
+
+        phase.setUnits([xstar,tstar,ustar])
+        phase.addBoundaryValue("Front",[0,1],[x0,t0])
+        phase.addBoundaryValue("Back", [1],  [tf])
+        phase.addIntegralObjective(ODE.obj()*iscale,[0,2])
+        phase.addValueObjective("Back",0,vscale)
+
+        phase.setAutoScaling(True)
+        phase.setThreads(1,1)
+        phase.optimizer.CNRMode =True
+        phase.optimizer.set_QPOrderingMode("MINDEG")
+        
+        if __name__ != "__main__":
+            phase.optimizer.PrintLevel = 3
+            phase.PrintMeshInfo = False
+            
+        phase.optimize()
+        xf2 = phase.returnTraj()[-1][0]
+        ###################################################
+        
+        #### AutoScaling, Value + IntegralParam ###############
+        phase = ode.phase(tmode,TrajIG,nsegs)
+        phase.setAdaptiveMesh(True)
+        phase.setMeshTol(mtol)
+
+        phase.setUnits([xstar,tstar,ustar])
+        phase.setStaticParams([0.0],[pstar])
+        
+        phase.addBoundaryValue("Front",[0,1],[x0,t0])
+        phase.addBoundaryValue("Back", [1],  [tf])
+        phase.addIntegralParamFunction(ODE.obj(),[0,2],0)
+        phase.addValueObjective("Back",0,vscale)
+        phase.addValueObjective("StaticParams",0,iscale)
+
+        phase.setAutoScaling(True)
+        
+        phase.setThreads(1,1)
+        phase.optimizer.CNRMode =True
+        phase.optimizer.set_QPOrderingMode("MINDEG")
+        
+        if __name__ != "__main__":
+            phase.optimizer.PrintLevel = 3
+            phase.PrintMeshInfo = False
+            
+        
+        phase.optimize()
+        xf3 = phase.returnTraj()[-1][0]
+        ###################################################
+        
+        #### AutoScaling, Value + IntegralParam style 2 ###############
+        phase = ode.phase(tmode,TrajIG,nsegs)
+        phase.setAdaptiveMesh(True)
+        phase.setMeshTol(mtol)
+
+        phase.setUnits([xstar,tstar,ustar])
+        phase.setStaticParams([0.0],[pstar])
+        
+        phase.addBoundaryValue("Front",[0,1],[x0,t0])
+        phase.addBoundaryValue("Back", [1],  [tf])
+        phase.addIntegralParamFunction(ODE.obj(),[0,2],0)
+        
+        phase.addStateObjective("Back",Args(2).dot([vscale,iscale]),[0],[],[0])
+
+        phase.setAutoScaling(True)
+        
+        phase.setThreads(1,1)
+        phase.optimizer.CNRMode =True
+        phase.optimizer.set_QPOrderingMode("MINDEG")
+        
+        if __name__ != "__main__":
+            phase.optimizer.PrintLevel = 3
+            phase.PrintMeshInfo = False
+            
+        
+        phase.optimize()
+        xf4 = phase.returnTraj()[-1][0]
+        ###################################################
+        
+        
+        #### AutoScaling, Multi Phase Value + Integral ###############
+        phase1 = ode.phase(tmode,TrajIG[0:int(nsegs/2)],nsegs)
+        phase1.setAdaptiveMesh(True)
+        phase1.setMeshTol(mtol)
+
+        phase1.setUnits([xstar,tstar,ustar])
+        phase1.addBoundaryValue("Front",[0,1],[x0,t0])
+        phase1.addIntegralObjective(ODE.obj()*iscale,[0,2])
+        phase1.addDeltaTimeEqualCon(tf/2.0)
+        
+        phase2 = ode.phase(tmode,TrajIG[int(nsegs/2):],nsegs)
+        phase2.setAdaptiveMesh(True)
+        phase2.setMeshTol(mtol)
+
+        phase2.setUnits([xstar*.99,tstar*.783,ustar*1.1])
+        phase2.setAdaptiveMesh(True)
+        phase2.setMeshTol(mtol)
+        
+        phase2.addIntegralObjective(ODE.obj()*iscale,[0,2])
+        phase2.addBoundaryValue("Back", [1],  [tf])
+        phase2.addValueObjective("Back",0,vscale)
+        
+        ocp = oc.OptimalControlProblem()
+        
+        ocp.addPhase(phase1)
+        ocp.addPhase(phase2)
+        
+        ocp.addForwardLinkEqualCon(phase1,phase2,range(0,3))
+
+        ocp.setAutoScaling(True,True)
+        ocp.setThreads(1,1)
+        ocp.optimizer.CNRMode =True
+        ocp.optimizer.set_QPOrderingMode("MINDEG")
+        
+        if __name__ != "__main__":
+            ocp.optimizer.PrintLevel = 3
+            ocp.PrintMeshInfo = False
+            
+        ocp.optimize()
+        xf5 = phase2.returnTraj()[-1][0]
+        ###################################################
+        
+        
+        
+        
+        
+        xferr1 = abs(xf1-self.FinalState)
+        self.assertLess(xferr1, self.MaxStateError,
+                 "Final state significantly differs from known answer")
+        
+        xferr2 = abs(xf2-self.FinalState)
+        self.assertLess(xferr2, self.MaxStateError,
+                 "Final state significantly differs from known answer")
+        
+        xferr3 = abs(xf3-self.FinalState)
+        self.assertLess(xferr3, self.MaxStateError,
+                 "Final state significantly differs from known answer")
+        
+        xferr4 = abs(xf4-self.FinalState)
+        self.assertLess(xferr4, self.MaxStateError,
+                 "Final state significantly differs from known answer")
+        
+        xferr5 = abs(xf5-self.FinalState)
+        self.assertLess(xferr5, self.MaxStateError,
+                 "Final state significantly differs from known answer")
+        
+    
+    def test_FullProblem(self):
+        
+        for tmode in ["LGL3","LGL5","LGL7","Trapezoidal"]:
+            with self.subTest(TranscriptionMode=tmode):
+                mtol = 1.0e-5 if tmode=="Trapezoidal" else 1.0e-7
+                with self.subTest(cmode="HighestOrderSpline"):
+                    self.problem_impl(tmode,"HighestOrderSpline",mtol)
+                with self.subTest(cmode="BlockConstant"):
+                    self.problem_impl(tmode,"BlockConstant",mtol)
+
+
+if __name__ == "__main__":
+    unittest.main(exit=False)
\ No newline at end of file
diff --git a/asset_asrl/test/test_AutoScaling/test_Reentry.py b/asset_asrl/test/test_AutoScaling/test_Reentry.py
new file mode 100644
index 00000000..f8d299e8
--- /dev/null
+++ b/asset_asrl/test/test_AutoScaling/test_Reentry.py
@@ -0,0 +1,246 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import asset_asrl as ast
+import asset_asrl.VectorFunctions as vf
+import asset_asrl.OptimalControl as oc
+from asset_asrl.VectorFunctions import Arguments as Args
+import unittest
+
+
+
+###################  ##################################
+
+Lstar = 100000.0     ## feet
+Tstar = 60.0         ## sec
+Vstar = Lstar/Tstar
+
+
+g0 = 32.2 
+W  = 203000
+
+
+tmax = 2500           
+Re = 20902900          
+S  = 2690.0            
+m  = (W/g0)            
+mu = (0.140765e17)    
+rho0 =.002378          
+h_ref = 23800        
+
+a0 = -.20704
+a1 = .029244
+
+b0 = .07854
+b1 = -.61592e-2
+b2 = .621408e-3
+
+c0 =  1.0672181
+c1 = -.19213774e-1
+c2 = .21286289e-3
+c3 = -.10117e-5
+
+Qlimit = 70.0
+
+##############################################################################
+class ShuttleReentry(oc.ODEBase):
+    def __init__(self):
+        ############################################################
+        args  = oc.ODEArguments(5,2)
+        
+        h       = args.XVar(0)
+        theta   = args.XVar(1)
+        v       = args.XVar(2)
+        gamma   = args.XVar(3)
+        psi     = args.XVar(4)
+        
+        h,theta,v,gamma,psi = args.XVec().tolist()
+        
+        alpha   = args.UVar(0)
+        beta    = args.UVar(1)
+        
+        alphadeg = (180.0/np.pi)*alpha
+        
+        CL  = a0 + a1*alphadeg
+        CD  = b0 + b1*alphadeg + b2*(alphadeg**2)
+        rho = rho0*vf.exp(-h/h_ref)
+        r   = h + Re
+        
+        L   = 0.5*CL*S*rho*(v**2)
+        D   = 0.5*CD*S*rho*(v**2)
+        g   = mu/(r**2)
+        
+        sgam = vf.sin(gamma)
+        cgam = vf.cos(gamma)
+        
+        sbet = vf.sin(beta)
+        cbet = vf.cos(beta)
+        
+        spsi = vf.sin(psi)
+        cpsi = vf.cos(psi)
+        tantheta = vf.tan(theta)
+        
+        hdot     = v*sgam
+        thetadot = (v/r)*cgam*cpsi
+        vdot     = -D/m - g*sgam
+        gammadot = (L/(m*v))*cbet +cgam*(v/r - g/v)
+        psidot   = L*sbet/(m*v*cgam) + (v/(r))*cgam*spsi*tantheta
+        
+    
+        ode = vf.stack([hdot,thetadot,vdot,gammadot,psidot])
+        
+        Vgroups = {}
+        Vgroups[('h','altitude')] =  h
+        Vgroups[("v","velocity")] =  v
+        Vgroups["theta"] =  theta
+        Vgroups["gamma"] =  gamma
+        Vgroups["psi"]   =  psi
+        Vgroups[("alpha","AoA")] =  alpha
+        Vgroups["beta"]  =  beta
+        Vgroups[("t","time")] =  args.TVar()
+        
+        ##############################################################
+        super().__init__(ode,5,2,Vgroups = Vgroups)
+
+def QFunc():
+    h,v,alpha = Args(3).tolist()
+    alphadeg = (180.0/np.pi)*alpha
+    rhodim = rho0*vf.exp(-h/h_ref)
+    vdim = v
+    
+    qr = 17700*vf.sqrt(rhodim)*((.0001*vdim)**3.07)
+    qa = c0 + c1*alphadeg + c2*(alphadeg**2)+ c3*(alphadeg**3)
+    
+    return qa*qr
+ 
+
+class test_Reentry(unittest.TestCase):
+    
+    @classmethod
+    def setUpClass(self):
+        self.NumSegments1 = 40
+        
+        
+        self.FinalObj1 = 0.5958800738629952 
+        self.MaxObjError1 = .01
+        
+        self.FinalObj2 = 0.534620087611498
+        self.MaxObjError2 = .01
+        
+    
+    def problem_impl(self,tmode,cmode,mtol):
+        
+        ##########################################################################
+        tf  = 2000
+
+        ht0  = 260000
+        htf  = 80000 
+        vt0  = 25600
+        vtf  = 2500 
+
+
+        gammat0 = np.deg2rad(-1.0)
+        gammatf = np.deg2rad(-5.0)
+        psit0   = np.deg2rad(90.0)
+
+
+        ts = np.linspace(0,tf,200)
+
+        TrajIG = []
+        for t in ts:
+            X = np.zeros((8))
+            X[0] = ht0*(1-t/tf) + htf*t/tf
+            X[1] = 0
+            X[2] = vt0*(1-t/tf) + vtf*t/tf
+            X[3] = gammat0*(1-t/tf) + gammatf*t/tf
+            X[4] = psit0
+            X[5] = t
+            X[6] =.00
+            X[7] =.00
+            TrajIG.append(np.copy(X))
+            
+            
+        ################################################################
+
+
+        ode = ShuttleReentry()
+        
+        phase = ode.phase(tmode,TrajIG,self.NumSegments1)
+        
+        phase.setAutoScaling(True)  
+        phase.setUnits(h = Lstar,
+                       v = Vstar,
+                       t = Tstar)
+        
+        phase.setControlMode(cmode)
+        phase.setAdaptiveMesh(True)
+
+        phase.addBoundaryValue("Front",range(0,6),TrajIG[0][0:6])
+        phase.addLUVarBound("Path","theta",np.deg2rad(-89.0),np.deg2rad(89.0))
+        phase.addLUVarBound("Path","gamma",np.deg2rad(-89.0),np.deg2rad(89.0))
+        phase.addLUVarBound("Path","AoA",np.deg2rad(-90.0),np.deg2rad(90.0))
+        phase.addLUVarBound("Path","beta" ,np.deg2rad(-90.0),np.deg2rad(1.0))
+        phase.addUpperDeltaTimeBound(tmax,1.0)
+        
+        phase.addBoundaryValue("Back" 
+                                  ,["h","v","gamma"]
+                                  ,[htf,vtf,gammatf])
+        
+        phase.addDeltaVarObjective("theta",-1.0)
+        
+        
+        phase.optimizer.set_SoeLSMode("L1")
+        phase.optimizer.set_OptLSMode("L1")
+        
+        phase.optimizer.MaxLSIters = 2
+        phase.optimizer.MaxAccIters = 100
+        phase.optimizer.PrintLevel = 3
+        phase.setThreads(1,1)
+        phase.optimizer.CNRMode =True
+        phase.PrintMeshInfo = False
+        phase.setMeshTol(mtol)
+
+        Flag1 = phase.solve_optimize()
+        
+        self.assertEqual(Flag1,ast.Solvers.ConvergenceFlags.CONVERGED, 
+                         "Problem did not converge")
+        
+        Traj1 = phase.returnTraj()
+
+        Obj1 = Traj1[-1][1]
+        ObjError1 = abs(Obj1-self.FinalObj1)
+        self.assertLess(ObjError1, self.MaxObjError1,
+                 "Final objective significantly differs from known answer")
+
+        
+        phase.addUpperFuncBound("Path",QFunc(),["h","v","alpha"],Qlimit,1/Qlimit)
+        Flag2 = phase.optimize()
+        Traj2 = phase.returnTraj()
+       
+        self.assertEqual(Flag2,ast.Solvers.ConvergenceFlags.CONVERGED, 
+                         "Problem did not converge")
+       
+        
+        Obj2 = Traj2[-1][1]
+        ObjError2 = abs(Obj2-self.FinalObj2)
+        self.assertLess(ObjError2, self.MaxObjError2,
+                 "Final objective significantly differs from known answer")
+
+        
+    
+    def test_FullProblem(self):
+        
+        for tmode in ["LGL3","LGL5","LGL7","Trapezoidal"]:
+            with self.subTest(TranscriptionMode=tmode):
+                
+                mtol = 1.0e-4 if tmode=="Trapezoidal" else 1.0e-7
+                
+                with self.subTest(cmode="HighestOrderSpline"):
+                    self.problem_impl(tmode,"HighestOrderSpline",mtol)
+                with self.subTest(cmode="BlockConstant"):
+                    self.problem_impl(tmode,"BlockConstant",mtol)
+
+
+
+
+if __name__ == "__main__":
+    unittest.main(exit=False)