/*----------------------------------------------------------------------*
 * M5Stack I2C Common Library v1.0                                      *
 *                                                                      *
 * This work is licensed under the GNU Lesser General Public            *
 * License v2.1                                                         *
 * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.en.html           *
 *----------------------------------------------------------------------*/
#include "CommUtil.h"
#include "../M5Stack.h"

extern M5Stack M5;

//debug for message of I2C ( bypass message to serial)
//#define  I2C_DEBUG_TO_SERIAL

CommUtil::CommUtil() {
}

// Wire.h read and write protocols
bool CommUtil::writeCommand(uint8_t address, uint8_t subAddress) {
  bool function_result = false;
  Wire.beginTransmission(address);               // Initialize the Tx buffer
  Wire.write(subAddress);                        // Put slave register address in Tx buffer
  function_result = (Wire.endTransmission() == 0); // Send the Tx buffer

  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("writeCommand:send to 0x%02x [0x%02x] result:%s\n", address, subAddress, function_result ? "OK" : "NG");
  #endif

  return (function_result);
}

// Wire.h read and write protocols
bool CommUtil::writeByte(uint8_t address, uint8_t subAddress, uint8_t data) {
  bool function_result = false;
  Wire.beginTransmission(address);  // Initialize the Tx buffer
  Wire.write(subAddress);           // Put slave register address in Tx buffer
  Wire.write(data);                 // Put data in Tx buffer
  function_result = (Wire.endTransmission() == 0); // Send the Tx buffer

  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("writeByte:send to 0x%02x [0x%2x] data=0x%02x result:%s\n", address, subAddress, data, function_result ? "OK" : "NG");
  #endif

  return (function_result);
}

// Wire.h read and write protocols
bool CommUtil::writeBytes(uint8_t address, uint8_t subAddress, uint8_t *data, uint8_t length) {
  bool function_result = false;

  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("writeBytes:send to 0x%02x [0x%02x] data=",address,subAddress);
  #endif

  Wire.beginTransmission(address);                // Initialize the Tx buffer
  Wire.write(subAddress);                         // Put slave register address in Tx buffer
  for(int i = 0; i < length; i++) {
    Wire.write(*(data+i));                        // Put data in Tx buffer
    #ifdef I2C_DEBUG_TO_SERIAL
      Serial.printf("%02x ", *(data+i));
    #endif
  }
  function_result = (Wire.endTransmission() == 0);  // Send the Tx buffer

  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("result:%s\n", function_result ? "OK" : "NG");
  #endif

  return function_result;             // Send the Tx buffer
}

bool CommUtil::readByte(uint8_t address, uint8_t *result) {
  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("readByte :read from 0x%02x requestByte=1 receive=", address);
  #endif

  if ( Wire.requestFrom(address, (uint8_t)1)) {
    *result = Wire.read();                          // Fill Rx buffer with result
    #ifdef I2C_DEBUG_TO_SERIAL
      Serial.printf("%02x\n", result);
    #endif
    return true;
  }
  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("none\n");
  #endif
  return false;
}

bool CommUtil::readByte(uint8_t address, uint8_t subAddress,uint8_t *result) {
  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("readByte :read from 0x%02x [0x%02x] requestByte=1 receive=", address, subAddress);
  #endif

  Wire.beginTransmission(address);         // Initialize the Tx buffer
  Wire.write(subAddress);                  // Put slave register address in Tx buffer
  if (Wire.endTransmission(false) == 0 && Wire.requestFrom(address, (uint8_t)1)) {
    *result = Wire.read();                 // Fill Rx buffer with result
    #ifdef I2C_DEBUG_TO_SERIAL
      Serial.printf("%02x\n",*result);
    #endif
    return true;
  }
  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("none\n");
  #endif
  return false;
}

bool CommUtil::readBytes(uint8_t address, uint8_t subAddress, uint8_t count,uint8_t * dest) {
  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("readBytes:read from 0x%02x [0x%02x] requestByte=%d receive=", address, subAddress, count);
  #endif

  Wire.beginTransmission(address);   // Initialize the Tx buffer
  Wire.write(subAddress);            // Put slave register address in Tx buffer
  uint8_t i = 0;
  if (Wire.endTransmission(false) == 0 && Wire.requestFrom(address, (uint8_t)count)) {
    while (Wire.available()) {
      dest[i++] = Wire.read();// Put read results in the Rx buffer
      #ifdef I2C_DEBUG_TO_SERIAL
        Serial.printf("%02x ", dest[i-1]);
      #endif
    }
    #ifdef I2C_DEBUG_TO_SERIAL
      Serial.printf(" (len:%d)\n", i);
    #endif
    return true;
  }
  #ifdef I2C_DEBUG_TO_SERIAL
    Serial.printf("none\n");
  #endif
  return false;
}

bool CommUtil::readBytes(uint8_t address, uint8_t count,uint8_t * dest) {
  uint8_t i = 0;
  if (Wire.requestFrom(address, (uint8_t)count)) {
    while (Wire.available()) {
      // Put read results in the Rx buffer
      dest[i++] = Wire.read();
    }
    return true;
  }
  return false;
}

void CommUtil::scanID(bool *result) {
  for(int i=0x00;i<=0x7f;i++) {
    *(result+i)=writeCommand(i,0x00);
  }
}
