// for ESP32-C3 and SN65HVD230 CAN receiver

#include "driver/twai.h"
#include <stdio.h>

// ESP32-C3のGPIOピン設定
#define TX_PIN GPIO_NUM_2
#define RX_PIN GPIO_NUM_3

// OBD2の標準的なCAN ID
#define OBD_REQUEST_ID  0x7DF  
#define OBD_RESPONSE_ID 0x7E8  

// 一括出力用のグローバル固定長バッファ
char outputBuffer[256];

// 各PIDのデータを格納する配列の構造体定義
struct ObdParameter {
  uint8_t pid;          // OBD2のPIDコード
  const char* name;     // 表示名
  // 各パース関数：バッファ、書き込み可能残サイズ、最終フラグを受け取る仕様に変更
  void (*parser)(uint8_t* data, const char* name, char* buf, size_t max_len, bool isLast); 
};

// すべてのシリアル出力にタイムスタンプを自動付加する
void logWithTimestamp(const char* message, bool addNewLine = true) {
  unsigned long currentMillis = millis();
  unsigned long total_seconds = currentMillis / 1000;
  unsigned long hours = total_seconds / 3600;
  unsigned long minutes = (total_seconds % 3600) / 60;
  unsigned long seconds = total_seconds % 60;
  unsigned long ms = currentMillis % 1000;

  char timestamp[32]; 
  snprintf(timestamp, sizeof(timestamp), "[%lu:%02lu:%02lu.%03lu] ", hours, minutes, seconds, ms);
  
  Serial.print(timestamp);
  if (addNewLine) {
    Serial.println(message);
  } else {
    Serial.print(message);
  }
}

// 各パラメーターの解析関数
void parseRPM(uint8_t* data, const char* name, char* buf, size_t max_len, bool isLast) {
  uint16_t rpm = ((data[3] << 8) | data[4]) / 4;
  size_t current_len = strlen(buf);
  snprintf(buf + current_len, max_len - current_len, "%s: %u rpm%s", name, rpm, isLast ? "" : " | ");
}

void parseSpeed(uint8_t* data, const char* name, char* buf, size_t max_len, bool isLast) {
  uint8_t speed = data[3];
  size_t current_len = strlen(buf);
  snprintf(buf + current_len, max_len - current_len, "%s: %u km/h%s", name, speed, isLast ? "" : " | ");
}

void parseSupportedPids(uint8_t* data, const char* name, char* buf, size_t max_len, bool isLast) {
  size_t current_len = strlen(buf);
  snprintf(buf + current_len, max_len - current_len, "%s: %02X%02X%02X%02X%s",
           name, data[3], data[4], data[5], data[6], isLast ? "" : " | ");
}

// 取得情報の配列
ObdParameter obd_pids[] = {
  { 0x0C, "RPM",   parseRPM },   // エンジン回転数 (rpm)         
  { 0x0D, "Speed", parseSpeed }, // 車速 (km/h)

  { 0x00, "PID00", parseSupportedPids }, // PIDs 01-20 のサポート状況
  { 0x20, "PID20", parseSupportedPids }, // PIDs 21-40 のサポート状況
  { 0x40, "PID40", parseSupportedPids }, // PIDs 41-60 のサポート状況
  { 0x60, "PID60", parseSupportedPids }, // PIDs 61-80 のサポート状況
  { 0x80, "PID80", parseSupportedPids }, // PIDs 81-A0 のサポート状況
};

const uint8_t total_pids = sizeof(obd_pids) / sizeof(obd_pids[0]);
unsigned long lastLoopTime = 0;

void setup() {
  Serial.begin(115200);
  while(!Serial) delay(10);

  logWithTimestamp("OBD2 CAN Initializing...");

  // 指定の GPIO2/GPIO3 に合わせて設定
  twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(TX_PIN, RX_PIN, TWAI_MODE_NORMAL);
  twai_timing_config_t t_config  = TWAI_TIMING_CONFIG_500KBITS();

  // 0x7E8 のみを通すフィルターマスクの設定
  twai_filter_config_t f_config;
  f_config.acceptance_code = (OBD_RESPONSE_ID << 21);
  f_config.acceptance_mask = 0x001FFFFF; 
  f_config.single_filter = true;

  if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK && twai_start() == ESP_OK) {
    logWithTimestamp("TWAI driver started.");
  } else {
    logWithTimestamp("Failed to start TWAI driver.");
    return;
  }
}

