void oled_128x64_init(void * I2C_Device)
{
    __I2C_Device = I2C_Device;
    
    suli_i2c_init(__I2C_Device);
    oled_128x64_cmd(SeeedOLED_Display_Off_Cmd);     //display off
    suli_delay_ms(5);
    oled_128x64_cmd(SeeedOLED_Display_On_Cmd);     //display on
    suli_delay_ms(5);
    oled_128x64_cmd(SeeedOLED_Normal_Display_Cmd);  //Set Normal Display (default)
}
int GPRS::readSMS(int messageIndex, char *message,int length)
{
    int i = 0;
    char gprsBuffer[100];
    char cmd[16];
    char *p,*s;
    
    sim900_check_with_cmd("AT+CMGF=1\r\n","OK\r\n",CMD);
    suli_delay_ms(1000);
    sprintf(cmd,"AT+CMGR=%d\r\n",messageIndex);
    sim900_send_cmd(cmd);
    sim900_clean_buffer(gprsBuffer,sizeof(gprsBuffer));
    sim900_read_buffer(gprsBuffer,sizeof(gprsBuffer),DEFAULT_TIMEOUT);
    if(NULL != ( s = strstr(gprsBuffer,"+CMGR:"))){
        if(NULL != ( s = strstr(s,"\r\n"))){
            p = s + 2;
            while((*p != '\r')&&(i < length-1)) {
                message[i++] = *(p++);
            }
            message[i] = '\0';
            return 0;
        }
    }
    return -1;   
}
int GPRS::readSMS(int messageIndex, char *message, int length, char *phone, char *datetime)  
{
  /* Response is like:
  AT+CMGR=2
  
  +CMGR: "REC READ","XXXXXXXXXXX","","14/10/09,17:30:17+08"
  SMS text here
  
  So we need (more or lees), 80 chars plus expected message length in buffer. CAUTION FREE MEMORY
  */

    int i = 0;
    char gprsBuffer[80 + length];
    char cmd[16];
    char *p,*p2,*s;
    
    sim900_check_with_cmd("AT+CMGF=1\r\n","OK\r\n",CMD);
    suli_delay_ms(1000);
    sprintf(cmd,"AT+CMGR=%d\r\n",messageIndex);
    sim900_send_cmd(cmd);
    sim900_clean_buffer(gprsBuffer,sizeof(gprsBuffer));
    sim900_read_buffer(gprsBuffer,sizeof(gprsBuffer),DEFAULT_TIMEOUT);
    
    if(NULL != ( s = strstr(gprsBuffer,"+CMGR:"))){
        // Extract phone number string
        p = strstr(s,",");
        p2 = p + 2; //We are in the first phone number character
        p = strstr((char *)(p2), "\"");
        if (NULL != p) {
            i = 0;
            while (p2 < p) {
                phone[i++] = *(p2++);
            }
            phone[i] = '\0';            
        }
        // Extract date time string
        p = strstr((char *)(p2),",");
        p2 = p + 1; 
        p = strstr((char *)(p2), ","); 
        p2 = p + 2; //We are in the first date time character
        p = strstr((char *)(p2), "\"");
        if (NULL != p) {
            i = 0;
            while (p2 < p) {
                datetime[i++] = *(p2++);
            }
            datetime[i] = '\0';
        }        
        if(NULL != ( s = strstr(s,"\r\n"))){
            i = 0;
            p = s + 2;
            while((*p != '\r')&&(i < length-1)) {
                message[i++] = *(p++);
            }
            message[i] = '\0';
        }
        return 0;
    }
    return -1;    
}
int GPRS::sendSMS(char *number, char *data)
{
    char cmd[32];
    if(0 != sim900_check_with_cmd("AT+CMGF=1\r\n", "OK", DEFAULT_TIMEOUT,CMD)) { // Set message mode to ASCII
        return -1;
    }
    suli_delay_ms(500);
    snprintf(cmd, sizeof(cmd),"AT+CMGS=\"%s\"\r\n", number);
    if(0 != sim900_check_with_cmd(cmd,">",DEFAULT_TIMEOUT,CMD)) {
        return -1;
    }
    suli_delay_ms(1000);
    sim900_send_cmd(data);
    suli_delay_ms(500);
    sim900_send_End_Mark();
    return 0;
}
//duration: the time sounds, unit: ms
//freq: the frequency of speaker, unit: Hz
bool GroveSpeaker::write_sound_ms(int freq, int duration_ms)
{
    if(freq == 0 || duration_ms == 0) return false;

	uint32_t interval = (uint32_t)1000000 / freq;//convert the unit to us

    if (interval > 10000)
    {
        interval = 10000;
    } else if (interval < 5)
    {
        interval = 5;
    }

    uint32_t times = (uint32_t)duration_ms * 1000 / interval; //calcuate how many times the loop takes
    uint32_t times_5ms = 5000 / interval;

    if (interval > 2000)
    {
        for (int i = 0; i < times; i++)
     	{
    		suli_pin_write(this->io, SULI_HIGH);
    		suli_delay_ms(interval/2000);
    		suli_pin_write(this->io, SULI_LOW);
    		suli_delay_ms(interval/2000);
        }
    } else
    {
        for (int i = 0; i < times; i++)
     	{
    		suli_pin_write(this->io, SULI_HIGH);
    		suli_delay_us(interval/2);
    		suli_pin_write(this->io, SULI_LOW);
    		suli_delay_us(interval/2);
            if (i % times_5ms == (times_5ms-1))
            {
                suli_delay_ms(0); //yield the cpu
            }
        }
    }

    return true;
}
int GPRS::callUp(char *number)
{
    char cmd[24];
    if(0 != sim900_check_with_cmd("AT+COLP=1\r\n","OK",DEFAULT_TIMEOUT,CMD)) {
        return -1;
    }
    suli_delay_ms(1000);
    sprintf(cmd,"ATD%s;\r\n", number);
    sim900_send_cmd(cmd);
    return 0;
}
int GPRS::send(const char * str, int len)
{
    char cmd[32];

    //suli_delay_ms(1000);
    if(len > 0){
        snprintf(cmd,sizeof(cmd),"AT+CIPSEND=%d\r\n",len);
        if(0 != sim900_check_with_cmd(cmd,">",CMD)) {
            return false;
        }
        /*if(0 != sim900_check_with_cmd(str,"SEND OK\r\n", DEFAULT_TIMEOUT * 10 ,DATA)) {
            return false;
        }*/
        suli_delay_ms(500);
        sim900_send_cmd(str);
        suli_delay_ms(500);
        sim900_send_End_Mark();
        if(0 != sim900_wait_for_resp("SEND OK\r\n", DATA, DEFAULT_TIMEOUT * 10, DEFAULT_INTERCHAR_TIMEOUT * 10)) {
            return false;
        }        

    }
    return len;
}
GroveCompass::GroveCompass(int pinsda, int pinscl)
{
    this->i2c = (I2C_T *)malloc(sizeof(I2C_T));
    suli_i2c_init(i2c, pinsda, pinscl);

    suli_delay_ms(5);

    //uint8_t config;
	//config = (0x01 << 5);
    //suli_i2c_write_reg(i2c, HMC5883L_ADDRESS, CONFIGURATION_REGISTERB, &config, 1);

    mode = MEASUREMENT_CONTINUOUS;
    suli_i2c_write_reg(i2c, HMC5883L_ADDRESS, MODE_REGISTER, &mode, 1);

    cmdbuf[0] = DATA_REGISTER_BEGIN;
	suli_i2c_write(i2c, HMC5883L_ADDRESS, cmdbuf, 1);
}
int GPRS::checkSIMStatus(void)
{
    char gprsBuffer[32];
    int count = 0;
    sim900_clean_buffer(gprsBuffer,32);
    while(count < 3) {
        sim900_send_cmd("AT+CPIN?\r\n");
        sim900_read_buffer(gprsBuffer,32,DEFAULT_TIMEOUT);
        if((NULL != strstr(gprsBuffer,"+CPIN: READY"))) {
            break;
        }
        count++;
        suli_delay_ms(300);
    }
    if(count == 3) {
        return -1;
    }
    return 0;
}
bool GroveCo2MhZ16::_update_from_sensor(void)
{
    /* MH-Z16 "get" command */
    /* suli can't use const constraint */
    /*const*/ uint8_t cmd_get_sensor[] =
    { 0xff, 0x01, 0x86, 0x00, 0x00, 0x00, 0x00, 0x00, 0x79 };

    _drain_uart(); // just to be sure there's no leftovers
    suli_uart_write_bytes(uart, cmd_get_sensor, sizeof(cmd_get_sensor));

    suli_delay_ms(10);

#define BYTES_IN_ANSWER 9
    byte data[BYTES_IN_ANSWER];
    int n =
        suli_uart_read_bytes_timeout(uart,data,BYTES_IN_ANSWER,100);

    if (n!=9)
    {
        last_error="too few bytes read";
        return false;
    }

    // checksum :
    if ((1 + (0xFF ^ (byte)(data[1] + data[2] + data[3]
                            + data[4] + data[5] + data[6]
                            + data[7]))) != data[8])
    {
        last_error="checksum error";
        return false;
    }

    CO2PPM = (int)data[2] * 256 + (int)data[3];
    temp = (int)data[4] - 40;

    return true;
}
bool GroveTempHumPro::_read(IO_T *io)
{
    uint8_t laststate = SULI_HIGH;
    uint8_t counter = 0;
    uint8_t j = 0, i;
    unsigned long currenttime;

    // pull the pin high and wait 250 milliseconds
    //digitalWrite(_pin, SULI_HIGH);
    //suli_pin_write(io, SULI_HIGH);
    //suli_delay_ms(250);

    currenttime = suli_millis();
    if (currenttime < _lastreadtime)
    {
        // ie there was a rollover
        _lastreadtime = 0;
    }
    if (!firstreading && ((currenttime - _lastreadtime) < 2000))
    {
        return true; // return last correct measurement
                     //delay(2000 - (currenttime - _lastreadtime));
    }
    firstreading = false;

    _lastreadtime = suli_millis();

    data[0] = data[1] = data[2] = data[3] = data[4] = 0;

    // now pull it low for ~20 milliseconds
    //pinMode(_pin, OUTPUT);
    suli_pin_dir(io, SULI_OUTPUT);
    //digitalWrite(_pin, LOW);
    suli_pin_write(io, SULI_LOW);
    suli_delay_ms(20);
    //cli();
    //digitalWrite(_pin, SULI_HIGH);
    suli_pin_write(io, SULI_HIGH);
    //delayMicroseconds(40);
    suli_delay_us(40);
    //pinMode(_pin, INPUT);
    suli_pin_dir(io, SULI_INPUT);
    // read in timings
    for (i = 0; i < MAXTIMINGS; i++)
    {
        counter = 0;
        //while (digitalRead(_pin) == laststate) {
        while (suli_pin_read(io) == laststate)
        {
            counter++;
            //delayMicroseconds(1);
            suli_delay_us(1);
            if (counter == 255)
            {
                break;
            }
        }
        //laststate = digitalRead(&_pin);
        laststate = suli_pin_read(io);

        if (counter == 255) break;

        // ignore first 3 transitions
        if ((i >= 4) && (i % 2 == 0))
        {
            // shove each bit into the storage bytes
            data[j / 8] <<= 1;
            if (counter > _count) //
                data[j / 8] |= 1;
            j++;
        }

    }

    // check we read 40 bits and that the checksum matches
    if ((j >= 40) &&
        (data[4] == ((data[0] + data[1] + data[2] + data[3]) & 0xFF)))
    {
        return true;
    }


    return false;

}