Some changes

Added figs, changed calc angle function