--upsidedown 13.5.2013:
--smooth transition
--coupling with steershift and changeDirection
-- not MP ready  (??)

--edit 3.11.2013
--coupling with GPS

XerionSteerV2 = {}

function XerionSteerV2.prerequisitesPresent(specializations)
    return SpecializationUtil.hasSpecialization(Steerable, specializations)
end

function XerionSteerV2:load(savegame)
  self.setXerionSteerV2                  = SpecializationUtil.callSpecializationsFunction("setXerionSteerV2")    
    self.isSelectable                      = true
    self.xerionSteerV2set                  = {}
    self.xerionSteerV2set.maxInvRadius     = 1 / Utils.getNoNil( getXMLFloat( xmlFile, "vehicle.xerionSteerV2#minRadius" ), 7.5 )
    self.xerionSteerV2set.maxShift         = Utils.getNoNil( getXMLFloat( xmlFile, "vehicle.xerionSteerV2#maxShift" ), 1.65 )
    self.xerionSteerV2set.minShift         = Utils.getNoNil( getXMLFloat( xmlFile, "vehicle.xerionSteerV2#minShift" ), -1.65 )
    self.xerionSteerV2set.enableFreeMode   = Utils.getNoNil( getXMLBool(  xmlFile, "vehicle.xerionSteerV2#enableFreeMode" ), false )
    self.xerionSteerV2set.speedLimit       = Utils.getNoNil( getXMLFloat( xmlFile, "vehicle.xerionSteerV2#speedLimit" ), 22 )
    self.xerionSteerV2set.rotMax           = Utils.getNoNil( getXMLFloat( xmlFile, "vehicle.ackermannSteering#rotMax" ), 15 )
    self.xerionSteerV2set.rotMin           = Utils.getNoNil( getXMLFloat( xmlFile, "vehicle.ackermannSteering#rotMin" ), -15 )
    self.xerionSteerV2set.maxCrabAngle     = math.rad( Utils.getNoNil( getXMLFloat( xmlFile, "vehicle.xerionSteerV2#maxCrabAngle" ), self.xerionSteerV2set.rotMax ) )
    self.xerionSteerV2set.minCrabAngle     = math.rad( Utils.getNoNil( getXMLFloat( xmlFile, "vehicle.xerionSteerV2#minCrabAngle" ), math.max( -math.deg( self.xerionSteerV2set.maxCrabAngle ), self.xerionSteerV2set.rotMin ) ) )
    self.xerionSteerV2set.defaultCrabAngle = math.rad( Utils.getNoNil( getXMLFloat( xmlFile, "vehicle.xerionSteerV2#defaultCrabAngle" ), 7.5 ) )
    self.xerionSteerV2set.curShift         = 0
    self.xerionSteerV2set.curCrabAngle     = self.xerionSteerV2set.defaultCrabAngle
    self.xerionSteerV2set.saveCrabAngle    = 0
    self.xerionSteerV2set.lastShift        = 0
    self.xerionSteerV2set.lastCrabAngle    = 0
    self.xerionSteerV2set.mode             = 0
    self.xerionSteerV2set.modeExtra        = false
    self.getSpeedLimit                     = Utils.overwrittenFunction( self.getSpeedLimit, XerionSteerV2.newGetSpeedLimit )

    local refNode = Utils.indexToObject(self.components, getXMLString(xmlFile, "vehicle.aiTractorDirectionNode#index"))
    if refNode == nil then
        refNode = self.components[1].node
    end
    
    self.xerionSteerV2set.refNode = createTransformGroup( "xsv2RefNode" )
    link( refNode, self.xerionSteerV2set.refNode )
    self.aiTractorDirectionNode = self.xerionSteerV2set.refNode
end

function XerionSteerV2:loadFromAttributesAndNodes(xmlFile, key, resetVehicles)  
  if not resetVehicles then
    self.xerionSteerV2set.mode = Utils.getNoNil(getXMLFloat(xmlFile, key.."#steermode"),0)
    end
  return BaseMission.VEHICLE_LOAD_OK
end

