clearscreen.

LIST ENGINES in engines.
SET vectors to LIST().

set i to 0.
until i = engines:length {
  print i.
  set pos to engines[i]:position.
  vectors:add(VECDRAWARGS(
    pos,
    engines[i]:facing:forevector * engines[i]:thrust,
    RGB(1, 0, 0),
    "",
    1.0,
    TRUE
  )).
  set engines[i]:thrustlimit to 100.
  set i to i + 1.
}

on AG1 {
  for vec in vectors {
    set vec:show to not (vec:show).
  }
  set torquevec:show to not (torquevec:show).
  PRESERVE.
}

set torquevec to VECDRAWARGS(V(0, 0, 0), V(0, 0, 0), RGB(0, 0, 1), "torque", 1.0, TRUE).

set exit to false.
on AG9 { set exit to true. }

set torques to LIST().
for eng in engines { torques:add(0). }

until exit {
  set total_torque to V(0, 0, 0).
  set i to 0.
  until i = engines:length {
    set thrust to engines[i]:facing:forevector * engines[i]:maxthrust * engines[i]:thrustlimit / 100.
    set torques[i] to VCRS(thrust, engines[i]:position).
    set total_torque to total_torque + torques[i].
    set vectors[i]:VEC to thrust.
    set vectors[i]:START to engines[i]:position.
    set i to i + 1.
  }

  set reset to true.
  until total_torque:MAG < 1.0 {
    if reset {
      set total_torque to V(0, 0, 0).
      set i to 0.
      until i = engines:length {
        set engines[i]:thrustlimit to 100.
        set thrust to engines[i]:facing:forevector * engines[i]:maxthrust * engines[i]:thrustlimit / 100.
        set torques[i] to VCRS(thrust, engines[i]:position).
        set total_torque to total_torque + torques[i].
        set i to i + 1.
      }
      set reset to false.
      print "Recalculating thrusts".
    }
    // get engine with lowest angle to current torque, and reduce it's throttle
    set lowestAngle to 360.
    set lowestEngine to 0.
    set i to 0.
    until i = engines:length {
      set angle to ARCCOS(total_torque * torques[i] / (total_torque:MAG *  torques[i]:MAG)).
      if angle < lowestAngle {
        set lowestAngle to angle.
        set lowestEngine to i.
      }
      set i to i+1.
    }

    set factor to 1 - (total_torque:MAG / (torques[lowestEngine]:mag * COS(lowestAngle))).
    //set deltaTorque to torques[i] * (factor - 1).
    set engines[lowestEngine]:thrustlimit to engines[lowestEngine]:thrustlimit * factor.
    set thrust to engines[lowestEngine]:facing:forevector * engines[lowestEngine]:maxthrust * engines[lowestEngine]:thrustlimit / 100.
    set deltaTorque to VCRS(thrust, engines[lowestEngine]:position) - torques[lowestEngine].
    set torques[lowestEngine] to torques[lowestEngine] + deltaTorque.
    set total_torque to total_torque + deltaTorque.
    //set torques[lowestEngine] to VCRS(thrust, engines[lowestEngine]:position).
  }

  set torquevec:VEC to total_torque. // visual
  wait 0.1.
}