Finally got this working, this was a nightmare to design and debug. Basically two program counters which are switched when an interrupt happens so that program memory can jump between the two. Also added Jumps, so you can jump inside an Interrupt service routine and when you exit (CLRIRQ) it will return to the program counter where it left off.