function XerionSteerV2:getSaveAttributesAndNodes(nodeIdent)
  local attributes = ' steermode="'..tonumber(self.xerionSteerV2set.mode)..'"'
  return attributes, nil
end

function XerionSteerV2:mouseEvent(posX, posY, isDown, isUp, button)
end

function XerionSteerV2:keyEvent(unicode, sym, modifier, isDown)
end

function XerionSteerV2:update(dt)
    
    if self:getIsActiveForInput(true) and self.xerionSteerV2set.mode >= 3 then
        if Input.isKeyPressed(Input.KEY_j) then
            self.xerionSteerV2set.curShift = math.min( self.xerionSteerV2set.curShift + 0.001 * dt, self.xerionSteerV2set.maxShift )
        end        
        if Input.isKeyPressed(Input.KEY_n) then
            self.xerionSteerV2set.curShift = math.max( self.xerionSteerV2set.curShift - 0.001 * dt, self.xerionSteerV2set.minShift )
        end
        if Input.isKeyPressed(Input.KEY_k) then
            self.xerionSteerV2set.curCrabAngle = math.min( self.xerionSteerV2set.curCrabAngle + 0.0001 * dt, self.xerionSteerV2set.maxCrabAngle )
        end        
        if Input.isKeyPressed(Input.KEY_m) then
            self.xerionSteerV2set.curCrabAngle = math.max( self.xerionSteerV2set.curCrabAngle - 0.0001 * dt, self.xerionSteerV2set.minCrabAngle )
        end
    end
    
    if self:getIsActiveForInput(false) then    
        if InputBinding.hasEvent( InputBinding.XerionSteerV2Mode ) then
            self.xerionSteerV2set.mode = self.xerionSteerV2set.mode + 1
                        
            if     self.xerionSteerV2set.mode > 5
                    or ( not ( self.xerionSteerV2set.enableFreeMode )
                     and self.xerionSteerV2set.mode > 4 ) then
                self.xerionSteerV2set.mode = 0
            elseif self.xerionSteerV2set.mode == 3 and math.abs( self.xerionSteerV2set.curCrabAngle ) < 1E-3 then
                if math.abs( self.xerionSteerV2set.saveCrabAngle ) < 1E-3 then
                    self.xerionSteerV2set.saveCrabAngle = self.xerionSteerV2set.defaultCrabAngle
                end
                self.xerionSteerV2set.curCrabAngle  = -self.xerionSteerV2set.saveCrabAngle
            elseif self.xerionSteerV2set.mode == 4 then
                self.xerionSteerV2set.modeExtra     = false
            elseif self.xerionSteerV2set.mode == 5 and math.abs( self.xerionSteerV2set.curCrabAngle ) >=1E-3 then
                self.xerionSteerV2set.saveCrabAngle = self.xerionSteerV2set.curCrabAngle
                self.xerionSteerV2set.curCrabAngle  = 0
            end
        end
        if InputBinding.hasEvent( InputBinding.XerionSteerV2Extra ) then
            if     self.xerionSteerV2set.mode == 3 
                    or self.xerionSteerV2set.mode == 5 then
                if math.abs( self.xerionSteerV2set.curCrabAngle ) < 1E-3 then
                    if math.abs( self.xerionSteerV2set.saveCrabAngle ) < 1E-3 then
                        self.xerionSteerV2set.saveCrabAngle = self.xerionSteerV2set.defaultCrabAngle
                    end
                    self.xerionSteerV2set.curCrabAngle  = -self.xerionSteerV2set.saveCrabAngle
                else
                    self.xerionSteerV2set.saveCrabAngle = self.xerionSteerV2set.curCrabAngle
                    self.xerionSteerV2set.curCrabAngle  = 0
                end
            elseif self.xerionSteerV2set.mode == 4 then
                self.xerionSteerV2set.modeExtra = not ( self.xerionSteerV2set.modeExtra )
            end
        end
    end

    if self.xerionSteerV2set.mode == 3 or self.xerionSteerV2set.mode == 4 then
        local m = XerionSteerV2.getMaxCrabAngleOfTrailer( self )
        if m ~= nil and m < math.rad(1) then
            if self.xerionSteerV2set.enableFreeMode then
                self.xerionSteerV2set.mode = 5
            else
                self.xerionSteerV2set.mode = 0
            end
            self.xerionSteerV2set.warningTimer = g_currentMission.time + 5000
        end
    end
    
    if self.xerionSteerV2set.warningTimer ~= nil and g_currentMission.time < self.xerionSteerV2set.warningTimer then
        g_currentMission:addWarning( g_i18n:getText("XerionSteerV2Warning"), 0.018, 0.033)
    end
    
    local steering   = 0
    if     self.rotatedTime > 0 and self.maxRotTime > 0 then
        steering =  self.rotatedTime / self.maxRotTime
    elseif self.rotatedTime < 0 and self.minRotTime < 0 then
        steering = -self.rotatedTime / self.minRotTime
    end
        
    tz0 = self.xerionSteerV2set.curShift
    ry0 = self.xerionSteerV2set.curCrabAngle
    sp1 = 0.002
    sp2 = 0.0002
    
    if     self.xerionSteerV2set.mode == 0 then
        local sp   = self.lastSpeed * 3600
        sp         = Utils.clamp( ( sp - 20 ) * 0.05, 0, 1 )
        sp         = ( 1 + sp+sp ) / 3
        tz0        = sp * self.xerionSteerV2set.minShift
        ry0        = 0
    elseif self.xerionSteerV2set.mode == 1 then
        tz0        = 0
        ry0        = 0
    elseif self.xerionSteerV2set.mode == 2 then
        tz0        = self.xerionSteerV2set.maxShift
        ry0        = 0
    elseif self.xerionSteerV2set.mode == 3 then
        tz0        = 0
    elseif self.xerionSteerV2set.mode == 4 then
        tz0        = 0
        sp2        = 0.005
        if self.xerionSteerV2set.modeExtra then
            ry0      = 0
        elseif steering >= 0 then
            ry0      =  steering * self.xerionSteerV2set.maxCrabAngle
            steering = 0
        else
            ry0      = -steering * self.xerionSteerV2set.minCrabAngle
            steering = 0
        end
    end
    
    if     self.xerionSteerV2set.lastShift < tz0 then
        self.xerionSteerV2set.lastShift = math.min( self.xerionSteerV2set.lastShift + sp1 * dt, tz0 )
    elseif self.xerionSteerV2set.lastShift > tz0 then
        self.xerionSteerV2set.lastShift = math.max( self.xerionSteerV2set.lastShift - sp1 * dt, tz0 )
    end
    if     self.xerionSteerV2set.lastCrabAngle < ry0 then
        self.xerionSteerV2set.lastCrabAngle = math.min( self.xerionSteerV2set.lastCrabAngle + sp2 * dt, ry0 )
    elseif self.xerionSteerV2set.lastCrabAngle > ry0 then
        self.xerionSteerV2set.lastCrabAngle = math.max( self.xerionSteerV2set.lastCrabAngle - sp2 * dt, ry0 )
    end
    
    local tx, ty, tz = getTranslation( self.xerionSteerV2set.refNode )
    local rx, ry, rz = getRotation( self.xerionSteerV2set.refNode )
    local tz1        = self.xerionSteerV2set.lastShift
    local ry1        = self.xerionSteerV2set.lastCrabAngle -- * ( 1 - math.abs( steering ) )
    if math.abs( tz - tz1 ) > 0.01 then
        setTranslation( self.xerionSteerV2set.refNode, tx, ty, tz1 )
    end
    if math.abs( ry - ry1 ) > 0.001 then
        setRotation(    self.xerionSteerV2set.refNode, rx, ry1, rz )
    end    
    self.GPSangleOffSet = ry1    
    --self.GPS_externalLRoffset = 0
    
    local invRadius =  steering * self.xerionSteerV2set.maxInvRadius
    
    self:setXerionSteerV2( true, invRadius, self.xerionSteerV2set.refNode )
