Битстаффинг

#include <iostream>
#include <stdio.h>
#include <vector>
 
class Zero_Insert {
 
private: unsigned short counter;
 
public: ~Zero_Insert(void){};
 
public: Zero_Insert(void)
        {
            Reset();
        };
 
public: void Reset(void)
        {
            counter=0;
        }
 
public: void Zeros_Insertion(vector <unsigned char> & InputVector, vector <unsigned char> & OutputVector)
        {
            
                        OutputVector.reserve(InputVector.size());
 
            for(int j = 0; j < InputVector.size(); j++)
            {
                if( InputVector[j] == 1 )
                {
                    counter++;
                }
                else
                {
                    counter=0;
                }
                OutputVector.push_back(InputVector[j]);
                if(counter==5){
                    OutputVector.push_back(0);
                    counter = 0;
                }
 
            }
        }
};