#include <Wire.h> // Thư viện I2C

#define tcaAddress 0x70 // Địa chỉ I2C của TCA9548A

// Biến cảm biến từ tính
int trangThaiNamCham = 0; // Giá trị của thanh ghi trạng thái (MD, ML, MH)
int byteThap; // Góc thô 7:0
word byteCao; // Góc thô 7:0 và 11:8
int gocThuc; // Góc thô cuối cùng
float gocDo; // Góc thô tính bằng độ (360/4096 * [giá trị từ 0-4095])

int soPhanTu, soPhanTuTruoc; // Số phần tử (quadrant) hiện tại và trước đó
float soVongQuay = 0; // Số vòng quay
float gocChinhXac = 0; // Góc đã chỉnh (tare) - dựa trên giá trị khởi đầu
float gocBatDau = 0; // Góc khởi đầu
float gocTong = 0; // Tổng góc quay tuyệt đối
float gocTongTruoc = 0; // Để so sánh nhằm tránh in ra không cần thiết

void setup()
{
  Serial.begin(9600); // Bắt đầu giao tiếp Serial
  Wire.begin(); // Khởi động I2C
  Wire.setClock(800000L); // Tốc độ xung nhịp nhanh

  for (int i = 0; i < 6; i++) {
    tcaselect(i);  // Chọn kênh trên TCA9548A
    kiemTraNamCham(); // Kiểm tra nam châm (khóa chương trình cho đến khi nam châm được phát hiện)
    docGocThuc(); // Đọc góc ban đầu để cập nhật gocDo
    gocBatDau = gocDo; // Đặt gocBatDau dựa trên giá trị ban đầu
  }
}

void loop()
{
  for (int i = 0; i < 6; i++) {
    tcaselect(i);  // Chọn kênh trên TCA9548A
    docGocThuc();  // Đọc giá trị từ cảm biến
    chinhGoc();    // Chỉnh góc
    kiemTraPhanTu(); // Kiểm tra phần tử, vòng quay, và vị trí góc tuyệt đối
    delay(500); // Tạm dừng giữa các lần đọc
  }
  delay(1000); // Tạm dừng 1 giây trước khi lặp lại
}

void tcaselect(uint8_t i)
{
  if (i > 7) return; // Chỉ có 8 kênh từ 0 đến 7
  Wire.beginTransmission(tcaAddress);
  Wire.write(1 << i); // Chọn kênh dựa trên bitmask
  Wire.endTransmission();
  delay(10); // Đợi 10ms để đảm bảo ổn định
}

void docGocThuc()
{
  // Đọc byte thấp của góc thô (7:0 bits)
  Wire.beginTransmission(0x36); // Địa chỉ I2C của AS5600
  Wire.write(0x0D); // Thanh ghi để đọc góc thô (7:0)
  Wire.endTransmission();
  Wire.requestFrom(0x36, 1); // Yêu cầu một byte
  while (Wire.available() == 0); 
  byteThap = Wire.read(); 

  // Đọc byte cao của góc thô (11:8 bits)
  Wire.beginTransmission(0x36);
  Wire.write(0x0C); // Thanh ghi để đọc góc thô (11:8)
  Wire.endTransmission();
  Wire.requestFrom(0x36, 1);
  while (Wire.available() == 0);  
  byteCao = Wire.read();

  // Kết hợp byte thấp và byte cao để tạo ra giá trị góc thô
  byteCao = byteCao << 8; // Dịch byte cao sang vị trí chính xác
  gocThuc = byteCao | byteThap; // Kết hợp byte cao và byte thấp

  // Tính toán góc tính bằng độ
  gocDo = gocThuc * 0.087890625; // 360/4096 = 0.087890625
  
  // In ra góc tính bằng độ
  Serial.print("Góc tính bằng độ: ");
  Serial.println(gocDo, 2); // In với 2 chữ số sau dấu phẩy
}

void chinhGoc()
{
  gocChinhXac = gocDo - gocBatDau;
  if (gocChinhXac < 0) 
  {
    gocChinhXac += 360; // Chuyển góc âm thành dương
  }
  Serial.print("Góc đã chỉnh: ");
  Serial.println(gocChinhXac, 2); // In giá trị góc đã chỉnh
}

void kiemTraPhanTu()
{
  // Xác định phần tử hiện tại dựa trên góc đã chỉnh
  if (gocChinhXac >= 0 && gocChinhXac <= 90)
  {
    soPhanTu = 1;
  }
  else if (gocChinhXac > 90 && gocChinhXac <= 180)
  {
    soPhanTu = 2;
  }
  else if (gocChinhXac > 180 && gocChinhXac <= 270)
  {
    soPhanTu = 3;
  }
  else if (gocChinhXac > 270 && gocChinhXac < 360)
  {
    soPhanTu = 4;
  }
  
  Serial.print("Phần tử: ");
  Serial.println(soPhanTu);

  // Kiểm tra và cập nhật số vòng quay
  if (soPhanTu != soPhanTuTruoc)
  {
    if (soPhanTu == 1 && soPhanTuTruoc == 4)
    {
      soVongQuay++;
    }
    else if (soPhanTu == 4 && soPhanTuTruoc == 1)
    {
      soVongQuay--;
    }
    soPhanTuTruoc = soPhanTu;
  }
  
  Serial.print("Số vòng quay: ");
  Serial.println(soVongQuay, 0);

  // Tính toán vị trí tuyệt đối
  gocTong = (soVongQuay * 360) + gocChinhXac;
  Serial.print("Tổng góc: ");
  Serial.println(gocTong, 2); // In với 2 chữ số sau dấu phẩy
}

void kiemTraNamCham()
{
  while ((trangThaiNamCham & 32) != 32)
  {
    trangThaiNamCham = 0;
    Wire.beginTransmission(0x36); 
    Wire.write(0x0B); // Đọc trạng thái nam châm
    Wire.endTransmission();
    Wire.requestFrom(0x36, 1); 
    while (Wire.available() == 0); 
    trangThaiNamCham = Wire.read();
    Serial.print("Trạng thái nam châm: ");
    Serial.println(trangThaiNamCham);
    delay(2000);
  }
}