end

function XerionSteerV2:draw()
    if self.xerionSteerV2set.mode == 0 then
        g_currentMission:addHelpButtonText(g_i18n:getText("XerionSteerV2Mode"),     InputBinding.XerionSteerV2Mode) 
    elseif     self.xerionSteerV2set.mode == 1 then
        g_currentMission:addHelpButtonText(g_i18n:getText("XerionSteerV2All"),      InputBinding.XerionSteerV2Mode) 
    elseif     self.xerionSteerV2set.mode == 2 then
        g_currentMission:addHelpButtonText(g_i18n:getText("XerionSteerV2Back"),     InputBinding.XerionSteerV2Mode) 
    elseif     self.xerionSteerV2set.mode == 3 then
        g_currentMission:addHelpButtonText(g_i18n:getText("XerionSteerV2SmallCrab"),InputBinding.XerionSteerV2Mode) 
        g_currentMission:addHelpButtonText(g_i18n:getText("XerionSteerV2Extra"),    InputBinding.XerionSteerV2Extra) 
    elseif     self.xerionSteerV2set.mode == 4 then
        g_currentMission:addHelpButtonText(g_i18n:getText("XerionSteerV2BigCrab"),  InputBinding.XerionSteerV2Mode) 
        g_currentMission:addHelpButtonText(g_i18n:getText("XerionSteerV2Extra"),    InputBinding.XerionSteerV2Extra) 
    else    
        g_currentMission:addHelpButtonText(g_i18n:getText("XerionSteerV2Free"),     InputBinding.XerionSteerV2Mode) 
        g_currentMission:addHelpButtonText(g_i18n:getText("XerionSteerV2Extra"),    InputBinding.XerionSteerV2Extra) 
    end    
    --local debugText = string.format( "Xerion debug: %0.1f° %1.2fm", math.deg(self.xerionSteerV2set.lastCrabAngle), self.xerionSteerV2set.lastShift )
    --g_currentMission:addExtraPrintText( debugText )
