Full algorithm implemented.

Signed-off-by: David Murray HDD <david.murray@institutoptique.fr>