local assertArgs = include "assertArgs"

local util = include "util"

local SmoothCellulo = lib "class" ("SmoothCellulo")

SmoothCellulo:include(mixin "Angle")
SmoothCellulo:include(mixin "Position")
SmoothCellulo:include(mixin "_has" ("Cellulo"))

local KIDNAPPED_STABILIZE_DELAY = 0.3

function SmoothCellulo:initialize(args)
    assertArgs(args, "cellulo")
    self:setCellulo(args.cellulo)

    self:setPosition(args.cellulo:getX(), args.cellulo:getY(), 0)
    self:setAngle(args.cellulo:getTheta(), 0, 1, 0)

    args.cellulo:clearTracking()

    self:updateTargetPose()
    self.kidnapped = true
    self.kidnappedTimer = KIDNAPPED_STABILIZE_DELAY

    return self
end

function SmoothCellulo:updateTargetPose()
    local x, y, theta = self:getCellulo():getX(), self:getCellulo():getY(), self:getCellulo():getTheta()
    if not (x and y and theta) then return end -- ensure no value is nil

    self.targetX, self.targetY, self.targetAngle = x, y, math.rad(theta)
end

function SmoothCellulo:update(dt)

    self:updateTargetPose()

    if self:getCellulo():getKidnapped() == 1 then
        self.kidnapped = true
        self.kidnappedTimer = KIDNAPPED_STABILIZE_DELAY
    end

    if self.kidnappedTimer then
        self.kidnappedTimer = self.kidnappedTimer - dt
        if self.kidnappedTimer <= 0 then
            self.kidnappedTimer = nil
            self.kidnapped = false

            -- released safely with stabilization, reset position to avoid jumping
            self.x, self.y, self.angle = self.targetX, self.targetY, self.targetAngle
        end
    end

    if not self.kidnapped then
        local LERP_POSITION = 5
        local LERP_ANGLE = 5
        self.x, self.y = util.lerp(self.x, self.targetX, LERP_POSITION * dt), util.lerp(self.y, self.targetY, LERP_POSITION * dt)
        self.angle = util.lerpAngle(self.angle, self.targetAngle, LERP_ANGLE * dt)
    end

end

function SmoothCellulo:getX() return self.x end
function SmoothCellulo:getY() return self.y end
function SmoothCellulo:getTheta() return self.angle end
function SmoothCellulo:getConnectionStatus() return self:getCellulo():getConnectionStatus() end
function SmoothCellulo:setHapticBackdriveAssist(...) return self:getCellulo():setHapticBackdriveAssist(...) end
function SmoothCellulo:setGoalVelocity(...) return self:getCellulo():setGoalVelocity(...) end
function SmoothCellulo:setGoalPose(...) return self:getCellulo():setGoalPose(...) end
function SmoothCellulo:setGoalPosition(...) return self:getCellulo():setGoalPosition(...) end
function SmoothCellulo:getKidnapped() return self.kidnapped end

return SmoothCellulo