--[[
Name = Attitude RCS
Script = Attitude.cfg
Key = 0x19 ; 'T'
Persist = vessel
END_PARSE
--]]


vi = vessel.get_focusinterface()

page = 1      -- MFD page index
navrec = 1    -- NAV receiver index
actmode = nil -- active attitude mode
t_warn = nil
txt_warn = nil
vessel_rot_stage = {}
vessel_rot_stage_idx = 0
R_premul = mat.identity()
R_curr   = mat.identity()

-- display geometry
hilight = 70*65536 + 70*256 + 70

vaxis = {'pitch','yaw','roll'}
modestr = {'prograde','retrograde','normal','antinormal','perpendicular (in)','perpendicular (out)','radial (down)','radial (up)','current (relative)','current (absolute)','dock sync'}

max_angmoment = {}
for i=THGROUP.ATT_PITCHUP,THGROUP.ATT_BANKRIGHT do
  nts = vi:get_groupthrustercount(i)
  M = vec.set(0,0,0)
  for j=0,nts-1 do
    ht = vi:get_groupthruster(i,j)
    dir = vi:get_thrusterdir(ht)
    ref = vi:get_thrusterpos(ht)
    maxth0 = vi:get_thrustermax0(ht)
    M = vec.add (M, vec.crossp(vec.mul(dir,maxth0), ref))
  end
  max_angmoment[i] = vec.length(M)
end

delta_angle = 0
delta_align = 0

function setup(w,h)
  dispw = w
  disph = h
  ch=1
  cw=1
end

-- List of MFD buttons and menu entries for all pages

btn_label_1 = {'SET','DCK','SUS','END','CFG'}
btn_menu_1 = {
  {l1='Set attitude mode',sel='s'},
  {l1='Dock sync mode',sel='d'},
  {l1='Suspend active mode',sel='p'},
  {l1='Cancel active mode',sel='x'},
  {l1='Configure vessel',sel='c'}
}

btn_label_2 = {'+R','-R','AX','+V','-V','UP','DN','GO','RTN'}
btn_menu_2 = {
  {l1='Add rotation',sel='a'},
  {l1='Delete rotation',sel='x'},
  {l1='Change rotation',l2='axis',sel='c'},
  {l1='Increase rotation',l2='angle',sel='.'},
  {l1='Decrease rotation',l2='angle',sel=','},
  {l1='Selection up',sel='u'},
  {l1='Selection down',sel='d'},
  {l1='Activate configuration',sel='g'},
  {l1='Return to',l2='main page',sel='r'}
}

btn_label_3 = {'BAS','INV','CUR','+R','-R','+V','-V','AX','UP','DN','GO','RTN'}
btn_menu_3 = {
  {l1='Next base mode',sel='b'},
  {l1='Invert base mode',sel='i'},
  {l1='Keep current',l2='attitude',sel='k'},
  {l1='Add rotation',sel='a'},
  {l1='Delete rotation',sel='x'},
  {l1='Increase rotation',l2='angle',sel='.'},
  {l1='Decrease rotation',l2='angle',sel=','},
  {l1='Change rotation',l2='axis',sel='c'},
  {l1='Selection up',sel='u'},
  {l1='Selection down',sel='d'},
  {l1='Activate mode',sel='g'},
  {l1='Return to',l2='main page',sel='r'}
}

btn_label_4 = {'NAV','ACT','CNC','RTN'}
btn_menu_4 = {
  {l1='NAV receiver',l2='select',sel='n'},
  {l1='Activate dock',l2='alignment mode',sel='a'},
  {l1='Cancel active mode',sel='x'},
  {l1='Return to',l2='main page',sel='r'}
}

function buttonlabel(bt)
  if page == 1 then
    if bt < 5 then
      return btn_label_1[bt+1]
    end
  elseif page == 2 then
    if bt < 9 then
      return btn_label_2[bt+1]
    end
  elseif page == 3 then
    if bt < 12 then
      return btn_label_3[bt+1]
    end
  elseif page == 4 then
    if bt < 4 then
      return btn_label_4[bt+1]
    end
  end
  return nil
end

function buttonmenu ()
  if page == 1 then
    return btn_menu_1,5
  elseif page == 2 then
    return btn_menu_2,9
  elseif page == 3 then
    return btn_menu_3,12
  else
    return btn_menu_4,4
  end
