Add Interpolation module and Core utilities from CTModels

- Interpolation module: ctinterpolate, ctinterpolate_constant
- Core utilities: matrix_utils, function_utils, macros
- Tests for all new modules

Author: ocots

src/Core/function_utils.jl

@@ -0,0 +1,32 @@
+"""
+$(TYPEDSIGNATURES)
+
+Convert an in-place function `f!` to an out-of-place function `f`.
+
+The resulting function `f` returns a vector of type `T` and length `n` by first allocating
+memory and then calling `f!` to fill it.
+
+# Arguments
+- `f!`: An in-place function of the form `f!(result, args...)`.
+- `n`: The length of the output vector.
+- `T`: The element type of the output vector (default is `Float64`).
+
+# Returns
+An out-of-place function `f(args...; kwargs...)` that returns the result as a vector or
+scalar, depending on `n`.
+
+# Example
+```julia-repl
+julia> f!(r, x) = (r[1] = sin(x); r[2] = cos(x))
+julia> f = to_out_of_place(f!, 2)
+julia> f(π/4)  # returns approximately [0.707, 0.707]
+```
+"""
+function to_out_of_place(f!, n; T=Float64)
+    function f(args...; kwargs...)
+        r = zeros(T, n)
+        f!(r, args...; kwargs...)
+        return n == 1 ? r[1] : r
+    end
+    return isnothing(f!) ? nothing : f
+end

src/Core/macros.jl

@@ -0,0 +1,26 @@
+"""
+    @ensure condition exception
+
+Throws the provided `exception` if `condition` is false.
+
+# Usage
+```julia-repl
+julia> @ensure true Exceptions.IncorrectArgument("This won't throw")
+julia> @ensure false Exceptions.IncorrectArgument("This will throw")
+ERROR: IncorrectArgument("This will throw")
+```
+
+# Arguments
+- `condition`: A Boolean expression to test.
+- `exception`: An instance of an exception to throw if `condition` is false.
+
+# Throws
+- The provided `exception` if the condition is not satisfied.
+"""
+macro ensure(cond, exc)
+    return esc(:(
+        if !($cond)
+            throw($exc)
+        end
+    ))
+end

src/Core/matrix_utils.jl

@@ -0,0 +1,39 @@
+"""
+$(TYPEDSIGNATURES)
+
+Return the default dimension for matrix storage.
+"""
+__matrix_dimension_storage() = 1
+
+"""
+$(TYPEDSIGNATURES)
+
+Transform a matrix into a vector of vectors along the specified dimension.
+
+Each row or column of the matrix `A` is extracted and stored as an individual vector,
+depending on `dim`.
+
+# Arguments
+- `A`: A matrix of elements of type `<:ctNumber`.
+- `dim`: The dimension along which to split the matrix (`1` for rows, `2` for columns).
+  Defaults to `1`.
+
+# Returns
+A `Vector` of `Vector`s extracted from the rows or columns of `A`.
+
+# Note
+This is useful when data needs to be represented as a sequence of state or control vectors
+in optimal control problems.
+
+# Example
+```julia-repl
+julia> A = [1 2 3; 4 5 6]
+julia> matrix2vec(A, 1)  # splits into rows: [[1, 2, 3], [4, 5, 6]]
+julia> matrix2vec(A, 2)  # splits into columns: [[1, 4], [2, 5], [3, 6]]
+```
+"""
+function matrix2vec(
+    A::Matrix{<:ctNumber}, dim::Int=__matrix_dimension_storage()
+)::Vector{<:Vector{<:ctNumber}}
+    return dim == 1 ? [A[i, :] for i in 1:size(A, 1)] : [A[:, i] for i in 1:size(A, 2)]
+end

src/Interpolation/Interpolation.jl

@@ -0,0 +1,18 @@
+"""
+    Interpolation
+
+Interpolation utilities for the Control Toolbox (CT) ecosystem.
+
+# Public API
+- `ctinterpolate`: linear interpolation with flat extrapolation
+- `ctinterpolate_constant`: piecewise-constant (steppost) interpolation
+"""
+module Interpolation
+
+import DocStringExtensions: TYPEDSIGNATURES
+
+include(joinpath(@__DIR__, "ctinterpolate.jl"))
+
+export ctinterpolate, ctinterpolate_constant
+
+end # module Interpolation

src/Interpolation/ctinterpolate.jl

