At 40 kHz I'd be using interrupts and counting clock cycles through timer2 for a much more accurate timing.
You're using the micros() function to do timing of a wave that lasts 25 microseconds, while that function has a resolution of 4 microseconds.
For your analog out: the best you can do is put out a PWM signal, smooth that to a 0-5V signal, and as you need negative as well you'll have to apply a -2.5V offset to that. PWM has 8-bit resolution, so you have 256 total steps in your output.
One of the hardest parts you have to deal with is code efficiency. You have 25 microseconds, that's 400 clock cycles, in which you get two interrupts which you have to process plus calculating the difference plus setting the output ports.
So you'll have to start by dropping all those digitalRead() and analogWrite() functions, and go for direct register reading/writing (you don't need to read any ports if you're using interrupts anyway).
You'll also have to drop all Serial.print() calls, as those are way too slow, even if you set your baud rate to something not so sluggish as the default 9600, such as 115200 bps. You may get away with printing 1-2 characters each cycle but still it's not too good an idea to do it each cycle - which is 40,000 times per second - even Superman will have a hard time reading that fast.
Anyway, you're approaching the limits of what an Arduino can do here, but with efficient programming you should be able to get it done.
Otherwise you may consider using a faster processor like the ESP8266 (80 MHz, giving you five times the number of clock cycles to do stuff - it'll happily handle this 40 kHz signal, I've been timing much higher frequencies than this), or the even faster Teensy (no experience with that one).
One more thing: no floating point maths allowed in this, it's too slow. Integer math only, and where possible try to use bit shifts instead (so make sure you divide by nice round numbers like 64 or 128, not by 100, so instead of doing a division in code you do a bitwise right shift - or a left shift for multiplications)