Вот что в итоге получилось...
http://hiasm.com/forum_serv.php?q=56&id=2320
Будет время - посмотрите код на предмет оптимизации.
Есть большая вероятность, что что-то можно было сделать проще.