@@ -0,0 +1,87 @@
+"""
+$(TYPEDSIGNATURES)
+
+Return a linear interpolation function for the data `f` defined at points `x`.
+
+This function creates a one-dimensional linear interpolant with flat extrapolation
+beyond the bounds of `x` (returns `f[1]` for `t < x[1]` and `f[end]` for `t >= x[end]`).
+
+# Arguments
+- `x`: A vector of points at which the values `f` are defined.
+- `f`: A vector of values to interpolate.
+
+# Returns
+A callable interpolation object that can be evaluated at new points.
+
+# Example
+```julia-repl
+julia> x = [0.0, 1.0, 2.0]
+julia> f = [1.0, 2.0, 3.0]
+julia> interp = ctinterpolate(x, f)
+julia> interp(0.5)  # Returns 1.5 (linear interpolation)
+julia> interp(-1.0)  # Returns 1.0 (flat extrapolation)
+julia> interp(3.0)  # Returns 3.0 (flat extrapolation)
+```
+"""
+function ctinterpolate(x, f)
+    function linear_interp(t)
+        if t < x[1]
+            return f[1]
+        elseif t >= x[end]
+            return f[end]
+        else
+            i = searchsortedlast(x, t)
+            if i == length(x)
+                return f[end]
+            end
+            α = (t - x[i]) / (x[i + 1] - x[i])
+            return f[i] + α * (f[i + 1] - f[i])
+        end
+    end
+    return linear_interp
+end
+
+"""
+$(TYPEDSIGNATURES)
+
+Return a piecewise-constant interpolation function for the data `f` defined at points `x`.
+
+This function creates a right-continuous piecewise constant interpolant:
+the value at knot `x[i]` is held constant on the interval `[x[i], x[i+1})`.
+
+This implements the standard steppost behavior for optimal control:
+- `u(t_i) = u_i` (value at the knot)
+- `u(t) = u_i` for all `t ∈ [t_i, t_{i+1})`
+- Right-continuous: `lim_{t→t_i^+} u(t) = u(t_i)`
+
+# Arguments
+- `x`: A vector of points at which the values `f` are defined.
+- `f`: A vector of values to interpolate.
+
+# Returns
+A callable interpolation object that can be evaluated at new points.
+
+# Example
+```julia-repl
+julia> x = [0.0, 1.0, 2.0]
+julia> f = [1.0, 2.0, 3.0]
+julia> interp = ctinterpolate_constant(x, f)
+julia> interp(0.0)  # Returns 1.0 (value at x[1])
+julia> interp(0.5)  # Returns 1.0 (held from x[1] on [0.0, 1.0))
+julia> interp(1.0)  # Returns 2.0 (value at x[2], right-continuous)
+julia> interp(1.5)  # Returns 2.0 (held from x[2] on [1.0, 2.0))
+```
+"""
+function ctinterpolate_constant(x, f)
+    function steppost_interp(t)
+        if t < x[1]
+            return f[1]
+        elseif t >= x[end]
+            return f[end]
+        else
+            i = searchsortedlast(x, t)
+            return f[i]
+        end
+    end
+    return steppost_interp
+end

test/suite/core/test_function_utils.jl

@@ -0,0 +1,74 @@
+module TestCoreFunctionUtils
+
+import Test
+import CTBase.Core
+
+const VERBOSE = isdefined(Main, :TestOptions) ? Main.TestOptions.VERBOSE : true
+const SHOWTIMING = isdefined(Main, :TestOptions) ? Main.TestOptions.SHOWTIMING : true
+
+function test_function_utils()
+    Test.@testset verbose = VERBOSE showtiming = SHOWTIMING "to_out_of_place" begin
+
+        Test.@testset "to_out_of_place - basic conversion" begin
+            f!(r, x) = (r[1] = sin(x); r[2] = cos(x))
+            f = Core.to_out_of_place(f!, 2)
+            result = f(π / 4)
+            Test.@test result isa Vector
+            Test.@test length(result) == 2
+            Test.@test result[1] ≈ sin(π / 4)
+            Test.@test result[2] ≈ cos(π / 4)
+        end
+
+        Test.@testset "to_out_of_place - scalar output (n=1)" begin
+            g!(r, x) = (r[1] = x^2)
+            g = Core.to_out_of_place(g!, 1)
+            result = g(3.0)
+            Test.@test result isa Float64
+            Test.@test result ≈ 9.0
+        end
+
+        Test.@testset "to_out_of_place - with kwargs" begin
+            h!(r, x; scale=1.0) = (r[1] = x * scale; r[2] = x^2 * scale)
+            h = Core.to_out_of_place(h!, 2)
+            Test.@test h(2.0)[1] ≈ 2.0
+            Test.@test h(2.0)[2] ≈ 4.0
+            Test.@test h(2.0; scale=3.0)[1] ≈ 6.0
+            Test.@test h(2.0; scale=3.0)[2] ≈ 12.0
+        end
+
+        Test.@testset "to_out_of_place - multiple arguments" begin
+            k!(r, x, y) = (r[1] = x + y; r[2] = x * y)
+            k = Core.to_out_of_place(k!, 2)
+            result = k(3.0, 4.0)
+            Test.@test result[1] ≈ 7.0
+            Test.@test result[2] ≈ 12.0
+        end
+
+        Test.@testset "to_out_of_place - custom type" begin
+            m!(r, x) = (r[1] = x + 1; r[2] = x + 2)
+            m = Core.to_out_of_place(m!, 2; T=Int)
+            result = m(5)
+            Test.@test result isa Vector{Int}
+            Test.@test result[1] == 6
+            Test.@test result[2] == 7
+        end
+
+        Test.@testset "to_out_of_place - nothing input" begin
+            Test.@test Core.to_out_of_place(nothing, 2) === nothing
+        end
+
+        Test.@testset "to_out_of_place - larger output" begin
+            big!(r, x) = (for i in 1:5; r[i] = x * i; end)
+            big = Core.to_out_of_place(big!, 5)
+            result = big(2.0)
+            Test.@test length(result) == 5
+            Test.@test result == [2.0, 4.0, 6.0, 8.0, 10.0]
+        end
+
+    end
+    return nothing
+end
+
+end # module TestCoreFunctionUtils
+
+test_function_utils() = TestCoreFunctionUtils.test_function_utils()

