#include <M5StickCPlus.h>

#define VL53L0X_REG_IDENTIFICATION_MODEL_ID         0xc0
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID      0xc2
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD   0x50
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
#define VL53L0X_REG_SYSRANGE_START                  0x00
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS         0x13
#define VL53L0X_REG_RESULT_RANGE_STATUS             0x14
#define address                                     0x29  // I2C address

byte gbuf[16];


TFT_eSprite spu = TFT_eSprite(&M5.Lcd); // Sprite object



uint16_t bswap(byte b[]) {
  // Big Endian unsigned short to little endian unsigned short
  uint16_t val = ((b[0] << 8) & b[1]);
  return val;
}

uint16_t makeuint16(int lsb, int msb) {
  return ((msb & 0xFF) << 8) | (lsb & 0xFF);
}

void write_byte_data(byte data) {
  Wire.beginTransmission(address);
  Wire.write(data);
  Wire.endTransmission();
}

void write_byte_data_at(byte reg, byte data) {
  // write data word at address and register
  Wire.beginTransmission(address);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

void write_word_data_at(byte reg, uint16_t data) {
  // write data word at address and register
  byte b0 = (data & 0xFF);
  byte b1 = ((data >> 8) && 0xFF);
  
  Wire.beginTransmission(address);
  Wire.write(reg);
  Wire.write(b0);
  Wire.write(b1);
  Wire.endTransmission();
}

byte read_byte_data() {
  Wire.requestFrom(address, 1);
  while (Wire.available() < 1) delay(1);
  byte b = Wire.read();
  return b;
}

byte read_byte_data_at(byte reg) {
  // write_byte_data((byte)0x00);
  write_byte_data(reg);
  Wire.requestFrom(address, 1);
  while (Wire.available() < 1) delay(1);
  byte b = Wire.read();
  return b;
}

uint16_t read_word_data_at(byte reg) {
  write_byte_data(reg);
  Wire.requestFrom(address, 2);
  while (Wire.available() < 2) delay(1);
  gbuf[0] = Wire.read();
  gbuf[1] = Wire.read();
  return bswap(gbuf);
}

void read_block_data_at(byte reg, int sz) {
  int i = 0;
  write_byte_data(reg);
  Wire.requestFrom(address, sz);
  for (i = 0; i < sz; i++) {
    while (Wire.available() < 1) delay(1);
    gbuf[i] = Wire.read();
  }
}

uint16_t VL53L0X_decode_vcsel_period(short vcsel_period_reg) {
  // Converts the encoded VCSEL period register value into the real
  // period in PLL clocks
  uint16_t vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
  return vcsel_period_pclks;
}

void setup() {
  
  M5.begin(115200);
  M5.Lcd.setRotation(3);
  
  spu.setColorDepth(8);
  spu.createSprite(240, 135);
  
  // put your setup code here, to run once:
  Wire.begin();          // join i2c bus (address optional for master)
  Serial.println("VLX53LOX test started.");
  
  
  //  M5.Lcd.setTextSize(2);
  //  M5.Lcd.fillScreen(TFT_YELLOW);
  //  M5.Lcd.setTextSize(2);
  //  M5.Lcd.setTextColor(TFT_BLACK);
  //  M5.Lcd.setCursor(5, 5);
  
}


void loop() {
  write_byte_data_at(VL53L0X_REG_SYSRANGE_START, 0x01);
  
  byte val = 0;
  int cnt  = 0;
  while (cnt < 100) {  // 1 second waiting time max
    delay(10);
    val = read_byte_data_at(VL53L0X_REG_RESULT_RANGE_STATUS);
    if (val & 0x01) break;
    cnt++;
  }
  if (val & 0x01){
    //    Serial.println("ready");
  } else {
    Serial.println("not ready");
  }
  read_block_data_at(0x14, 12);
  uint16_t acnt                  = makeuint16(gbuf[7], gbuf[6]);
  uint16_t scnt                  = makeuint16(gbuf[9], gbuf[8]);
  uint16_t dist                  = makeuint16(gbuf[11], gbuf[10]);
  byte DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3);
  
  spu.fillSprite( CYAN );
  
  spu.setCursor(93, 65, 4);  spu.setTextSize(2);
  spu.setTextColor( WHITE, BLUE );
  if (DeviceRangeStatusInternal != 11) {
    spu.setTextColor( WHITE, RED );
  }
  spu.printf(" %d \n", DeviceRangeStatusInternal);
  
  spu.setCursor(93, 5, 4);  spu.setTextSize(2);
//  spu.setTextColor( WHITE, BLUE );
  spu.printf(" %d \n", dist);
  
  //
  //  spu.setTextColor( BLUE, CYAN );
  //  spu.setCursor(10, 20, 4);  spu.setTextSize(1);
  //  spu.println("TVOC : ");
  //  spu.setCursor(10, 80, 4);  spu.setTextSize(1);
  //  spu.println("eCO2 : ");
  
  spu.pushSprite(0, 0); // 画面に反映させる
  
  //  Serial.printf("TVOC: %d eCO2: %d\n", sgp.TVOC, sgp.eCO2);
  
  M5.update();
  if (M5.Axp.GetBtnPress() == 2) {
    M5.Axp.Write1Byte(0x32, M5.Axp.Read8bit(0x32) | 0x80); // Power Off
  }
  delay(1000);
}