Reorganized files, fixed code, Simulink working