Welcome to SimpleFOC, @varun_dixit !
You might be able to optimize it, but I’m doubtful…
Software interrupts are a bit problematic to begin with, and the Nano is a slow 8bit MCU…
If you switch to a faster 32 bit MCU my guess is it will work much better (and you probably won’t need software interrupts in the first place)