end

function consumebutton(bt,event)
  return consumebutton_func[page](bt,event)
end

function consumekeybuffered(key)
  return consumekeybuffered_func[page](key)
end

function update(skp)
  mfd:set_title(skp,'RCS Attitude')
  skp:set_font(mfd:get_defaultfont(0))
  skp:set_textcolor(256*255)
  ch,cw = skp:get_charsize()
  update_func[page](skp)
  if t_warn ~= nil then
    if t_warn > oapi.get_simtime() then
      skp:set_backgroundmode(SKP.OPAQUE)
      skp:set_backgroundcolor(128)
      skp:set_textcolor(65536*255+256*255+255)
      t = '  '..txt_warn..'  '
      skp:text(cw*(38-#t)/2,ch*10,t,#t)
    else
      t_warn = nil
    end
  end
  return true
end

function poststep(simt,simtd,mjd)
  if actmode ~= nil and actmode.suspend ~= true then
    rotation_func[actmode.base]()
  end
end

function storestatus()
  -- oapi.dbg_out('storestatus called at '..oapi.get_simtime())
end

function recallstatus()
  -- oapi.dbg_out('recallstatus called at '..oapi.get_simtime())
end

-- Page 1: MFD main page

function consumebutton1(bt,event)
  if bt==0 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setpage_defmode()
    end
    return true
  elseif bt==1 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setpage_dock()
    end
    return true
  elseif bt==2 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      set_suspendmode()
    end
    return true
  elseif bt==3 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      set_cancelmode()
    end
    return true
  elseif bt==4 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setpage_config()
    end
    return true
  end
  return false
end

function consumekeybuffered1(key)
  if key==OAPI_KEY.S then
    setpage_defmode()
    return true
  elseif key==OAPI_KEY.D then
    setpage_dock()
    return true
  elseif key==OAPI_KEY.P then
    set_suspendmode()
    return true
  elseif key==OAPI_KEY.X then
    set_cancelmode()
    return true
  elseif key==OAPI_KEY.C then
    setpage_config()
    return true
  else
    return false
  end
end

function update1(skp)
  if actmode ~= nil then
    if actmode.suspend == false then
      skp:text(cw,ch*3,'Active mode:',12)
    else
      skp:text(cw,ch*3,'Suspended',9)
      skp:text(cw,ch*4,'mode:',5)
    end
    t = modestr[actmode.base]
    skp:text(cw*14,ch*3,t,#t)
    if actmode.base == 11 then
      update_navinfo(skp)
    else
      for i=1,#actmode.stage do
        t = string.format('+ %5s %+4.0f', vaxis[actmode.stage[i].axis], actmode.stage[i].angle*DEG)
        skp:text(cw*14,ch*(3+i),t,#t)
      end
    end
    skp:text(cw,disph-(ch*5)/2,'Deviation:',10)
    t = string.format('Dr = %06.2fº',delta_angle*DEG)
    skp:text(cw*14,disph-ch*3,t,#t)
    t = string.format('Da = %06.2fº',delta_align*DEG)
    skp:text(cw*14,disph-ch*2,t,#t)
  else
    skp:text(cw,ch*3,'Active mode: none',17)
  end
end


-- Page 2: Configure vessel orientation

function consumebutton2(bt,event)
  if bt==0 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      config_addrotation()
    end
    return true
  elseif bt==1 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      config_delrotation()
    end
    return true
  elseif bt==2 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      config_rotaxis()
    end
    return true
  elseif bt==3 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      config_incvalue()
    end
    return true
  elseif bt==4 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      config_decvalue()
    end
    return true
  elseif bt==5 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      if vessel_rot_stage_idx > 0 then
        vessel_rot_stage_idx = vessel_rot_stage_idx-1
        mfd:invalidate_display()
      end
    end
    return true  
  elseif bt==6 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      if vessel_rot_stage_idx < #vessel_rot_stage then
        vessel_rot_stage_idx = vessel_rot_stage_idx+1
        mfd:invalidate_display()
      end
    end
    return true
  elseif bt==7 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      config_apply()
    end
    return true
  elseif bt==8 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setpage_main()
    end
    return true
  end
  return false
end

function consumekeybuffered2(key)
  return false
end

function update2(skp)
  skp:text(cw,ch*2,'Set vessel configuration',24)
  skp:set_backgroundmode(SKP.OPAQUE)
  skp:set_backgroundcolor(hilight)
  skp:text(cw,ch*(4+vessel_rot_stage_idx),'                    ',20)
  skp:set_backgroundmode(SKP.TRANSPARENT)
  skp:set_backgroundcolor(0)
  skp:text(cw,ch*4,'Standard orientation',20)
  for i=1,#vessel_rot_stage do
    t = '+ '..vaxis[vessel_rot_stage[i].axis]
    skp:text(cw,ch*(4+i),t,#t)
    t = tostring(vessel_rot_stage[i].angle*DEG)
    skp:text(cw*9,ch*(4+i),t,#t)
  end
end


-- Page 3: Define attitude mode

function consumebutton3(bt,event)
  if bt==0 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_nextbasemode()
    end
    return true
  elseif bt==1 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_invbasemode()
    end
    return true
  elseif bt==2 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_curratt()
    end
    return true
  elseif bt==3 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_addrotation()
    end
    return true
  elseif bt==4 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_delrotation()
    end
    return true
  elseif bt==5 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_incvalue()
    end
    return true
  elseif bt==6 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_decvalue()
    end
    return true
  elseif bt==7 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_rotaxis()
    end
    return true
  elseif bt==8 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_selup()
    end
    return true
  elseif bt==9 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_seldown()
    end
    return true
  elseif bt==10 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setmode_activate()
    end
    return true
  elseif bt==11 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setpage_main()
    end
    return true
  end
  return false
end
  
function consumekeybuffered3(key)
  return false
end

function update3(skp)
  skp:set_backgroundmode(SKP.OPAQUE)
  skp:set_backgroundcolor(hilight)
  skp:text(cw*10,ch*(3+defmode.curstage),'                    ',20)
  skp:set_backgroundmode(SKP.TRANSPARENT)
  skp:set_backgroundcolor(0)

  skp:text(cw/2,ch,'Define attitude mode',20)
  t = 'Basemode: '..modestr[defmode.base]
  skp:text(cw/2,ch*3,t,#t)
  for i=1,#defmode.stage do
    t = string.format('+ %5s %+4.0f', vaxis[defmode.stage[i].axis], defmode.stage[i].angle*DEG)
    skp:text((cw*21)/2,ch*(3+i),t,#t)
  end
end


-- Page 4: Sync with dock

function consumebutton4(bt,event)
  if bt==0 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      dock_nav()
    end
    return true
  elseif bt==1 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      dock_act()
    end
    return true
  elseif bt==2 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      dock_cnc()
    end
    return true
  elseif bt==3 then
    if event%PANEL_MOUSE.LBPRESSED == PANEL_MOUSE.LBDOWN then
      setpage_main()
    end
    return true
  end
  return false
end

function consumekeybuffered4(key)
  return false
end
  
function update4(skp)
  update1(skp)
  if actmode == nil or actmode.base ~= 11 then
    update_navinfo(skp)
  end
end

function update_navinfo(skp)
  chn = vi:get_navchannel(navrec-1)
  f = chn*0.05+108
  t = string.format('NAV%d %0.2fMHz',navrec,f)
  skp:text(dispw-cw*15,0,t,#t)
  hnav = vi:get_navsource(navrec-1)
  if hnav~=nil then
    ndata = oapi.get_navdata(hnav)
    t = ndata.descr
    if ndata.type ~= TRANSMITTER.IDS then
      skp:set_textcolor(256*128+128)
    end
  else
    t = 'No signal'
    skp:set_textcolor(256*128+128)
  end
  skp:text(dispw-cw*(#t+1),ch,t,#t)
  skp:set_textcolor(256*255)
end

function set_page(p)
  page = p
  mfd:invalidate_buttons()
  mfd:invalidate_display()
end

function set_suspendmode()
  if actmode ~= nil then
    if actmode.suspend == true then
      actmode.suspend = false
    else
      actmode.suspend = true
    end
    mfd:invalidate_display()
  end
end

function set_cancelmode()
  if actmode ~= nil then
    end_navmode()
    mfd:invalidate_display()
  end
end

function setmode_nextbasemode()
  if defmode.base%2 == 1 then
    defmode.base = defmode.base+2
  else
    defmode.base = defmode.base+1
  end
  if defmode.base > 8 then
    defmode.base = 1
  end
  mfd:invalidate_display()
end

function setmode_invbasemode()
  if defmode.base%2 == 1 then
    defmode.base = defmode.base+1
  else
    defmode.base = defmode.base-1
  end
  mfd:invalidate_display()
end

function setmode_curratt()
  if defmode.base == 9 then
    defmode.base = 10
  else
    defmode.base = 9
  end
  mfd:invalidate_display()
end

function setmode_addrotation()
  idx = defmode.curstage+1
  for i=#defmode.stage,idx,-1 do
    defmode.stage[i+1] = defmode.stage[i]
  end
  defmode.stage[idx] = {}
  defmode.stage[idx].axis = 1
  defmode.stage[idx].angle = 0
  defmode.curstage = defmode.curstage+1
  mfd:invalidate_display()
end

function setmode_delrotation()
  if defmode.curstage > 0 then
    for i = defmode.curstage,#defmode.stage do
      defmode.stage[i] = defmode.stage[i+1]
    end
    if defmode.curstage > #defmode.stage then
      defmode.curstage = #defmode.stage
    end
    mfd:invalidate_display()
  end
end

function setmode_incvalue()
  idx = defmode.curstage
  if idx > 0 then
    defmode.stage[idx].angle = defmode.stage[idx].angle+RAD
    mfd:invalidate_display()
  end
end

function setmode_decvalue()
  idx = defmode.curstage
  if idx > 0 then
    defmode.stage[idx].angle = defmode.stage[idx].angle-RAD
    mfd:invalidate_display()
  end
end

function setmode_rotaxis()
  idx = defmode.curstage
  if idx > 0 then
    defmode.stage[idx].axis = (defmode.stage[idx].axis%3)+1
    mfd:invalidate_display()
  end
end

function setmode_selup()
  if defmode.curstage > 0 then
    defmode.curstage = defmode.curstage-1
    mfd:invalidate_display()
  end
end

function setmode_seldown()
  if defmode.curstage < #defmode.stage then
    defmode.curstage = defmode.curstage+1
    mfd:invalidate_display()
  end
end

function setmode_activate()
  actmode = {}
  actmode.base = defmode.base
  actmode.stage = defmode.stage
  actmode.suspend = false
  if actmode.base == 9 then
    register_attcurr_relative()
  elseif actmode.base == 10 then
    register_attcurr_absolute()
  end
  set_transform()
end

function setpage_main()
  set_page(1)
end

function setpage_defmode()
  defmode = {}
  if actmode ~= nil and actmode.base <= 8 then
    defmode.base = actmode.base
    defmode.stage = actmode.stage
  else
    defmode.base = 1
    defmode.stage = {}
  end
  defmode.curstage = 0
  set_page(3)
end

function setpage_dock()
  set_page(4)
end

function setpage_config()
  set_page(2)
end

function config_addrotation()
  idx = vessel_rot_stage_idx+1
  for i=#vessel_rot_stage,idx,-1 do
    vessel_rot_stage[i+1] = vessel_rot_stage[i]
  end
  vessel_rot_stage[idx] = {}
  vessel_rot_stage[idx].axis = 1
  vessel_rot_stage[idx].angle = 0
  vessel_rot_stage_idx = vessel_rot_stage_idx+1
  mfd:invalidate_display()
end

function config_delrotation()
  if vessel_rot_stage_idx > 0 then
    for i = vessel_rot_stage_idx,#vessel_rot_stage do
      vessel_rot_stage[i] = vessel_rot_stage[i+1]
    end
    if vessel_rot_stage_idx > #vessel_rot_stage then
      vessel_rot_stage_idx = #vessel_rot_stage
    end
    set_premul()
    mfd:invalidate_display()
  end
end

function config_rotaxis()
  i = vessel_rot_stage_idx
  if i > 0 then
    vessel_rot_stage[i].axis = (vessel_rot_stage[i].axis % 3)+1
    mfd:invalidate_display()
  end
end

function config_incvalue()
  i = vessel_rot_stage_idx
  if i > 0 then
    vessel_rot_stage[i].angle = vessel_rot_stage[i].angle + 1.0*RAD
    mfd:invalidate_display()
  end
end

function config_decvalue()
  i = vessel_rot_stage_idx
  if i > 0 then
    vessel_rot_stage[i].angle = vessel_rot_stage[i].angle - 1.0*RAD
    mfd:invalidate_display()
  end
end

function config_apply()
  set_premul()
end

function dock_nav()
  if actmode ~= nil and actmode.base == 11 then
    end_navmode()
  end
  navrec = navrec+1
  if navrec > vi:get_navcount() then
    navrec = 1
  end
  mfd:invalidate_display()
end

function dock_act()
  if actmode == nil or actmode.base ~= 11 then
    hnav = vi:get_navsource(navrec-1)
    if hnav ~= nil then
      ndata = oapi.get_navdata(hnav)
      if ndata.type == TRANSMITTER.IDS then
        actmode = {}
        actmode.base = 11
        actmode.suspend = false
        mfd:invalidate_display()
        return
      end
    end
    t_warn = oapi.get_simtime()+3
    txt_warn = 'No IDS signal'
    mfd:invalidate_display()
  end
end

function dock_cnc()
  if actmode ~= nil then
    end_navmode()
    mfd:invalidate_display()
  end
end

function end_navmode()
  actmode = nil
  -- for i=THGROUP.ATT_PITCHUP,THGROUP.ATT_BANKRIGHT do
  --   vi:set_thrustergrouplevel(i,0)
  -- end
end

function set_premul()
  R_premul = mat.identity()
  for i=1,#vessel_rot_stage do
    sina = math.sin(vessel_rot_stage[i].angle)
    cosa = math.cos(vessel_rot_stage[i].angle)
    if vessel_rot_stage[i].axis == 1 then
      R = {m11=1, m12=0, m13=0, m21=0, m22=cosa, m23=-sina, m31=0, m32=sina, m33=cosa}
    elseif vessel_rot_stage[i].axis == 2 then
      R = {m11=cosa, m12=0, m13=-sina, m21=0, m22=1, m23=0, m31=sina, m32=0, m33=cosa}
    else
      R = {m11=cosa, m12=-sina, m13=0, m21=sina, m22=cosa, m23=0, m31=0, m32=0, m33=1}
    end
    R_premul = mat.mmul(R, R_premul)
  end
  if actmode ~= nil then
    set_transform()
  end
end

function set_transform()
  actmode.trans = mat.identity()
  if actmode.base == 9 or actmode.base == 10 then
    actmode.trans = R_curr
  end
  for i=1,#actmode.stage do
    sina = math.sin(actmode.stage[i].angle)
    cosa = math.cos(actmode.stage[i].angle)
    if actmode.stage[i].axis == 1 then
      R = {m11=1, m12=0, m13=0, m21=0, m22=cosa, m23=-sina, m31=0, m32=sina, m33=cosa}
    elseif actmode.stage[i].axis == 2 then
      R = {m11=cosa, m12=0, m13=-sina, m21=0, m22=1, m23=0, m31=sina, m32=0, m33=cosa}
    else
      R = {m11=cosa, m12=-sina, m13=0, m21=sina, m22=cosa, m23=0, m31=0, m32=0, m33=1}
    end
    actmode.trans = mat.mmul(R, actmode.trans)
  end
  if actmode.base <= 8 then
    actmode.trans = mat.mmul(R_premul, actmode.trans)
  end
end

function register_attcurr_relative()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = mat.tmul(R, vec.unit (vi:get_relativevel(hCbody)))
  r = mat.tmul(R, vec.unit (vi:get_relativepos(hCbody)))
  n = vec.unit (vec.crossp (v,r))
  p = vec.unit (vec.crossp (n,v))
  R_curr = {m11=p.x, m12=n.x, m13=v.x, m21=p.y, m22=n.y, m23=v.y, m31=p.z, m32=n.z, m33=v.z}
end

function register_attcurr_absolute()
  R = vi:get_rotationmatrix()
  v = mat.tmul(R, {x=0,y=0,z=1})
  n = mat.tmul(R, {x=0,y=1,z=0})
  p = vec.unit (vec.crossp (n,v))
  R_curr = {m11=p.x, m12=n.x, m13=v.x, m21=p.y, m22=n.y, m23=v.y, m31=p.z, m32=n.z, m33=v.z}
end


function rot_to_frame(dir,rot)
  dvmax = 1250/(250+math.max(1,oapi.get_tacc()))*RAD
  -- fudge: more damping at higher time acceleration to improve stability
  alpha0 = 20*RAD
  f0 = dvmax/alpha0

  av = vi:get_angvel()
  mass = vi:get_mass()
  pmi = vi:get_pmi()

  tgtdir = dir
  delta_angle = math.acos(vec.dotp(tgtdir,{x=0,y=0,z=1}))
  
  v1 = f0 * math.asin(tgtdir.y)
  if v1 >= 0 then
    dth = (math.min(v1,dvmax)-av.x)/max_angmoment[THGROUP.ATT_PITCHDOWN]
  else
    dth = (math.max(v1,-dvmax)-av.x)/max_angmoment[THGROUP.ATT_PITCHUP]
  end
  idt = 1.0/oapi.get_simstep()
  dth = dth * idt * pmi.x * mass
  if dth >= 0 then
    vi:inc_thrustergrouplevel_singlestep(THGROUP.ATT_PITCHUP, math.min(dth,0.4))
  else
    vi:inc_thrustergrouplevel_singlestep(THGROUP.ATT_PITCHDOWN, math.min(-dth,0.4))
  end
  
  v1 = -f0 * math.atan2(tgtdir.x,tgtdir.z)
  if v1 >= 0 then
    dth = (math.min(v1,dvmax)-av.y)/max_angmoment[THGROUP.ATT_YAWRIGHT];
  else
    dth = (math.max(v1,-dvmax)-av.y)/max_angmoment[THGROUP.ATT_YAWLEFT];
  end
  dth = dth * idt * pmi.y * mass
  if dth >= 0 then
    vi:inc_thrustergrouplevel_singlestep(THGROUP.ATT_YAWLEFT, math.min(dth,0.4))
  else
    vi:inc_thrustergrouplevel_singlestep(THGROUP.ATT_YAWRIGHT, math.min(-dth,0.4))
  end

  tgtdir = rot  
  delta_align = math.acos(vec.dotp(tgtdir,{x=0,y=1,z=0}))

  v1 = math.asin(tgtdir.x) * f0
  if v1 >= 0 then
    dth = (math.min(v1,dvmax)-av.z)/max_angmoment[THGROUP.ATT_BANKRIGHT]
  else
    dth = (math.max(v1,-dvmax)-av.z)/max_angmoment[THGROUP.ATT_BANKLEFT]
  end
  dth = dth * idt * pmi.z * mass
  if dth >= 0 then
    vi:inc_thrustergrouplevel_singlestep(THGROUP.ATT_BANKRIGHT, math.min(dth,0.4))
  else
    vi:inc_thrustergrouplevel_singlestep(THGROUP.ATT_BANKLEFT, math.min(-dth,0.4))
  end
end


function rot_prograde()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = vi:get_relativevel(hCbody)
  r = vi:get_relativepos(hCbody)
  n = vec.crossp (v,r)
  tgtdir = vec.unit (mat.tmul (actmode.trans, mat.tmul(R,v)))  -- prograde direction in vessel frame
  tgtrot = vec.unit (mat.tmul (actmode.trans, mat.tmul(R,n)))  -- normal direction in vessel frame
  rot_to_frame(tgtdir,tgtrot)
end

function rot_retrograde()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = vi:get_relativevel(hCbody)
  r = vi:get_relativepos(hCbody)
  tgtdir = vec.unit (mat.tmul (actmode.trans, mat.tmul(R,v)))
  tgtdir = {x=-tgtdir.x, y=-tgtdir.y, z=-tgtdir.z}        -- retrograde direction in vessel frame
  tgtrad = vec.unit (mat.tmul (actmode.trans, mat.tmul(R,r)))
  tgtrad = {x=-tgtrad.x, y=-tgtrad.y, z=-tgtrad.z}        -- radial direction in vessel frame
  rot_to_frame(tgtdir,vec.crossp(tgtrad,tgtdir))
end

function rot_normal()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = vi:get_relativevel(hCbody)
  r = vi:get_relativepos(hCbody)
  tgtdir = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, vec.crossp (v,r)))) -- normal direction in vessel frame
  tgtrot = vec.unit (mat.tmul (actmode.trans, mat.tmul(R,v)))
  rot_to_frame(tgtdir,tgtrot)
end

function rot_antinormal()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = vi:get_relativevel(hCbody)
  r = vi:get_relativepos(hCbody)
  tgtdir = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, vec.crossp (v,r))))
  tgtdir = {x=-tgtdir.x, y=-tgtdir.y, z=-tgtdir.z}        -- antinormal direction in vessel frame
  tgtrot = vec.unit (mat.tmul (actmode.trans, mat.tmul(R,v)))
  rot_to_frame(tgtdir,tgtrot)
end

function rot_perpin()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = vi:get_relativevel(hCbody)
  r = vi:get_relativepos(hCbody)
  n = vec.crossp (v,r)
  p = vec.crossp (v,n)
  tgtdir = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, p)))
  tgtrot = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, v)))
  rot_to_frame(tgtdir,tgtrot)
end

function rot_perpout()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = vi:get_relativevel(hCbody)
  r = vi:get_relativepos(hCbody)
  n = vec.crossp (v,r)
  p = vec.crossp (n,v)
  tgtdir = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, p)))
  tgtrot = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, {x=-v.x, y=-v.y, z=-v.z})))
  rot_to_frame(tgtdir,tgtrot)
