PHP Code Highlight?

Well, I am a PHP developer myself, and my website is at https://www.aim-journals.com. I was looking at wordpress and wondered if there was a way to post Highlighted PHP code. I figured out that you would have to do this manually because no such thing exists. However, there is a PHP function that will do this.
If I knew the workings of WordPress, I would edit it myself so that if I post
[PHP]$something = "yes";
if($something == "yes") {
echo "yes";
}[/PHP]
then it would take the code in between [PHP] and [/PHP] and put it into the string (for example) $php_str. Then I would have it do this code:

$php_str = "<?\n".$php_str."\n?".">";
$php_str = str_replace("$","\$",$php_str);
$php_str = str_replace("\"","\\"",$php_str);
$php_str = str_replace("'","\'",$php_str);
$output_str = "<div style='margin:0px 5% 0px 5%;'>PHP:<hr />";
$output_str .= highlight_string($php_str,true);
$output_str .= "<hr /></div>";

and then it would replace [PHP]…[/PHP] with $output_str.
If you see what I’m trying to do here, can anyone make a hack that would do this? If you need more explanation of to what I want, just tell me… thanks!