Modified the FDP solver. Make it faster and code reorg