fixed bug to diagnal algoritghm

update with fix to the bug to the diagnal.  it mostly work now but there
are few bug need to be track down and fix.  if anyone can spot a bug let
me know, and if you can give me what cause the bug, the better