--- smsdaemon/SmsPdu.cpp	2008/06/13 06:38:43	69
+++ smsdaemon/SmsPdu.cpp	2008/06/13 09:09:36	73
@@ -36,6 +36,9 @@
 
 	string EncodePhonenr(string input)
 	{
+		if ( input.at(0) == '+' )
+			input.erase(0,1);
+
 		if ( (input.length() % 2) == 1)
 			input.append("F");
 		return SwitchChars(input);
@@ -78,11 +81,11 @@
 		return vec;
 	}
 
-	std::string Decode8to7bit(vector<unsigned char> input)
+	std::string Decode8to7bit(vector<unsigned char> input, int shift_start)
 	{
 		string result;
 
-		int shift = 0;
+		int shift = shift_start;
 		for (unsigned int i=0; i<input.size(); ++i)
 		{
 			unsigned char current = input.at(i);
@@ -110,11 +113,11 @@
 
 
 
-	vector<unsigned char> Encode7to8bit(std::string str)
+	vector<unsigned char> Encode7to8bit(std::string str, int shift_start)
 	{
 		vector<unsigned char> buf;
 
-		int shift = 0;
+		int shift = shift_start;
 		for (unsigned int i=0; i<str.size(); ++i)
 		{
 			unsigned char current = str.at(i) & 0x7F;
@@ -178,13 +181,14 @@
 			pdu.push_back(to.length() ); //length of phone nr
 			pdu.push_back(0x81); // type of address (international nr  + ISDN/telephone range) - else try 0x81
 
-		
-			string phone = EncodePhonenr(to);
+
+			vector<unsigned char> phone = HexDecodeString( EncodePhonenr(to ));
 			pdu.insert( pdu.end(), phone.begin(), phone.end());
 
 			pdu.push_back(0x00); // Protocol identifier
 			pdu.push_back(0x00); // Data coding scheme 
 
+			int shift_start = 0;
 			string message_part;
 			if (multipart)
 			{
@@ -199,7 +203,7 @@
 				pdu.push_back( part_count );
 				pdu.push_back( partnr+1 );
 				pdu.push_back( 0x00);
-
+				//shift_start = 6;
 			}
 			else
 			{
@@ -216,7 +220,7 @@
 				pdu.push_back( message_part.length() ); //UserDataLength
 			}
 
-			vector<unsigned char> userData = Encode7to8bit(message_part);
+			vector<unsigned char> userData = Encode7to8bit(message_part, shift_start);
 
 			pdu.insert( pdu.end(), userData.begin(), userData.end());
 
@@ -284,6 +288,9 @@
 		it += (*it++); // Skip smsc info
 
 		unsigned char deliver_first_octet = (*it++);
+
+		bool UDHI = (deliver_first_octet & 0x40) > 0;
+
 		unsigned char sender_len = (*it++);
 		if ( (sender_len % 2) == 1)
 			sender_len++;
@@ -299,12 +306,26 @@
 		result.timestamp = DecodeTimestamp(it, it+7);
 		it += 7;
 
+
 		unsigned char data_len = (*it++);
 
+		int shift_start = 0;
+		if (UDHI)
+		{
+			int udh_len = (*it++); 
+			cout << "UDH_LEN:" << udh_len << endl;
+			it += udh_len; //just ignore the User Data Header
+			data_len -= udh_len;
+
+			shift_start = udh_len+1; //make the 8to7bit decode start with the right shift level
+		}
+		cout << hex<< (int)(*it) << endl;;
+
+
 		vector<unsigned char> user_data;
 		user_data.insert(user_data.end(), it, it+data_len);
 		
-		result.message = Decode8to7bit(user_data);
+		result.message = Decode8to7bit(user_data, shift_start);
 		
 
 		return result;