Maybe start by defining the problem? What exactly is the desired behavior of your firmware? What sort of input will it take?
One of the simplest would be to take target pitch/yaw/roll rates and total combined propeller speed (throttle). That translates fairly directly into speed commands for the four propellers.
Slightly more complicated would be to specify a target pitch and roll angle, yaw rate, and total combined propeller speed. Then you PID the pitch and roll rates using the target angles and IMU data.
Slightly more complicated than that would be to specify target acceleration in each direction and use a bit of trigonometry to figure out the necessary pitch/roll angles and throttle. You'll need to decide whether the target accelerations are relative to the current heading (forward/back/left/right) or north/south/east/west.
A level up from that would be a system where the AI tries to maintain a fixed position in 3D space until commands are given, and the commands would be something like target forward/back speed, left/right speed, up/down speed, and either yaw rate or compass heading or automatically point in the direction it's moving. This gets tricky because the IMU can only sense acceleration, not speed. GPS would be one way to measure horizontal speed, and altimeter for vertical. For this you need to PID the target acceleration in each direction to maintain the target speed. But I don't think GPS or altimeter will be all that precise, so it may take more than simple PID tuning to get good behavior out of it.
A level up from that would be to specify a target position in 3D space (and maybe time of arrival) and have it figure out how to get there on its own.
You could potentially build the system up in layers so each mode gives commands to the next lower level mode. But it might be easier to just choose one and send speed commands to the ESC's directly from there.