Adapted from Dalessi et al. in Matlab