#include "REG.h"
#include "U8glib.h"
#include "wit_c_sdk.h"
#include <math.h>

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE);

/*
Test on MEGA 2560. use WT901CTTL sensor

WT901CTTL     MEGA 2560a
    VCC <--->  5V/3.3V
    TX  <--->  19(TX1)
    RX  <--->  18(RX1)
    GND <--->  GND
*/

#define ACC_UPDATE 0x01
#define GYRO_UPDATE 0x02
#define ANGLE_UPDATE 0x04
#define MAG_UPDATE 0x08
#define READ_UPDATE 0x80
static volatile char s_cDataUpdate = 0, s_cCmd = 0xff;

static void CmdProcess(void);
static void AutoScanSensor(void);
static void SensorUartSend(uint8_t *p_data, uint32_t uiSize);
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs);
const uint32_t c_uiBaud[8] = {0,     4800,  9600,   19200,
                              38400, 57600, 115200, 230400};

int i;
double dAcc[3], dGyro[3], dAngle[3];

double correction[3];

double dVelocity[3] = {0}, dDistance[3] = {0};

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  WitInit(WIT_PROTOCOL_NORMAL, 0x50);
  WitSerialWriteRegister(SensorUartSend);
  WitRegisterCallBack(SensorDataUpdata);
  WitDelayMsRegister(Delayms);
  AutoScanSensor();

  // if (WitStartAccCali() == WIT_HAL_OK) {
  //   Serial.print("Start Calibri");
  //   delay(1000);
  //   Serial.print("Stop calibri");
  //   WitStopAccCali();
  // } else {
  //   Serial.print("Cannot start calibri acc\r\n");
  // }

  if (WitSetUartBaud(WIT_BAUD_115200) != WIT_HAL_OK)
    Serial.print("\r\nSet Baud Error\r\n");
  else {
    Serial1.begin(c_uiBaud[WIT_BAUD_115200]);
    Serial.print("115200 Baud rate modified successfully\r\n");
  }

  if (WitSetOutputRate(RRATE_10HZ) != WIT_HAL_OK) {
    Serial.print("Set Report Rate Error\r\n");
  } else {
    Serial.print("Set Report Rate Success\r\n");
  }

  if (WitSetContent(RSW_ACC | RSW_ANGLE) != WIT_HAL_OK) {
    Serial.print("Set output content Error\r\n");
  } else {
    Serial.print("Set output content Success\r\n");
  }

  if (u8g.getMode() == U8G_MODE_R3G3B2) {
    u8g.setColorIndex(255); // white
  } else if (u8g.getMode() == U8G_MODE_GRAY2BIT) {
    u8g.setColorIndex(3); // max intensity
  } else if (u8g.getMode() == U8G_MODE_BW) {
    u8g.setColorIndex(1); // pixel on
  } else if (u8g.getMode() == U8G_MODE_HICOLOR) {
    u8g.setHiColorByRGB(255, 255, 255);
  }

  u8g.setFont(u8g_font_profont12);
}

void loop() {
  while (Serial1.available()) {
    WitSerialDataIn(Serial1.read());
  }

  if (s_cDataUpdate) {
    for (i = 0; i < 3; i++) {
      dAcc[i] = sReg[AX + i] / 32768.0 * 16.0 * 9.8;           // Unit: m/s^2
      dGyro[i] = sReg[GX + i] / 32768.0 * 2000.0 / 180.0 * PI; // Unit: rad/s
      dAngle[i] = sReg[Roll + i] / 32768.0 * 180.0;               // Unit: rad
    }

    if (s_cDataUpdate & ACC_UPDATE) {
      Serial.print("acc:");
      Serial.print(dAcc[0], 3);
      Serial.print(" ");
      Serial.print(dAcc[1], 3);
      Serial.print(" ");
      Serial.print(dAcc[2], 3);
      Serial.print("\r\n");

      s_cDataUpdate &= ~ACC_UPDATE;
    }
    if (s_cDataUpdate & ANGLE_UPDATE) {
      Serial.print("angle:");
      Serial.print(dAngle[0], 3);
      Serial.print(" ");
      Serial.print(dAngle[1], 3);
      Serial.print(" ");
      Serial.print(dAngle[2], 3);
      Serial.print("\r\n");
      s_cDataUpdate &= ~ANGLE_UPDATE;
    }
    s_cDataUpdate = 0;
  }

  u8g.firstPage();
  do {
    u8g.setFontPosTop();
    // u8g.drawStr(0, 8, "x: ");
    // u8g.setPrintPos(16, 8);
    // u8g.print(dAngle[0]);
    
    u8g.drawStr(0, 0, "Acc(m^2/s)");
    u8g.drawStr(64, 0, "Angle(deg)");

    u8g.drawStr(0, 16, "x: ");
    u8g.setPrintPos(14, 16);
    u8g.print(dAcc[0]);
    u8g.drawStr(0, 32, "y: ");
    u8g.setPrintPos(14, 32);
    u8g.print(dAcc[1]);
    u8g.drawStr(0, 48, "z: ");
    u8g.setPrintPos(14, 48);
    u8g.print(dAcc[2]);

    u8g.drawStr(64, 16, "x: ");
    u8g.setPrintPos(78, 16);
    u8g.print(dAngle[0]);
    u8g.drawStr(64, 32, "y: ");
    u8g.setPrintPos(78, 32);
    u8g.print(dAngle[1]);
    u8g.drawStr(64, 48, "z: ");
    u8g.setPrintPos(78, 48);
    u8g.print(dAngle[2]);
  } while (u8g.nextPage());
}

static void SensorUartSend(uint8_t *p_data, uint32_t uiSize) {
  Serial1.write(p_data, uiSize);
  Serial1.flush();
}
static void Delayms(uint16_t ucMs) { delay(ucMs); }
static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum) {
  int i;
  for (i = 0; i < uiRegNum; i++) {
    switch (uiReg) {
    case AZ:
      s_cDataUpdate |= ACC_UPDATE;
      break;
    case GZ:
      s_cDataUpdate |= GYRO_UPDATE;
      break;
    case HZ:
      s_cDataUpdate |= MAG_UPDATE;
      break;
    case Yaw:
      s_cDataUpdate |= ANGLE_UPDATE;
      break;
    default:
      s_cDataUpdate |= READ_UPDATE;
      break;
    }
    uiReg++;
  }
}

static void AutoScanSensor(void) {
  int i, iRetry;

  for (i = 0; i < sizeof(c_uiBaud) / sizeof(c_uiBaud[0]); i++) {
    Serial1.begin(c_uiBaud[i]);
    Serial1.flush();
    iRetry = 2;
    s_cDataUpdate = 0;
    do {
      WitReadReg(AX, 3);
      delay(200);
      while (Serial1.available()) {
        WitSerialDataIn(Serial1.read());
      }
      if (s_cDataUpdate != 0) {
        Serial.print(c_uiBaud[i]);
        Serial.print(" baud find sensor\r\n\r\n");
        return;
      }
      iRetry--;
    } while (iRetry);
  }
  Serial.print("can not find sensor\r\n");
  Serial.print("please check your connection\r\n");
}