msp430 - Msp430g2553 i2c communication library with itg3200 gyroscope

I've been creating an I2C library for communicating with ITG3200 gyroscope for MSP430g2553 experimental launchpad. I still have slight problems with sequential reading and the signed values but I'm nearly complete. Can you help out the last problems that I'm dealing with? It seems that I'm stuck on a point. Here is the library http://e2e.ti.com/cfs-file.ashx/__key/communityserver-discussions-components-files/166/8272.i2c_5F00_lib.rar

family user guide : http://www.ti.com/lit/ug/slau144i/slau144i.pdf

gyro user guide : https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf

If you do not want to download those 3 source files, here is the explanation of the required functions:

Main:

WDTCTL = WDTPW + WDTHOLD; // Stop WDT
BCSCTL1 = CALBC1_1MHZ; // Set DCO to 1Mhz
DCOCTL = CALDCO_1MHZ;

P1SEL |= BIT1 + BIT2 + BIT6 + BIT7; // Assign I2C pins to USCI_B0 // Assign Uart pins to USCI_A0
P1SEL2 |= BIT1 + BIT2 + BIT6 + BIT7; // Assign I2C pins to USCI_B0 // Assign Uart pins to USCI_A0
init_I2C(); // initialize i2c
initUart(); // initialize uart

..

..

I2C init:

void init_I2C(void) {
    UCB0CTL1 |= UCSWRST; // Enable SW reset
    UCB0CTL0 = UCMST + UCMODE_3 + UCSYNC; // I2C Master, synchronous mode
    UCB0CTL1 = UCSSEL_2 + UCSWRST; // Use SMCLK, keep SW reset
    UCB0BR0 = 10; // fSCL = 1Mhz/10 = ~100kHz
    UCB0BR1 = 0;
    UCB0I2CSA = itgAddress; // Slave Address is 069h
    UCB0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
    IE2 |= UCB0RXIE + UCB0TXIE; // Enable RX and TX interrupt
}

Receive function:

uint8_t Receive(char registerAddr){
    uint8_t receivedByte;
    while (UCB0CTL1 & UCTXSTP); // Ensure stop condition got sent
    UCB0CTL1 |= UCTR + UCTXSTT; // I2C start condition with UCTR flag for transmit
    while((IFG2 & UCB0TXIFG) == 0); //UCB0TXIFG is set immidiately
    UCB0TXBUF = registerAddr; //write registerAddr in TX buffer
    while((IFG2 & UCB0TXIFG) == 0); // wait until TX buffer is empty and transmitted
    UCB0CTL1 &= ~UCTR ; // Clear I2C TX flag for receive

    UCB0CTL1 |= UCTXSTT + UCTXNACK; // I2C start condition with NACK for single byte reading
    while (UCB0CTL1 & UCTXSTT); // Start condition sent? RXBuffer full?
    receivedByte = UCB0RXBUF;
    UCB0CTL1 |= UCTXSTP; // I2C stop condition
    return receivedByte;
}

Uart init:

void initUart(void) {
    UCA0CTL1 |= UCSSEL_2; // Use SMCLK

    UCA0BR0 = 104; // 1MHz 9600
    UCA0BR1 = 0; // 1MHz 9600
    UCA0MCTL = UCBRS0; // Modulation UCBRSx = 1
    UCA0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
}

integer print:

void serialPrintInteger(uint16_t num) {
    int i;
    uint16_t num_send[5];

    uint16_t numTemp;
    num_send[0] = num/10000; // extract 5th digit
    numTemp = num % 10000; // get remaining 4
    num_send[1] = numTemp/1000; // extract 4th digit
    numTemp = numTemp % 1000; // get remamining 3
    num_send[2] = numTemp/100; // extract 3th digit
    numTemp = numTemp % 100; // get remaining 2
    num_send[3] = numTemp/10; // extract 2th digit
    num_send[4] = numTemp % 10; // extract 1th digit


    if(num_send[0] > 0) { // if num is 5 digit
        for(i = 0 ; i <= 4 ; i++)
            serialWrite(num_send[i]); // send each digit as one byte
    }
    else if(num_send[1] > 0) { // if num is 4 digit
        for(i = 1 ; i <= 4 ; i++)
            serialWrite(num_send[i]);
    }
    else if(num_send[2] > 0) { // if num is 3 digit
        for(i = 2 ; i <= 4 ; i++)

            serialWrite(num_send[i]);
    }
    else if(num_send[3] > 0) { // if num is 2 digit
        for(i = 3 ; i <= 4 ; i++)
            serialWrite(num_send[i]);
    }
    else { // if num is 1 digit
        serialWrite(num_send[4]);
    }
}


void serialPrintAscii(uint8_t ascii) {
    UCA0TXBUF = ascii;
    while (!(IFG2&UCA0TXIFG)); // USCI_A0 TX buffer ready?
}

And here is my problem. When i try these readings in my main loop seperately i get the results that i should have.

serialPrintInteger(Receive(0x00));

or

serialPrintInteger(Receive(0x15));

or

serialPrintInteger(Receive(0x16));

and the outputs of these are 0x69 from Receive(0x00) which reads slave address register of the gyroscope, 9 from Receive(0x15) where i wrote 9 inside 0x15 register for configuration and 25 from Receive(0x16) where i wrote aswell.

I don't think my serialPrint functions are corrupted aswell, I've tried with a lot of combination under range of 16 bit where it should be. This sequence is working fine:

serialPrintInteger(5);
serialPrintAscii(Ascii_Comma);
serialPrintInteger(10);

serialPrintAscii(Ascii_And);
serialPrintInteger(15);
serialPrintAscii(Ascii_Dot);

I see an output like this in my serial console: 5,10&15.5,10&15.5...

The funny part starting when i try this logic on my Receive function. Here is the sequence i use

serialPrintInteger(Receive(0x00)); // result 105
serialPrintAscii(Ascii_Comma);
serialPrintInteger(Receive(0x15)); // result 9
serialPrintAscii(Ascii_And);

serialPrintInteger(Receive(0x00)); // result 105
serialPrintAscii(Ascii_Dot);

The sequence in my console is like this: 105,105&9.105,105&9.105...

First i thought i wasn't properly send NACK to the slave for single byte and i thought it kept incrementing the register address on his own but they are working fine seperately and gyro has x,y,z registers aswell and they are not corrupting my sequence.

I was struggling with gyro motion registers for a while but i realised that I'm not in full control of my I2C just yet. So can you point out what i'm doing wrong here?