-- SINMM.lua
-- LUA-mix Script for SFO Ornithopter
-- Implements two flying modes:
--    > Low Throttle  = Mode-1 Sinus Flapping
--    > High Throttle = Mode-2 Mix-Max Flapping
--
-- Threshold between Sinus & Min-Max is at Thtrottle 200 (20%)
-- You may use a curve in model to adapt the stick level
--
-- In Sinus mode, the Plus parameter controls extra Amplitude
--
-- T. JOUBERT   2026-02-22      Release
-- T. JOUBERT   2026-03-14      Protected against nil inputs 

local inputs = {
                 { "Ampl", SOURCE }, 
                 { "Ailr", SOURCE }, 
                 { "Elev", SOURCE }, 
                 { "Rudd", SOURCE }, 
                 { "Freq", SOURCE }, 
                 { "Plus", SOURCE }, 
               }
               
local outputs = { "WingL", "WingR" }

-- Common time variables
local before = 0
local now = 0
local delta = 0  

-- Mode1-Sinus variables
local sin_table = {}
local speed = 0
local angle = 0
local wave = 0  
local flapping = 0

-- Mode2-MinMax variables
local flapping_time = 0
local GoUp = false  -- toggle between two half periods
local WingR = 0
local WingL = 0

local function init_func()
    -- Prepare sinus table (index = degrees)
    for i = 0, 360 do
        sin_table[i] = math.sin(math.rad(i))
    end
    before = getTime()
    angle = 0
    GoUp = false    -- first flap goes down
    flapping_time = 0  -- initialize MinMax period timer
end

local function run(Amplitude, Aileron, Elevator, Rudder, Frequency, OffsetSin)
    
    Amplitude = Amplitude or 0
    Aileron = Aileron or 0
    Elevator = Elevator or 0
    Rudder = Rudder or 0
    Frequency = Frequency or 0
    OffsetSin = OffsetSin or 0
    
    -- Time between two LUA-Mix cycles (unit = 10ms)
    now = getTime()
    delta = now - before
    before = now
    
    if delta < 0 then
        delta = 4
    end
    
    -- FLAPPING ---------------
    if Frequency > -980 then 
    
        -- MODE-1 SINUS ----------
        if Frequency < 200 then
        
            -- Throttle gives the rotation speed for the angle
            speed = (Frequency + 1024) / 90
            angle = angle + (speed * delta)
            
            -- Check angle overflow
            if angle >= 360 then
                angle = angle - 360
            end
            
            -- Calculate the smooth motion
            wave = sin_table[math.floor(angle)]
    
            -- Set final positions
            flapping = (wave * (1024 + Amplitude + OffsetSin) / 2)
            WingL = flapping + Aileron + Elevator + Rudder
            WingR = -flapping + Aileron - Elevator + Rudder
            
        -- MODE-2 MIN-MAX -------
        else
        
            -- Flapping period between 700ms and 230ms 
            -- half period is in units of 10ms and goes from 35 to 12.5
            local half_period = 12.5 + (1024 - Frequency) / 100
            
            -- Increment Min-Max period measurement 
            flapping_time = flapping_time + delta 
            
            if not GoUp then
                WingL = (1024 + Amplitude)/2  + Aileron + Elevator + Rudder
                WingR = -(1024 + Amplitude)/2 + Aileron - Elevator + Rudder
                
                -- Half period done, going up now
                if (flapping_time >= half_period) then
                    GoUp = true
                end            
            else            
                WingL = -(1024 + Amplitude)/2 + Aileron + Elevator + Rudder
                WingR = (1024 + Amplitude)/2  + Aileron - Elevator + Rudder
            end
            
            -- One full flap done going down again
            if flapping_time >= (2*half_period) then
                GoUp = false
                flapping_time = 0  -- reset period measurement
            end
        end
        
    -- GLIDING -------
    else
        WingL = Aileron + Elevator
        WingR = Aileron - Elevator
    end
    
    return WingL, WingR
end

return { input=inputs, output=outputs, run=run, init=init_func }