void Task_I2C_Server_FSM(void)
{
    static enum {S0, S1, S2, S3, S4, S5, S6, S7, S8, S9, S10, S11} next_state = S0;
    static uint8_t dummy, num_bytes_read=0, is_last_read=0;
    SET_BIT(DEBUG_I2C_CODE);                    //  set i2c bit high
    switch(next_state)
    {
        case S0:
            // Initial function checks
            if(g_I2C_Msg.Command != READ && g_I2C_Msg.Status == IDLE)
            {
                CLEAR_BIT(DEBUG_I2C_CODE);                  //  set i2c bit low
                return;
            }
            else if(g_I2C_Msg.Command == READ)
            {
                g_I2C_Msg.Status = READING;
                I2C_TRAN;                                                   //  set to transmit mode
                SET_BIT(DEBUG_MSG_ON_BUS);              //  set msg on bus bit high
                I2C_M_START;                                            //  send start
                I2C0->D = g_I2C_Msg.Dev_adx;                                //  send dev address (write)
                lock_detect = 0;
                next_state = S1;
            }
            else
            {
                next_state = S11;
            }
            break;
        case S1:
            //  check for completion
            SET_BIT(DEBUG_I2C_BUSY_WAIT);               //  set i2c busy-wait bit high
            while( ( ( I2C0->S & I2C_S_IICIF_MASK ) == 0 ) & ( lock_detect < LOCK_DETECT_THRESHOLD )) {
                lock_detect++;
                next_state = S1;
            }
            //else
            //{
                next_state = S2;
            //}
            CLEAR_BIT(DEBUG_I2C_BUSY_WAIT);             //  set i2c busy-wait bit low
            break;
        case S2:
            I2C0->S |= I2C_S_IICIF_MASK;
            CLEAR_BIT(DEBUG_I2C_BUSY_WAIT);             //  set i2c busy-wait bit low
            I2C0->D = g_I2C_Msg.Reg_adx;                                //  send register address
            lock_detect = 0;
            next_state = S3;
            break;
        case S3:
            //  check for completion
            SET_BIT(DEBUG_I2C_BUSY_WAIT);               //  set i2c busy-wait bit high
            while( ( ( I2C0->S & I2C_S_IICIF_MASK ) == 0 ) & ( lock_detect < LOCK_DETECT_THRESHOLD )) {
                lock_detect++;
                next_state = S3;
            }
            //else
            //{
                next_state = S4;
            //}
            CLEAR_BIT(DEBUG_I2C_BUSY_WAIT);             //  set i2c busy-wait bit low
            break;
        case S4:
            I2C0->S |= I2C_S_IICIF_MASK;
            CLEAR_BIT(DEBUG_I2C_BUSY_WAIT);             //  set i2c busy-wait bit low
            I2C_M_RSTART;                                           //  repeated start
            I2C0->D = g_I2C_Msg.Dev_adx | 0x01 ;                //  send dev address (read)
            lock_detect = 0;
            next_state = S5;
            break;
        case S5:
            //  check for completion
            SET_BIT(DEBUG_I2C_BUSY_WAIT);               //  set i2c busy-wait bit high
            while( ( ( I2C0->S & I2C_S_IICIF_MASK ) == 0 ) & ( lock_detect < LOCK_DETECT_THRESHOLD )) {
                lock_detect++;
                next_state = S5;
            }
            //else
            //{
                next_state = S6;
            //}
            CLEAR_BIT(DEBUG_I2C_BUSY_WAIT);             //  set i2c busy-wait bit low
            break;
        case S6:
            I2C0->S |= I2C_S_IICIF_MASK;
            I2C_REC;                                                    //  set to receive mode
            next_state = S7;
            break;
        case S7:
            if(num_bytes_read < g_I2C_Msg.Data_count)
            {
                next_state = S8;
            }
            else
            {
                g_I2C_Msg.Status = READ_COMPLETE;
                next_state = S11;
            }
            break;
        case S8:
            is_last_read = (num_bytes_read == g_I2C_Msg.Data_count-1)? 1: 0;
            if (is_last_read){
                NACK;                                                   // tell HW to send NACK after read
            } else {
                ACK;                                                    // tell HW to send ACK after read
            }
            dummy = I2C0->D;                                //  dummy read
            next_state = S9;
            break;
        case S9:
            //  check for completion
            SET_BIT(DEBUG_I2C_BUSY_WAIT);               //  set i2c busy-wait bit high
            while( ( ( I2C0->S & I2C_S_IICIF_MASK ) == 0 ) & ( lock_detect < LOCK_DETECT_THRESHOLD )) {
                lock_detect++;
                next_state = S9;
            }
            //else
            //{
                next_state = S10;
            //}
            CLEAR_BIT(DEBUG_I2C_BUSY_WAIT);             //  set i2c busy-wait bit low
            break;
        case S10:
            I2C0->S |= I2C_S_IICIF_MASK;
            if (is_last_read){
                I2C_M_STOP;                                     //  send stop
                CLEAR_BIT(DEBUG_MSG_ON_BUS);        //  set msg on bus bit low
            }
            data[num_bytes_read++] = I2C0->D; //    read data
            next_state = S7;
            break;
        case S11:
            CLEAR_BIT(DEBUG_I2C_CODE);                  //  set i2c bit low
            next_state = S0;
            return;
        default:
            return;
    }
    CLEAR_BIT(DEBUG_I2C_CODE);                  //  set i2c bit low
}