end

function XerionSteerV2:setXerionSteerV2( enabled, invRadius, refNode )
    if enabled then
        self.xerionSteerV2rto           = {}
        self.xerionSteerV2rto.invRadius = invRadius 
        self.xerionSteerV2rto.refNode   = refNode 
        local rx, ry, rz   = getRotation( self.xerionSteerV2rto.refNode )
        self.xerionSteerV2rto.crabAngle = ry 
    else
        self.xerionSteerV2rto           = nil
    end
end

function XerionSteerV2:delete()
end

function XerionSteerV2:getMaxCrabAngleOfTrailer()
    if self.attachedImplements == nil then
        return nil
    end
    
    local maxCrabAngle = nil
    for _, implement in pairs(self.attachedImplements) do
        if implement.object ~= nil then
            -- attached implements of implements
            local m1 = XerionSteerV2.getMaxCrabAngleOfTrailer( implement.object )
            if maxCrabAngle == nil or maxCrabAngle > m1 then
                maxCrabAngle = m1
            end
            
            -- wheel rotMax/rotMin
            for _,wheel in pairs(implement.object.wheels) do
                local m2 = 0
                if     math.abs( wheel.rotSpeed )          > 1E-4 then
                    m2 = math.min( wheel.rotMax, -wheel.rotMin )
                elseif math.abs( wheel.steeringAxleScale ) > 1E-4 then
                    m2 = math.min( wheel.steeringAxleRotMax, -wheel.steeringAxleRotMin )
                end
                if maxCrabAngle == nil or maxCrabAngle > m2 then
                    maxCrabAngle = m2
                end
            end
        end
    end
    
    return maxCrabAngle
end

