Encoding / Decoding 7 bit User Data for SMS PDU (PDU Bit Packer)

// Move the bits to the appropriate byte (unpack the bits)
foreach (byte b in packedBytes)
{
    if (unpackIndex <= shiftedBytes.Length) // my correct
    {
        if (moveOffset == 7)
        {
            moveOffset = 0;
            unpackIndex++;
            unpackedBytes[unpackIndex - 1] = shiftedBytes[unpackIndex - 1];
        }

        // Extract the bits to be moved
        int extractedBitsByte = (packedBytes[moveIndex] & _decodeMask[moveOffset]);
        // Shift the extracted bits to the proper offset
        extractedBitsByte = (extractedBitsByte >> (7 - moveOffset));
        // Move the bits to the appropriate byte (unpack the bits)
        int movedBitsByte = (extractedBitsByte | shiftedBytes[unpackIndex]);

        unpackedBytes[unpackIndex] = (byte)movedBitsByte;

        moveOffset++;
        unpackIndex++;
        moveIndex++;
    }
}

std::string conv_utf8_to_gsm7(std::string str) 
{
	std::stringstream ss;
	ss << std::hex << std::uppercase;
	for (std::size_t ii = 0; ii < str.size(); ii++) 
	{
		if (ii && ii % 7 == 0) 
			str = str.erase(ii, 1);

		if (ii < str.size()) 
		{
			char ch = str[ii] >> (ii % 7);
			if (ii < str.size() - 1) 
				ch |= str[ii + 1] << (7 - (ii % 7));
			ss << std::setw(2) << std::setfill('0') << (int)(unsigned char)ch;
		}
	}
	return ss.str();
}

static vector<unsigned char> UnpackBytes(vector<unsigned char> const & packedBytes)
{
    if (packedBytes.empty()) return vector<unsigned char>();

    const unsigned char _decodeMask[7] = { 128, 192, 224, 240, 248, 252, 254 };
    vector<char> shiftedBytes((packedBytes.size() * 8) / 7, 0);

    int shiftOffset = 0;
    int shiftIndex = 0;

    // Shift the packed bytes to the left according to the offset (position of the byte)
    ;
    for (vector<unsigned char>::const_iterator b = packedBytes.begin(); b!=packedBytes.end(); ++b)
    {
        if (shiftOffset == 7)
        {
            shiftedBytes[shiftIndex] = 0;
            shiftOffset = 0;
            shiftIndex++;
        }

        shiftedBytes[shiftIndex] = (unsigned char)((*b << shiftOffset) & 127);

        shiftOffset++;
        shiftIndex++;
    }

    int moveOffset = 0;
    int moveIndex = 0;
    int unpackIndex = 1;
    vector<unsigned char> unpackedBytes(shiftedBytes.size(), 0);

    //
    if (shiftedBytes.size() > 0)
    {
        unpackedBytes[unpackIndex - 1] = shiftedBytes[unpackIndex - 1];
    }

    // Move the bits to the appropriate byte (unpack the bits)
    for (vector<unsigned char>::const_iterator b = packedBytes.begin(); b!= packedBytes.end(); ++b)
    {
        if (unpackIndex != shiftedBytes.size())
        {
            if (moveOffset == 7)
            {
                moveOffset = 0;
                unpackIndex++;
                unpackedBytes[unpackIndex - 1] = shiftedBytes[unpackIndex - 1];
            }

            if (unpackIndex != shiftedBytes.size())
            {
                // Extract the bits to be moved
                int extractedBitsByte = (packedBytes[moveIndex] & _decodeMask[moveOffset]);
                // Shift the extracted bits to the proper offset
                extractedBitsByte = (extractedBitsByte >> (7 - moveOffset));
                // Move the bits to the appropriate byte (unpack the bits)
                int movedBitsByte = (extractedBitsByte | shiftedBytes[unpackIndex]);

                unpackedBytes[unpackIndex] = (unsigned char)movedBitsByte;

                moveOffset++;
                unpackIndex++;
                moveIndex++;
            }
        }
    }

    // Remove the padding if exists
    if (unpackedBytes[unpackedBytes.size() - 1] == 0)
    {
        unpackedBytes.erase(unpackedBytes.end() - 1);
    }

    return unpackedBytes;
}

//Remove 2 lines:
//tmpByte = (byte)(b << shiftOffset);
//shiftedBytes[shiftIndex] = (byte)(tmpByte & 127);
//Add a new line:
shiftedBytes[shiftIndex] = (byte)((b << shiftOffset) & 127); //fixed arthmetic overflow