fixed matrix singularity issues, simplified subfunctions