#include <Arduino.h>	// Arduino 基本API
#include <Wire.h>	// I2C 通信
#include <arduinoFFT.h>	// FFT ライブラリ
#include "freertos/FreeRTOS.h"	// FreeRTOS 基本
#include "freertos/semphr.h"	// FreeRTOS セマフォ
#include <WiFi.h>	// Wi-Fi
#include <WiFiUdp.h>	// UDP 送信
#include "mbedtls/base64.h"	// Base64 エンコード
#include <FastLED.h>	// WS2812B LED 制御
CRGB leds[1];	// LED 1個分のバッファ
#define WIFI_SSID     "tora-neko"	// 接続するSSID
#define WIFI_PASS     "nyanyanya"	// Wi-Fi パスワード
#define SERVER_IP     "192.168.8.191"	// UDP 送信先 IP
#define SERVER_PORT   50000	// UDP 送信先 ポート
const int  node_id = 1;	// ノードID（N00, N01…の00部分）
WiFiUDP udp;	// UDP ソケット
IPAddress serverIp;	// 送信先 IP を保持

static inline void i2c_write(uint8_t reg, uint8_t val, uint8_t addr){ 
  Wire.beginTransmission(addr);	// I2C指定アドレスへ
  Wire.write(reg); Wire.write(val);	// データ書き込み
  Wire.endTransmission();
}
static inline void i2c_read(uint8_t reg, uint8_t *d, uint8_t n, uint8_t addr){ 
  Wire.beginTransmission(addr);	//I2C指定アドレスからnバイト読み取り
  Wire.write(reg); Wire.endTransmission(false); 
  Wire.requestFrom(addr, n);
  for(uint8_t i=0;i<n;i++) d[i]=Wire.read();
}
static inline void mag_xyz_read(int16_t *v){	//BM1422AGMVを2台読み出し
  uint8_t b[6];
  i2c_read(0x10,b,6,0x0E);	// 0x0E の測定データ取得
  v[0]=(int16_t)(b[0]|(b[1]<<8));	// X
  v[1]=(int16_t)(b[2]|(b[3]<<8));	// Y
  v[2]=(int16_t)(b[4]|(b[5]<<8));	// Z
  i2c_write(0x1D,0x40,0x0E);	// 測定開始（次回用トリガ）
  i2c_read(0x10,b,6,0x0F);	// 0x0F の測定データ取得
  v[0]=(v[0]+(int16_t)(b[0]|(b[1]<<8)))/2;	// 2台の平均
  v[1]=(v[1]+(int16_t)(b[2]|(b[3]<<8)))/2;	// グラディオメータの場合は、
  v[2]=(v[2]+(int16_t)(b[4]|(b[5]<<8)))/2;	// 引き算に変更する
  i2c_write(0x1D,0x40,0x0F);	// 測定開始（次回用トリガ）
}
using T=float; ArduinoFFT<T> FFT;	// FFT は float 精度で実施
const uint16_t NFFT=1024;	// FFT ポイント数
const uint8_t  NAVG=16;	// Welch 平均の枚数（リングバッファ長）
const T        FS_HZ=1000.0f;	// サンプリング周波数（1 kHz）
const T SENS_uT_PER_LSB=0.042f;	// BM1422AGMVセンサ感度[µT/LSB]
const T LOG_SENS  = 20.0f*log10f(SENS_uT_PER_LSB);	// dB換算のオフセット（振幅→µTへ）
const T Uwin      = 0.375f;	// Hann窓の二乗平均（arduinoFFT のHannと整合）
const T LOG_DENPS = 10.0f*log10f(FS_HZ*(T)NFFT*Uwin);	// PSDスケールの分母(Fs*N*U)の dB
const T LOG_TWO   = 10.0f*log10f(2.0f);	// 片側スペクトルの2倍補正（≒3.01dB）
const T eps       = 1e-36f;	// log10(0)回避の微小量
const T QSTEP     = 0.5f;	// Base64送信用量子化ステップ（dB/LSB）
const T QBIAS     = 64.0f;	// 量子化バイアス（-64〜+63.5dB を 0..255 に対応）
T xyzReal[3][NFFT], xyzImag[3][NFFT];	// 各軸の実部・虚部バッファ
T xyzPowHist[3][NAVG][NFFT/2];	// Welch平均用の履歴（パワー）リング
T xyzPowSum[3][NFFT/2];	// 各ビンの累積和（履歴の和）
T xyz_mean_acc[3];	// DC平均算出用の積算
uint16_t acc_counter=0;	// 取り込みサンプル数カウンタ
uint32_t seq=0;	// UDP パケットのシーケンス番号
hw_timer_t *timer=nullptr;	// 1kHz ハードウェアタイマ
SemaphoreHandle_t  sampleSem;	// タスク通知用のバイナリセマフォ