end

function rot_radialin()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = vi:get_relativevel(hCbody)
  r = vi:get_relativepos(hCbody)
  tgtdir = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, {x=-r.x, y=-r.y, z=-r.z})))
  tgtrot = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, v)))
  rot_to_frame(tgtdir,tgtrot)
end

function rot_radialout()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = vi:get_relativevel(hCbody)
  r = vi:get_relativepos(hCbody)
  tgtdir = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, r)))
  tgtrot = vec.unit (mat.tmul (actmode.trans, mat.tmul (R, {x=-v.x, y=-v.y, z=-v.z})))
  rot_to_frame(tgtdir,tgtrot)
end

function rot_relcurr()
  hCbody = vi:get_gravityref()
  R = vi:get_rotationmatrix()
  v = vi:get_relativevel(hCbody)
  r = vi:get_relativepos(hCbody)
  n = vec.crossp (v,r)
  tgtdir = vec.unit (mat.tmul (actmode.trans, mat.tmul(R,v)))  -- prograde direction in vessel frame
  tgtrot = vec.unit (mat.tmul (actmode.trans, mat.tmul(R,n)))  -- normal direction in vessel frame
  rot_to_frame(tgtdir,tgtrot)
end

function rot_abscurr()
  R = vi:get_rotationmatrix()
  v = mat.tmul(R, {x=0,y=0,z=1})
  n = mat.tmul(R, {x=0,y=1,z=0})
  tgtdir = vec.unit (mat.tmul (actmode.trans, v))  -- prograde direction in vessel frame
  tgtrot = vec.unit (mat.tmul (actmode.trans, n))  -- normal direction in vessel frame
  rot_to_frame(tgtdir,tgtrot)