function XerionSteerV2:newUpdateWheelSteeringAngle(superFunc, wheel, dt)
    local rootVehicle = self
    if rootVehicle.xerionSteerV2rto == nil then
        rootVehicle = self.getRootAttacherVehicle(self)
    end
    
    if rootVehicle == nil or rootVehicle.xerionSteerV2rto == nil then
        return superFunc( self, wheel, dt )
    end
    
    local wx, wy, wz    = getWorldTranslation( wheel.driveNode )
    local x,  y,  z     = worldToLocal( rootVehicle.xerionSteerV2rto.refNode, wx, wy, wz )
    local offsetAngle   = 0
    if rootVehicle ~= self then
        offsetAngle = Utils.getYRotationBetweenNodes(rootVehicle.steeringAxleNode, self.steeringAxleNode)
    end
    local steeringAngle = rootVehicle.xerionSteerV2rto.crabAngle - offsetAngle

    if rootVehicle.xerionSteerV2rto.invRadius >= 0.01 then
        steeringAngle  = steeringAngle + math.atan2( z, 1/rootVehicle.xerionSteerV2rto.invRadius  - x )
    elseif math.abs(z) >= 0.25 then
        steeringAngle  = steeringAngle + math.atan2( z * rootVehicle.xerionSteerV2rto.invRadius, 1  - x * rootVehicle.xerionSteerV2rto.invRadius )
    end
    
    if     math.abs( wheel.rotSpeed )          > 1E-4 then
        wheel.steeringAngle = Utils.clamp( steeringAngle, wheel.rotMin, wheel.rotMax )
    elseif math.abs( wheel.steeringAxleScale ) > 1E-4 then
        wheel.steeringAngle = Utils.clamp( rootVehicle.xerionSteerV2rto.crabAngle + self.steeringAxleAngle * wheel.steeringAxleScale, wheel.steeringAxleRotMin, wheel.steeringAxleRotMax )
    else
        return superFunc( self, wheel, dt )
    end
    
--print(string.format("%3.3f %3.3f %3.3f %3.3f %3.3f",wheel.steeringAngle,wheel.rotSpeed,wheel.steeringAxleScale,wheel.rotMax,wheel.steeringAxleRotMax ))
end

WheelsUtil.updateWheelSteeringAngle  = Utils.overwrittenFunction( WheelsUtil.updateWheelSteeringAngle, XerionSteerV2.newUpdateWheelSteeringAngle )

function XerionSteerV2:newGetSpeedLimit(superFunc, ...)
    if self.xerionSteerV2rto == nil or self.xerionSteerV2set == nil or self.xerionSteerV2set.mode == 0 then
        return superFunc( self, ... )
    end
    local r1, r2 = superFunc( self, ... )
    r1 = math.min( self.xerionSteerV2set.speedLimit, r1 )
    return r1, r2
end


function XerionSteerV2:writeStream(streamId, connection)
    streamWriteInt8(streamId, self.XS_LR)
    streamWriteInt8(streamId, self.xerionSteerV2set.mode)
end


function XerionSteerV2:readStream(streamId, connection)
    self.XS_LR = streamReadInt8(streamId)
    self.xerionSteerV2set.mode = streamReadInt8(streamId)
end


XerionSteerV2_Event = {}
  XerionSteerV2_Event_mt = Class(XerionSteerV2_Event, Event)
 
  InitEventClass(XerionSteerV2_Event, "XerionSteerV2_Event")
  
  function XerionSteerV2_Event:emptyNew()
      local self = Event:new(XerionSteerV2_Event_mt)
      return self
  end
  
  function XerionSteerV2_Event:new(object, lr,mode)
      local self = XerionSteerV2_Event:emptyNew()
      self.lr = lr
      self.mode = mode
      
      self.object = object
      return self
  end
  
  function XerionSteerV2_Event:readStream(streamId, connection)
      local id = streamReadInt32(streamId)
      self.lr = streamReadInt8(streamId)
      self.mode = streamReadInt8(streamId)

  
      self.object = networkGetObject(id)
      self:run(connection)
  end
  
  function XerionSteerV2_Event:writeStream(streamId, connection)
      streamWriteInt32(streamId, networkGetObjectId(self.object))
      streamWriteInt8(streamId, self.lr)
      streamWriteInt8(streamId, self.mode)

  end
  
  function XerionSteerV2_Event:run(connection)
      self.object.XS_LR = self.lr
      self.object.XSV2Mode = self.mode
      
      if not connection:getIsServer() then
          g_server:broadcastEvent(XerionSteerV2_Event:new(self.object, self.lr, self.mode), nil, connection, self.object)
      end
  end