void IRAM_ATTR onTimerISR(){	// タイマ割り込み（1kHz）
  BaseType_t hp=pdFALSE; 
  if(sampleSem) xSemaphoreGiveFromISR(sampleSem,&hp);	//セマフォ待ちで
  if(hp) portYIELD_FROM_ISR();	// 高優先度(hp)タスク(samplingTask)があれば切替
}

void send_spectrum_b64(const uint8_t *q, size_t nbins, float fs, uint16_t nfft, char axis){
  const size_t qlen = nbins;	// エンコードする生配列長
  const size_t b64cap = ((qlen+2)/3)*4 + 8;	// Base64後の最大長（余裕8）
  static char b64[768];	// Base64 文字列バッファ
  size_t olen=0;
  mbedtls_base64_encode((unsigned char*)b64, b64cap, &olen, q, qlen);	// Base64化
  b64[olen] = '\0';	// C文字列終端
  char json[1024];	// 送信用JSONの箱
  int len = snprintf(json, sizeof(json),	// 送信用JSONを組み立て
    "{\"id\":\"N%02d\",\"seq\":%lu,\"fs\":%u,\"n\":%u,\"axis\":\"%c\",\"qstep\":%.3f,\"qbias\":%.1f,\"dby_b64\":\"%s\"}\n",
    node_id, (unsigned long)(seq++), (unsigned)fs,(unsigned)nfft, axis, (double)QSTEP, (double)QBIAS, b64);
  udp.beginPacket(serverIp, SERVER_PORT);	// UDP 送信開始
  udp.write((const uint8_t*)json, len);	// JSON を送信
  udp.endPacket();	// UDP 送信完了
}