test/suite/core/test_macros.jl

@@ -0,0 +1,64 @@
+module TestCoreMacros
+
+import Test
+import CTBase.Core
+import CTBase.Exceptions
+
+const VERBOSE = isdefined(Main, :TestOptions) ? Main.TestOptions.VERBOSE : true
+const SHOWTIMING = isdefined(Main, :TestOptions) ? Main.TestOptions.SHOWTIMING : true
+
+function test_macros()
+    Test.@testset verbose = VERBOSE showtiming = SHOWTIMING "@ensure" begin
+
+        Test.@testset "@ensure - condition true" begin
+            x = 5
+            Test.@test_nowarn Core.@ensure x > 0 Exceptions.IncorrectArgument("x must be positive")
+            Test.@test_nowarn Core.@ensure x == 5 Exceptions.IncorrectArgument("x must be 5")
+        end
+
+        Test.@testset "@ensure - condition false" begin
+            x = -5
+            Test.@test_throws Exceptions.IncorrectArgument Core.@ensure x > 0 Exceptions.IncorrectArgument("x must be positive")
+            y = 10
+            Test.@test_throws Exceptions.IncorrectArgument Core.@ensure y < 0 Exceptions.IncorrectArgument("y must be negative")
+        end
+
+        Test.@testset "@ensure - with different exception types" begin
+            x = 0
+            Test.@test_throws ArgumentError Core.@ensure x != 0 ArgumentError("x cannot be zero")
+            Test.@test_throws DomainError Core.@ensure x > 0 DomainError(x, "x must be positive")
+        end
+
+        Test.@testset "@ensure - complex conditions" begin
+            x = 5
+            y = 10
+            Test.@test_nowarn Core.@ensure x < y Exceptions.IncorrectArgument("x must be less than y")
+            Test.@test_throws Exceptions.IncorrectArgument Core.@ensure x > y Exceptions.IncorrectArgument("x must be greater than y")
+            Test.@test_nowarn Core.@ensure (x > 0 && y > 0) Exceptions.IncorrectArgument("both must be positive")
+            Test.@test_throws Exceptions.IncorrectArgument Core.@ensure (x < 0 || y < 0) Exceptions.IncorrectArgument("at least one must be negative")
+        end
+
+        Test.@testset "@ensure - exception message verification" begin
+            x = -5
+            try
+                Core.@ensure x > 0 Exceptions.IncorrectArgument("x must be positive")
+                Test.@test false
+            catch e
+                Test.@test e isa Exceptions.IncorrectArgument
+                Test.@test e.msg == "x must be positive"
+            end
+        end
+
+        Test.@testset "@ensure - with type checks" begin
+            x = 5
+            Test.@test_nowarn Core.@ensure x isa Int Exceptions.IncorrectArgument("x must be an Int")
+            Test.@test_throws Exceptions.IncorrectArgument Core.@ensure x isa String Exceptions.IncorrectArgument("x must be a String")
+        end
+
+    end
+    return nothing
+end
+
+end # module TestCoreMacros
+
+test_macros() = TestCoreMacros.test_macros()