#include "ir.h"

IRData IR::data;
TwoWire *IR::i2c;

void IR::init()
{
  Serial.println("IR Init");
  data.mode = IR_MODE;
  data.count = IR_COUNT;
  #if IR_MODE == 1
  {
    uint8_t pins[IR_COUNT] = IR_PINS;
    for (int i = 0; i < IR_COUNT; i++)
    {
      data.pins[i] = pins[i];
    };
  }
  #elif IR_MODE == 2
  {
    uint8_t pins[2] = IR_PINS;
    data.pins[0] = pins[0]; // SCL
    data.pins[1] = pins[1]; // SDA
    
    // 检查引脚值是否有效
    if (data.pins[0] >= 0 && data.pins[1] >= 0) {
      i2c = new TwoWire(0);
      if (i2c->begin(data.pins[1], data.pins[0])) {
        Serial.println("I2C initialized successfully");
      } else {
        Serial.println("I2C initialization failed");
      }
    } else {
      Serial.println("Invalid I2C pins");
    }
  }
  #endif
}

void IR::update()
{
  #if IR_MODE == 1
  {
    for (int i = 0; i < data.count; i++)
    {
      // 读取传感器数据
      bool value = digitalRead(data.pins[i]);
      
      // 根据 IR_REVERSE 判断是否需要反转电平
      #if IR_OUTPUT_REVERSE
      value = !value;
      #endif

      // 根据传感器排列方向存储数据
      #ifdef IR_DIRECTION_REVERSE
      data.data[data.count - 1 - i] = value;
      #else
      data.data[i] = value;
      #endif
    }
  }
  #elif IR_MODE == 2
  {
    if (i2c != nullptr)
    {
      byte value = 0;
      
      delay(10);
      
      i2c->requestFrom(0x12, 1);
      if(i2c->available()) {
        value = i2c->read();
        
        // 根据 IR_COUNT 读取数据
        for(int i = 0; i < std::min<int>(data.count, IR_COUNT); i++) {
          // 读取传感器数据
          bool sensorValue = !((value >> (7-i)) & 0x01);
          
          // 根据 IR_REVERSE 判断是否需要反转电平
          #if IR_OUTPUT_REVERSE
          sensorValue = !sensorValue;
          #endif

          // 根据传感器排列方向存储数据
          #ifdef IR_DIRECTION_REVERSE
          data.data[std::min<int>(data.count, IR_COUNT) - 1 - i] = sensorValue;
          #else
          data.data[i] = sensorValue;
          #endif
        }
      } else {
        // 通信失败时清零数据
        memset(data.data, 0, IR_COUNT);
      }
    }
  }
  #endif
}