void samplingTask(void*){	//サンプリングタスク（優先度3)
  uint8_t avgHead=0, framesAccum=0;	// Welch履歴の書き込み位置と平均枚数
  xyz_mean_acc[0]=xyz_mean_acc[1]=xyz_mean_acc[2]=0;	// DC平均用を初期化
  const uint16_t BINS = NFFT/2;	// 片側スペクトルのビン数
  static uint8_t qbuf[BINS];	// 量子化後の dB 配列（送信用）
  while(1){	// 常駐ループで
    xSemaphoreTake(sampleSem, portMAX_DELAY);	// ここで待ち。1kHzごとに起床
    int16_t v[3];	// 磁気データXYZ軸
    mag_xyz_read(v);	// 磁気センサ読み出し
    for(uint8_t a=0;a<3;a++){	// 3軸サンプル格納
      xyzReal[a][acc_counter]=(T)v[a];	// 実部に書き込み
      xyzImag[a][acc_counter]=0;	// 虚部はゼロ
      xyz_mean_acc[a]+=(T)v[a];	// 平均値計算用積算
    }	//サンプリングのみの場合はここで終了
    if(++acc_counter>=NFFT){	// 1024サンプル集まったらFFTする
      timerAlarmDisable(timer);	// タイマ停止（処理中に割り込み禁止）
      if(framesAccum<NAVG) framesAccum++;	// dB計算に使う平均枚数を進める
      const T TOTAL_OFFSET =             	 /* dB(µT/√Hz)への定数項 */
        -10.0f*log10f((T)framesAccum)    	 /* 平均枚数で割る */
        - LOG_DENPS                       	/* Fs*N*U で割る */
        + LOG_SENS;                       	/* LSB→µT への換算 */
      const T mx=xyz_mean_acc[0]/(T)NFFT;	// DC x 
      const T my=xyz_mean_acc[1]/(T)NFFT;	// DC y
      const T mz=xyz_mean_acc[2]/(T)NFFT;	// DC z
      for(uint16_t i=0;i<NFFT;i++){	// DC除去（各軸の平均を引く
        xyzReal[0][i]-=mx; xyzReal[1][i]-=my; xyzReal[2][i]-=mz;
      }
      T mag_y = my * SENS_uT_PER_LSB;	// LED：Y軸の直流磁界表示
      uint8_t led_g=0, led_r=0;	// 緑と赤の明るさ初期化
      if(mag_y > 0){ led_r = (mag_y > 255.0f) ? 255 : (uint8_t)mag_y;
      }else{ led_g = (mag_y < -255.0f) ? 255 : (uint8_t)(-mag_y); }
      leds[0] = CRGB(led_r,led_g,0);	// LED明るさ設定
      FastLED.show();	// LEDへ転送
      for(uint8_t a=0;a<3;a++){	// X,Y,Zの順でFFT
        FFT.windowing(xyzReal[a],NFFT,FFT_WIN_TYP_HANN,FFT_FORWARD);// 窓(Hann) 
        FFT.compute   (xyzReal[a],xyzImag[a],NFFT,FFT_FORWARD);	// FFT 計算 
        FFT.complexToMagnitude(xyzReal[a],xyzImag[a],NFFT);	// 複素→振幅|X|
      }
      for (uint16_t k=0; k<BINS; k++){	// Welch移動平均：|X|^2 
        for(uint8_t a=0;a<3;a++){	// X,Y,Z軸ループ
          T p = xyzReal[a][k] * xyzReal[a][k];	// パワー（振幅^2）
          xyzPowSum[a][k] += p - xyzPowHist[a][avgHead][k];	// 移動平均
          xyzPowHist[a][avgHead][k] = p;	// 最新値を記録
        }
      }
      auto quantize_db = [](T dB)->uint8_t {	// 量子化: 0.5 dB/LSB, バイアス= +64 dB
        int qi = (int)lrintf( (dB + QBIAS) / QSTEP );	// -64〜+63.5 dB を 0..255 で表現）
        if(qi<0) qi=0; else if(qi>255) qi=255;	// 0..255 にクリップ
        return (uint8_t)qi;
      };
      for(char ax : {'X','Y','Z'}){	// X/Y/Z の順に 3パケット送信（各512bin）
         uint8_t aidx = (ax=='X')?0: (ax=='Z')?2:1;	// 軸→添字
         for (uint16_t k=0; k<BINS; k++){
           const T LOG_OS = (k==0 || k==(BINS-1)) ? 0.0f : LOG_TWO;
	// dB = 10log10(平均|X|^2) + 補正（枚数・片側・スケール）
           T dB = 10.0f*log10f(xyzPowSum[aidx][k] + eps) + TOTAL_OFFSET + LOG_OS;
           qbuf[k] = quantize_db(dB);	// 0..255 に量子化
         }
         send_spectrum_b64(qbuf, BINS, FS_HZ, NFFT, ax);	// 1軸ぶん送信
      }	// 3軸ぶんループ                                             
      avgHead=(avgHead+1)%NAVG;	// 履歴ポインタ更新、
      xyz_mean_acc[0]=xyz_mean_acc[1]=xyz_mean_acc[2]=0;	// 平均用累算クリア
      acc_counter=0;	// カウンタリセット
      timerAlarmEnable(timer);	// 1kHzタイマ再開
    }
  }
}	//サンプリングタスクここまで
void setup(){
  delay(node_id * 50);	// 起動遅延(ノードIDx50ms)
  Serial.begin(115200);	// デバッグ用シリアル
  Wire.begin(); Wire.setClock(400000);	// I2C 400kHz
  FastLED.addLeds<WS2812B, 21, GRB>(leds, 1);	// LED(WS2812B)をGPIO21に1個接続
  pinMode(38,OUTPUT); digitalWrite(38,HIGH);	// M5Stamp S3A LED電源 ON
  WiFi.mode(WIFI_STA);	// Wi-Fi 接続
  WiFi.setSleep(false);
  WiFi.begin(WIFI_SSID, WIFI_PASS);
  while(WiFi.status()!=WL_CONNECTED){ delay(200); }	// WiFi接続待ち
  serverIp.fromString(SERVER_IP);	// 送信先IPを設定
  sampleSem=xSemaphoreCreateBinary();	// タイマ→タスク通知セマフォ
  timer=timerBegin(0,80,true);	// タイマ0を ÷80で1MHz
  timerAttachInterrupt(timer,&onTimerISR,true);	// ISR登録（エッジ）
  timerAlarmWrite(timer,1000,true);	// 1000カウントごと（1ms）に割り込み
  i2c_write(0x1B,0xDA,0x0E);	// CTRL1: Active=1,14bit,Single
  i2c_write(0x1B,0xDA,0x0F);
  delay(2);
  i2c_write(0x5D,0x00,0x0E);	// CTRL4: Reset解除
  i2c_write(0x5D,0x00,0x0F);
  i2c_write(0x1C,0x0C,0x0E);	// CTRL2: DRDY出力有効（動作確認用）
  i2c_write(0x1C,0x0C,0x0F);
  i2c_write(0x40,0x00,0x0E);	// AVE_A: 4回平均
  i2c_write(0x40,0x00,0x0F);
  i2c_write(0x1D,0x40,0x0E);	// CNTL3: 測定開始（トリガ）
  i2c_write(0x1D,0x40,0x0F);
  for(uint16_t k=0;k<NFFT/2;k++){	// Welch バッファ初期化（履歴と累積和をゼロ）
    xyzPowSum[0][k]=xyzPowSum[1][k]=xyzPowSum[2][k]=0;
    for(uint8_t j=0;j<NAVG;j++){
      xyzPowHist[0][j][k]=xyzPowHist[1][j][k]=xyzPowHist[2][j][k]=0;
    }
  }
	// サンプリングタスクをCore1で起動（優先度3, 8KBスタック）
  xTaskCreatePinnedToCore(samplingTask,"sampler",8192,nullptr,3,nullptr,1);
  timerAlarmEnable(timer);	// タイマ開始 → サンプリングスタート
}
void loop(){}	// メインループは未使用（タスクとISRで駆動）

