Spinner and engine identification lua code, version 3.0

--[[
Spinner and engine index identification, version 3.0
Created by Madwand 9/23/2015
This code will help you discover the indices of spinners and
propulsion on your aircraft, for use with the "VTSpinners",
"VTOLSpinners", and "VTOLEngines" settings for the advanced
aerial AI.
Note that the potential pitch and roll engines identified
do not include any on spin blocks, which must be identified
with the "VTOLSpinners" setting.
The potential spinner and engine reports are just there so you
know which spinners/engines may end up used for what purpose.
The engine/spinner index identified is what is important.
--]]


RollSpinners = {}
PitchSpinners = {}
YawSpinners = {}
AllSpinners = {}
RollEngines = {}
PitchEngines = {}
SpinnerStartRotations={}
firstpass = true

function Update(I)
  CoM = I:GetConstructCenterOfMass()
  ForwardV = I:GetConstructForwardVector()
  if firstpass then
    firstpass = false
    for p = 0, I:GetSpinnerCount() - 1 do
      ClassifySpinner(I,p)
    end
    PrintList(I,'Potential roll spinners',RollSpinners)
    PrintList(I,'Potential pitch spinners',PitchSpinners)
    PrintList(I,'Potential yaw spinners',YawSpinners)
    for p = 0, I:Component_GetCount(9) - 1 do
      ClassifyEngine(I,p,false)
    end
    PrintList(I,"Potential roll engines",RollEngines)
    PrintList(I,"Potential pitch engines",PitchEngines)
  end
end

function ClassifySpinner(I,p)
  local info = I:GetSpinnerInfo(p)
  local h,a,b = EulerAngles(info.LocalRotation)
  local pos = info.LocalPositionRelativeToCom
  SpinnerStartRotations[p]=info.LocalRotation
  a=math.floor(a+.5)
  b=math.floor(b+.5)
  I:Log(string.format("Spinner index: %d Orientation: (%.2f, %.2f, %.2f) Position: (%.2f, %.2f, %.2f)", p, h, a, b, pos.x, pos.y, pos.z))
  if (a==0 and b==0) then
    tinsert(pos.z,YawSpinners,1,-1,p)
    table.insert(AllSpinners,p)
  elseif (a==0 and math.abs(b)>170) then
    tinsert(pos.z,YawSpinners,-1,1,p)
    table.insert(AllSpinners,p)
  else
    local h,a,b = EulerAngles(info.LocalRotation*Quaternion.Euler(0, 0, 90))
    h=math.floor(h+.5)
    a=math.floor(a+.5)
    if (h==0 and a==0) then -- pointed right
      tinsert(pos.z,PitchSpinners,1,-1,p)
      if VTOLSpinners=='all' then tinsert(pos.z,DownSpinners,-1,-1,p) end
      if pos.x<-1 then  -- on left
        tinsert(pos.z,RollSpinners,-1,-1,p)
      elseif pos.x>1 then -- on right
        tinsert(pos.z,RollSpinners,1,1,p)
      end
      table.insert(AllSpinners,p)
    elseif (a==0 and math.abs(h)>170) then -- pointed left
      tinsert(pos.z,PitchSpinners,-1,1,p)
      if VTOLSpinners=='all' then tinsert(pos.z,DownSpinners,1,1,p) end
      if pos.x<-1 then -- on left
        tinsert(pos.z,RollSpinners,1,1,p)
      elseif pos.x>1 then -- on right
        tinsert(pos.z,RollSpinners,-1,-1,p)
      end
      table.insert(AllSpinners,p)
    end
  end
end

function GetPosRelativeToCoM(pos)
  local rot = Quaternion.Inverse(Quaternion.LookRotation(ForwardV))
  local rpos = rot*(pos-CoM)
  for i, p in ipairs(rpos) do rpos[i]=math.floor(p*10+.5)/10 end
  return rpos
end

function ClassifyEngine(I,p,force)
  local info = I:Component_GetBlockInfo(9,p)
  local h,a,b = EulerAngles(info.LocalRotation)
  local rot = Quaternion.Inverse(Quaternion.LookRotation(I:GetConstructForwardVector()))
  local pos = GetPosRelativeToCoM(info.Position)
  df = I:Component_GetFloatLogic(9,p)
  I:Log(string.format("Engine index: %d  Drive fraction: %.02f Orientation: (%.2f, %.2f, %.2f) Position: (%f, %f, %f)", p, df, h, a, b, pos.x, pos.y, pos.z))
  if (force or math.floor(b+.5)==-90) then
    tinsert(pos.z,PitchEngines,1,-1,p)
    if pos.x<-1.1 then  -- on left
      tinsert(pos.z,RollEngines,-1,-1,p)
    elseif pos.x>1.1 then -- on right
      tinsert(pos.z,RollEngines,1,1,p)
    end
  end
end

function tinsert(z,s,x,y,p)
  s[p] = z>0 and x or y
end

function PrintList(I, name, list)
  str = name..': '
  for i,v in pairs(list) do
    str = str..string.format("%d ", i)
  end
  I:Log(str)
end

function EulerAngles(q1)
  local sqw = q1.w*q1.w
  local sqx = q1.x*q1.x
  local sqy = q1.y*q1.y
  local sqz = q1.z*q1.z
  local unit = sqx + sqy + sqz + sqw --if normalised is one, otherwise is correction factor
  local test = q1.x*q1.y + q1.z*q1.w
  local heading, attitude, bank
  if (test > 0.499*unit) then --singularity at north pole
    heading = 2 * math.atan2(q1.x,q1.w)
    attitude = math.pi/2;
    bank = 0
  elseif (test < -0.499*unit) then --singularity at south pole
    heading = -2 * math.atan2(q1.x,q1.w)
    attitude = -math.pi/2
    bank = 0
  else
    heading = math.atan2(2*q1.y*q1.w-2*q1.x*q1.z , sqx - sqy - sqz + sqw)
    attitude = math.asin(2*test/unit)
    bank = math.atan2(2*q1.x*q1.w-2*q1.y*q1.z , -sqx + sqy - sqz + sqw)
  end
  return math.deg(heading), math.deg(attitude), math.deg(bank)
end