Constraints work

Added functions to compute constraints
Added system solve
Added some debugging and solver(2)