Сортировка массива

[sanu0074]

Задача отсортировать массив по вложенности, по ключу p_id, на ходе имеем такой массив:

array(6) {
  [0]=>
  array(9) {
    ["id"]=>
    string(2) "23"
    ["el_id"]=>
    string(1) "9"
    ["p_id"]=>
    string(1) "0"
    ["avt_id"]=>
    string(1) "2"
    ["date_create"]=>
    string(19) "2015-03-03 04:12:10"
    ["like_p"]=>
    string(1) "0"
    ["like_m"]=>
    string(1) "0"
    ["text"]=>
    string(12) "первый"
    ["avt_login"]=>
    string(6) "JbWR6v"
  }
  [1]=>
  array(9) {
    ["id"]=>
    string(2) "24"
    ["el_id"]=>
    string(1) "9"
    ["p_id"]=>
    string(2) "23"
    ["avt_id"]=>
    string(1) "2"
    ["date_create"]=>
    string(19) "2015-03-03 04:12:25"
    ["like_p"]=>
    string(1) "0"
    ["like_m"]=>
    string(1) "0"
    ["text"]=>
    string(19) "под первым"
    ["avt_login"]=>
    string(6) "JbWR6v"
  }
  [2]=>
  array(9) {
    ["id"]=>
    string(2) "25"
    ["el_id"]=>
    string(1) "9"
    ["p_id"]=>
    string(1) "0"
    ["avt_id"]=>
    string(1) "2"
    ["date_create"]=>
    string(19) "2015-03-03 04:27:37"
    ["like_p"]=>
    string(1) "0"
    ["like_m"]=>
    string(1) "0"
    ["text"]=>
    string(18) "послений 1"
    ["avt_login"]=>
    string(6) "JbWR6v"
  }
  [3]=>
  array(9) {
    ["id"]=>
    string(2) "26"
    ["el_id"]=>
    string(1) "9"
    ["p_id"]=>
    string(1) "0"
    ["avt_id"]=>
    string(1) "2"
    ["date_create"]=>
    string(19) "2015-03-03 04:27:44"
    ["like_p"]=>
    string(1) "0"
    ["like_m"]=>
    string(1) "0"
    ["text"]=>
    string(16) "вфывфывф"
    ["avt_login"]=>
    string(6) "JbWR6v"
  }
  [4]=>
  array(9) {
    ["id"]=>
    string(2) "27"
    ["el_id"]=>
    string(1) "9"
    ["p_id"]=>
    string(2) "25"
    ["avt_id"]=>
    string(1) "2"
    ["date_create"]=>
    string(19) "2015-03-03 04:27:53"
    ["like_p"]=>
    string(1) "0"
    ["like_m"]=>
    string(1) "0"
    ["text"]=>
    string(16) "фвфывфыв"
    ["avt_login"]=>
    string(6) "JbWR6v"
  }
  [5]=>
  array(9) {
    ["id"]=>
    string(2) "28"
    ["el_id"]=>
    string(1) "9"
    ["p_id"]=>
    string(2) "26"
    ["avt_id"]=>
    string(1) "2"
    ["date_create"]=>
    string(19) "2015-03-03 04:28:02"
    ["like_p"]=>
    string(1) "0"
    ["like_m"]=>
    string(1) "0"
    ["text"]=>
    string(16) "фвфывфыв"
    ["avt_login"]=>
    string(6) "JbWR6v"
  }
}

На выходе должны получить как в файле (пример как надо сделать)
Вот функция которая это делает:

    private function mapTree($dataset) {
        $tree = array();
        foreach ($dataset as $id=>&$node) {
            if (!$node['p_id']) {
                $tree[$id] = &$node;
            } else {
                $dataset[$node['p_id']]['childs'][$id] = &$node;
            }
        }
        return $tree;
    }

но она выдает ошибку: Undefined index: p_id
и печатает только элементы