vcs.maemo.org Git - mardrone/blob - mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/VLIB/video_huffman.c

   1 #include <VLIB/Platform/video_utils.h>
   2
   3 #include <VP_Os/vp_os_malloc.h>
   4 #include <VLIB/video_huffman.h>
   5 #include <VLIB/video_packetizer.h>
   6
   7 huffman_tree_t* huffman_alloc( int32_t num_max_codes, int32_t max_code_length )
   8 {
   9   int32_t tree_size = sizeof(huffman_tree_t) + num_max_codes * sizeof(huffman_tree_data_t);
  10   huffman_tree_t* tree = (huffman_tree_t*) vp_os_malloc( tree_size );
  11
  12   tree->num_used_codes    = 0;
  13   tree->num_max_codes     = num_max_codes;
  14   tree->max_code_length   = max_code_length;
  15
  16   return tree;
  17 }
  18
  19 void huffman_free( huffman_tree_t* tree )
  20 {
  21   vp_os_free( tree );
  22 }
  23
  24 C_RESULT huffman_add_codes( huffman_tree_t* tree, huffman_code_t* codes, int32_t num_codes )
  25 {
  26   while( (tree->num_used_codes < tree->num_max_codes) && num_codes-- )
  27   {
  28     tree->data[tree->num_used_codes].code = codes++;
  29     tree->data[tree->num_used_codes].weight = 0;
  30
  31     tree->num_used_codes ++;
  32   }
  33
  34   return C_OK;
  35 }
  36
  37 static inline int32_t compare_codes( huffman_code_t* c1, huffman_code_t* c2, int32_t max_length )
  38 {
  39   int32_t i1, i2;
  40
  41   i1 = c1->vlc << (max_length - c1->length);
  42   i2 = c2->vlc << (max_length - c2->length);
  43
  44   return i1 < i2 ? -1 : 1;
  45 }
  46
  47 static C_RESULT huffman_sort_codes_internal( huffman_tree_data_t *begin, huffman_tree_data_t *end, int32_t max_length )
  48 {
  49   huffman_tree_data_t *pivot  = begin;
  50   huffman_tree_data_t *left   = begin;
  51   huffman_tree_data_t *right  = end;
  52
  53   while( right != left )
  54   {
  55     if( compare_codes(right->code, left->code, max_length) < 0 )
  56     {
  57       huffman_code_t* temp_code = right->code;
  58       right->code = left->code;
  59       left->code  = temp_code;
  60
  61       pivot = pivot == left ? right : left;
  62     }
  63
  64     pivot == left ? right-- : left++;
  65   }
  66
  67   if( begin < left - 1 )
  68     huffman_sort_codes_internal( begin, left - 1, max_length );
  69   if( end > right + 1 )
  70     huffman_sort_codes_internal( right + 1, end, max_length );
  71
  72   return C_OK;
  73 }
  74
  75 C_RESULT huffman_sort_codes( huffman_tree_t* tree )
  76 {
  77   int32_t i;
  78
  79   // Sort codes (basic qsort implementation)
  80   huffman_sort_codes_internal( &tree->data[0], &tree->data[tree->num_used_codes-1], tree->max_code_length);
  81
  82   // Generates weight & counts for each code
  83   for(i = 0; i < tree->num_used_codes; i++ )
  84   {
  85     tree->data[i].weight  = (1 << (1 + tree->max_code_length - tree->data[i].code->length)) - 1;
  86     tree->data[i].weight += tree->data[i].code->vlc << (1 + tree->max_code_length - tree->data[i].code->length);
  87   }
  88
  89   return C_OK;
  90 }
  91
  92 C_RESULT huffman_check_code( huffman_tree_t* tree, uint32_t code, uint32_t length )
  93 {
  94   int32_t i;
  95   uint32_t w;
  96
  97   w  = (1 << (1 + tree->max_code_length - length)) - 1;
  98   w += code << (1 + tree->max_code_length - length);
  99
 100   for(i = 0; i < tree->num_used_codes && w != tree->data[i].weight; i++ );
 101
 102   if( i == tree->num_used_codes )
 103     return C_FAIL;
 104
 105   return tree->data[i].weight == w ? C_OK : C_FAIL;
 106 }
 107
 108 int32_t huffman_stream_code( huffman_tree_t* tree, video_stream_t* stream )
 109 {
 110   huffman_code_t* huffman_code;
 111   int32_t i, w;
 112   uint32_t c;
 113
 114   c = 0;
 115
 116   video_peek_data( stream, &c, tree->max_code_length );
 117
 118   w = (c << 1) + 1;
 119
 120   for(i = 0; i < tree->num_used_codes && w > tree->data[i].weight; i++ );
 121
 122   huffman_code = tree->data[i].code;
 123
 124   // Update stream with read data
 125   video_read_data( stream, &c, huffman_code->length );
 126
 127   return tree->data[i].code->index;
 128 }