end


function rot_docksync()
  hnav = vi:get_navsource(navrec-1)
  if hnav == nil or oapi.get_navdata(hnav).type ~= TRANSMITTER.IDS then
    end_navmode()
    t_warn = oapi.get_simtime()+3
    txt_warn = 'IDS signal lost'
    return
  end
  hdock = ndata.hdock
  hdockvessel = ndata.hvessel
  dvi = vessel.get_interface(hdockvessel)
  pos,dir,rot = dvi:get_dockparams(hdock)
  Rt = dvi:get_rotationmatrix()
  R  = vi:get_rotationmatrix()
  dt = mat.tmul(R,mat.mul(Rt,dir)) -- target dock direction vector in vessel frame
  dt = {x=-dt.x, y=-dt.y, z=-dt.z}
  rt = mat.tmul(R,mat.mul(Rt,rot)) -- target dock alignment vector in vessel frame
  -- rt = {x=-rt.x, y=-rt.y, z=-rt.z}
  qt = vec.crossp(rt,dt)
  
  hdock0 = vi:get_dockhandle(0)
  p,ds,rs = vi:get_dockparams(hdock0)
  qs = vec.crossp(rs,ds)
  
  -- now for some tedious linear algebra:
  -- create rotation matrix that rotates source to target docking frame
  
  Rd = {}
  den = -ds.z*qs.y*rs.x + ds.y*qs.z*rs.x + ds.z*qs.x*rs.y - ds.x*qs.z*rs.y - ds.y*qs.x*rs.z + ds.x*qs.y*rs.z
  Rd.m11 = (-dt.x*qs.z*rs.y + ds.z*qt.x*rs.y + dt.x*qs.y*rs.z - ds.y*qt.x*rs.z - ds.z*qs.y*rt.x + ds.y*qs.z*rt.x)/den
  Rd.m12 = ( dt.x*qs.z*rs.x - ds.z*qt.x*rs.x - dt.x*qs.x*rs.z + ds.x*qt.x*rs.z + ds.z*qs.x*rt.x - ds.x*qs.z*rt.x)/den
  Rd.m13 = (-dt.x*qs.y*rs.x + ds.y*qt.x*rs.x + dt.x*qs.x*rs.y - ds.x*qt.x*rs.y - ds.y*qs.x*rt.x + ds.x*qs.y*rt.x)/den
  Rd.m21 = (-dt.y*qs.z*rs.y + ds.z*qt.y*rs.y + dt.y*qs.y*rs.z - ds.y*qt.y*rs.z - ds.z*qs.y*rt.y + ds.y*qs.z*rt.y)/den
  Rd.m22 = ( dt.y*qs.z*rs.x - ds.z*qt.y*rs.x - dt.y*qs.x*rs.z + ds.x*qt.y*rs.z + ds.z*qs.x*rt.y - ds.x*qs.z*rt.y)/den
  Rd.m23 = (-dt.y*qs.y*rs.x + ds.y*qt.y*rs.x + dt.y*qs.x*rs.y - ds.x*qt.y*rs.y - ds.y*qs.x*rt.y + ds.x*qs.y*rt.y)/den
  Rd.m31 = (-dt.z*qs.z*rs.y + ds.z*qt.z*rs.y + dt.z*qs.y*rs.z - ds.y*qt.z*rs.z - ds.z*qs.y*rt.z + ds.y*qs.z*rt.z)/den
  Rd.m32 = ( dt.z*qs.z*rs.x - ds.z*qt.z*rs.x - dt.z*qs.x*rs.z + ds.x*qt.z*rs.z + ds.z*qs.x*rt.z - ds.x*qs.z*rt.z)/den
  Rd.m33 = (-dt.z*qs.y*rs.x + ds.y*qt.z*rs.x + dt.z*qs.x*rs.y - ds.x*qt.z*rs.y - ds.y*qs.x*rt.z + ds.x*qs.y*rt.z)/den

  -- now use the rotation matrix to create synthetic target vectors
  
  tgtdir = mat.mul(Rd,{x=0,y=0,z=1})
  tgtrot = mat.mul(Rd,{x=0,y=1,z=0})  
  -- Rpr = {m11=qs.x, m12=rs.x, m13=ds.x, m21=qs.y, m22=rs.y, m23=ds.y, m31=qs.z, m32=rs.z, m33=ds.z}
  -- tgtdir = mat.tmul(Rpr,{x=-dt.x,y=-dt.y,z=-dt.z})
  -- tgtrot = mat.tmul(Rpr,{x=-rt.x,y=-rt.y,z=-rt.z})
  rot_to_frame(tgtdir,tgtrot)
end


update_func = {
  update1,
  update2,
  update3,
  update4
}
consumebutton_func = {
  consumebutton1,
  consumebutton2,
  consumebutton3,
  consumebutton4
}
consumekeybuffered_func = {
  consumekeybuffered1,
  consumekeybuffered2,
  consumekeybuffered3,
  consumekeybuffered4
}
rotation_func = {
  rot_prograde,
  rot_retrograde,
  rot_normal,
  rot_antinormal,
  rot_perpin,
  rot_perpout,
  rot_radialin,
  rot_radialout,
  rot_relcurr,
  rot_abscurr,
  rot_docksync
}