void loop() {
  unsigned long currentMillis = millis();

  // 1秒に1回、まとめて情報を取りにいく
  if (currentMillis - lastLoopTime >= 1000) {
    lastLoopTime = currentMillis;

    // char配列バッファを空文字列で初期化
    outputBuffer[0] = '\0';

    // 配列に入っているすべてのPIDを順番に処理
    for (uint8_t i = 0; i < total_pids; i++) {
      bool isLastItem = (i == total_pids - 1);

      twai_message_t rx_msg;     
      
      // 送信前に受信バッファに残っている古いデータをすべて読み飛ばす（フラッシュ）
      while (twai_receive(&rx_msg, 0) == ESP_OK) {
        // 何もしない（バッファを空にするため）
      }

      // リクエストを送信
      sendObdRequest(obd_pids[i].pid);

      // 返信を待つ
      unsigned long startTime = millis();
      bool received = false;

      while (millis() - startTime < 30) { 
        if (twai_receive(&rx_msg, pdMS_TO_TICKS(5)) == ESP_OK) {
          if (rx_msg.identifier == OBD_RESPONSE_ID && rx_msg.data_length_code == 8) {
            if (rx_msg.data[1] == 0x41 && rx_msg.data[2] == obd_pids[i].pid) {
              
              // 登録された関数で出力バッファに書き込み
              obd_pids[i].parser(rx_msg.data, obd_pids[i].name, outputBuffer, sizeof(outputBuffer), isLastItem);
              received = true;
              break; 
            }
          }
        }
      }

      if (!received) {
        // タイムアウトした場合は N/A を出力
        size_t current_len = strlen(outputBuffer);
        snprintf(outputBuffer + current_len, sizeof(outputBuffer) - current_len, "%s: N/A%s", obd_pids[i].name, isLastItem ? "" : " | ");
      }

      // 車両ECUの負荷軽減のためのウェイト
      delay(20);
    }

    // Serial出力
    logWithTimestamp(outputBuffer);
  }
}

// OBD2リクエスト送信関数
void sendObdRequest(uint8_t pid) {
  twai_message_t tx_msg = {}; // all 0
  
  tx_msg.identifier = OBD_REQUEST_ID;
  tx_msg.data_length_code = 8;

  tx_msg.data[0] = 2;    // 追加データのバイト数
  tx_msg.data[1] = 1;    // モード01
  tx_msg.data[2] = pid;  // 対象のPID

  twai_transmit(&tx_msg, pdMS_TO_TICKS(10));
}

[0:00:26.360] RPM: N/A | Speed: N/A | PID00: N/A | PID20: N/A | PID40: N/A | PID60: N/A | PID80: N/A
[0:00:27.360] RPM: N/A | Speed: N/A | PID00: N/A | PID20: N/A | PID40: N/A | PID60: N/A | PID80: N/A
[0:00:28.360] RPM: N/A | Speed: N/A | PID00: N/A | PID20: N/A | PID40: N/A | PID60: N/A | PID80: N/A

# エンジン始動
[0:01:03.270] RPM: 164 rpm | Speed: 0 km/h | PID00: 981B8013 | PID20: 8001A001 | PID40: CCC00001 | PID60: 00890141 | PID80: 21300000
[0:01:04.270] RPM: 804 rpm | Speed: 0 km/h | PID00: 981B8013 | PID20: 8001A001 | PID40: CCC00001 | PID60: 00890141 | PID80: 21300000

# 実走行
[0:01:37.272] RPM: 785 rpm | Speed: 2 km/h | PID00: 981B8013 | PID20: 8001A001 | PID40: CCC00001 | PID60: 00890141 | PID80: 21300000
[0:01:38.272] RPM: 826 rpm | Speed: 4 km/h | PID00: 981B8013 | PID20: 8001A001 | PID40: CCC00001 | PID60: 00890141 | PID80: 21300000
[0:01:40.271] RPM: 979 rpm | Speed: 8 km/h | PID00: 981B8013 | PID20: 8001A001 | PID40: CCC00001 | PID60: 00890141 | PID80: 21300000

PIDS = {0x00: 0x981B8013, 
        0x20: 0x8001A001, 
        0x40: 0xCCC00001, 
        0x60: 0x00890141, 
        0x80: 0x21300000}

for id in PIDS:
    bits = PIDS[id]
    print(f"0x{id:02x}:", end="")
    for b in range(32):
        if (bits << b) & 0x80000000:
            print(f" 0x{id + b + 1:02X}", end="")
    print()

0x00: 0x01 0x04 0x05 0x0C 0x0D 0x0F 0x10 0x11 0x1C 0x1F 0x20
0x20: 0x21 0x30 0x31 0x33 0x40
0x40: 0x41 0x42 0x45 0x46 0x49 0x4A 0x60
0x60: 0x69 0x6D 0x70 0x78 0x7A 0x80
0x80: 0x83 0x88 0x8B 0x8C