Pict_Camera_LCD.ino

#include <esp_camera.h>
#include <SPI.h>
#include <SD.h>
#include <M5UnitLCD.h>
#include <M5Unified.h>
#include "Unit_Encoder.h"
#include <algorithm>

#define POWER_GPIO_NUM 18
camera_fb_t* fb;

M5UnitLCD display;
M5Canvas canvas0;
M5Canvas canvas1;

Unit_Encoder encoder;                   // Unit Encoderのインスタンス
bool buttonPressed = false;             // ボタンの状態
signed short int newEncoderValue = 0;   // エンコーダの値　新
signed short int lastEncoderValue = 0;  // エンコーダの値　旧
int accumulatedChange = 0;              // エンコーダ値の累積変化量
int mode = -1;                          // 撮影モード (-1から7)
const int modeMax = 7;                  // モードの最大値
char filename[64];                      // SDカード保存ファイル名
char filesaveprogress[24];              // 保存中表示名
char currentmode[16];                   // モード表示
int filecounter = 1;                    // ファイルカウンターは電源を入れるたびにリセットされる　極稀にファイル名が被るかも
uint8_t graydata[240 * 176];            // 輝度情報保存
int currentPalettelndex = 0;            // 現在のパレットのインデックス
int maxPalettelndex = 8;                // パレット総数
uint32_t LED_ON_DURATION = 120000;      // LED 点灯時間 (ミリ秒)
uint32_t keyOnTime = 0;                 // キースイッチを操作した時間
int dither = 0;                         // 0 = ディザ未使用 1 = ディザ使用
int levels = 8;                        // ディザ階調数 2以上

// 最大8色のカラーパレット デフォルト
uint32_t ColorPalettes[8][8] = {
  { // パレット0 slso8
    0x0D2B45, 0x203C56, 0x544E68, 0x8D697A, 0xD08159, 0xFFAA5E, 0xFFD4A3, 0xFFECD6 },
  { // パレット1 都市伝説解体センター風
    0x000000, 0x000B22, 0x112B43, 0x437290, 0x437290, 0xE0D8D1, 0xE0D8D1, 0xFFFFFF },
  { // パレット2 ファミレスを享受せよ風
    0x010101, 0x33669F, 0x33669F, 0x33669F, 0x498DB7, 0x498DB7, 0xFBE379, 0xFBE379 },
  { // パレット3 gothic-bit
    0x0E0E12, 0x1A1A24, 0x333346, 0x535373, 0x8080A4, 0xA6A6BF, 0xC1C1D2, 0xE6E6EC },
  { // パレット4 noire-truth
    0x1E1C32, 0x1E1C32, 0x1E1C32, 0x1E1C32, 0xC6BAAC, 0xC6BAAC, 0xC6BAAC, 0xC6BAAC },
  { // パレット5 2BIT DEMIBOY
    0x252525, 0x252525, 0x4B564D, 0x4B564D, 0x9AA57C, 0x9AA57C, 0xE0E9C4, 0xE0E9C4 },
  { // パレット6 deep-maze
    0x001D2A, 0x085562, 0x009A98, 0x00BE91, 0x38D88E, 0x9AF089, 0xF2FF66, 0xF2FF66 },
  { // パレット7 night-rain
    0x000000, 0x012036, 0x3A7BAA, 0x7D8FAE, 0xA1B4C1, 0xF0B9B9, 0xFFD159, 0xFFFFFF },
};

camera_config_t camera_config = {
  .pin_pwdn = -1,
  .pin_reset = -1,
  .pin_xclk = 21,
  .pin_sscb_sda = 12,
  .pin_sscb_scl = 9,
  .pin_d7 = 13,
  .pin_d6 = 11,
  .pin_d5 = 17,
  .pin_d4 = 4,
  .pin_d3 = 48,
  .pin_d2 = 46,
  .pin_d1 = 42,
  .pin_d0 = 3,

  .pin_vsync = 10,
  .pin_href = 14,
  .pin_pclk = 40,

  .xclk_freq_hz = 20000000,
  .ledc_timer = LEDC_TIMER_0,
  .ledc_channel = LEDC_CHANNEL_0,

  .pixel_format = PIXFORMAT_RGB565,
  .frame_size = FRAMESIZE_HQVGA,
  // FRAMESIZE_96X96,    // 96x96
  // FRAMESIZE_QQVGA,    // 160x120
  // FRAMESIZE_QCIF,     // 176x144
  // FRAMESIZE_HQVGA,    // 240x176
  // FRAMESIZE_240X240,  // 240x240
  // FRAMESIZE_QVGA,     // 320x240

  .jpeg_quality = 0,
  .fb_count = 2,
  .fb_location = CAMERA_FB_IN_PSRAM,
  .grab_mode = CAMERA_GRAB_LATEST,
  .sccb_i2c_port = 0,
};

bool CameraBegin() {
  esp_err_t err = esp_camera_init(&camera_config);
  if (err != ESP_OK) {
    return false;
  }

  // カメラ追加設定
  sensor_t* s = esp_camera_sensor_get();

  // AtomS3R Cam のときはこちら
  s->set_hmirror(s, 1);  // 左右反転 0無効 1有効
  s->set_vflip(s, 1);    // 上下反転 0無効 1有効

  // AtomS3R M12 のときはこちら
  //s->set_lenc(s, 1);     // レンズ補正？ 効いてるか微妙
  //s->set_hmirror(s, 1);  // 左右反転 0無効 1有効
  //s->set_vflip(s, 0);    // 上下反転 0無効 1有効

  return true;
}

bool CameraGet() {
  fb = esp_camera_fb_get();
  if (!fb) {
    return false;
  }
  return true;
}

bool CameraFree() {
  if (fb) {
    esp_camera_fb_return(fb);
    return true;
  }
  return false;
}

bool loadPaletteFromSD(int paletteIndex) {
  if (paletteIndex < 0 || paletteIndex > 7) {
    return false;
  }

  // ファイル名を生成 (例: /ColorPalette0.txt)
  String filename = "/ColorPalette" + String(paletteIndex) + ".txt";

  // ファイルが存在するか確認
  if (!SD.exists(filename)) {
    return false;  // ファイルが存在しない場合はデフォルトを使うのでfalseを返す
  }

  // ファイルを開く
  File file = SD.open(filename, FILE_READ);
  if (!file) {
    return false;  // ファイルオープン失敗
  }

  // ファイルから8つのカラーコードを読み込む
  int colorCount = 0;
  while (file.available() && colorCount < 8) {
    String line = file.readStringUntil('\n');  // 1行読み込む
    line.trim();                               // 前後の空白や改行文字を削除

    if (line.length() > 0) {
      // strtoul(const char *str, char **endptr, int base)
      // base=0 で 0x (16進), 0 (8進), それ以外 (10進) を自動判別
      uint32_t colorValue = strtoul(line.c_str(), NULL, 0);

      // エラーチェック (strtoulはエラー時に0を返すことがあるが、0x000000も有効な色なので完全ではない)
      // ここでは単純に読み込んだ値を格納する
      ColorPalettes[paletteIndex][colorCount] = colorValue;
      colorCount++;
    }
  }

  file.close();  // ファイルを閉じる

  // 8色読み込めたか確認
  if (colorCount == 8) {
    return true;  // 成功
  } else {
    // もし8色以下の場合は読み込めた分だけ反映して残りはデフォルトを使用する
    return false;  // 読み込み失敗（色が足りない）
  }
}

void saveToSD_OriginalBMP() {
  sprintf(filename, "/%010d_%04d_Original.bmp", keyOnTime, filecounter);
  File file = SD.open(filename, "w");
  if (file) {
    uint8_t* out_bmp = NULL;
    size_t out_bmp_len = 0;
    frame2bmp(fb, &out_bmp, &out_bmp_len);
    file.write(out_bmp, out_bmp_len);
    file.close();
    free(out_bmp);
  } else {
    error_lamp();
  }
}

void saveToSD_DisplayBMP() {
  sprintf(filename, "/%010d_%04d_Display.bmp", keyOnTime, filecounter);
  File file = SD.open(filename, "w");
  if (file) {

    int width = display.width();
    int height = display.height();
    int rowSize = (3 * width + 3) & ~3;

    lgfx::bitmap_header_t bmpheader;
    bmpheader.bfType = 0x4D42;
    bmpheader.bfSize = rowSize * height + sizeof(bmpheader);
    bmpheader.bfOffBits = sizeof(bmpheader);
    bmpheader.biSize = 40;
    bmpheader.biWidth = width;
    bmpheader.biHeight = height;
    bmpheader.biPlanes = 1;
    bmpheader.biBitCount = 24;
    bmpheader.biCompression = 0;
    bmpheader.biSizeImage = 0;
    bmpheader.biXPelsPerMeter = 2835;
    bmpheader.biYPelsPerMeter = 2835;
    bmpheader.biClrUsed = 0;
    bmpheader.biClrImportant = 0;

    file.write((std::uint8_t*)&bmpheader, sizeof(bmpheader));
    std::uint8_t buffer[rowSize];
    memset(&buffer[rowSize - 4], 0, 4);
    for (int y = height - 1; y >= 0; y--) {
      display.readRect(0, y, width, 1, (lgfx::rgb888_t*)buffer);
      file.write(buffer, rowSize);
    }
    file.close();
  } else {
    error_lamp();
  }
}

void saveToSD_ConvertBMP() {
  sprintf(filename, "/%010d_%04d_Palette%01d.bmp", keyOnTime, filecounter, currentPalettelndex);
  File file = SD.open(filename, "w");
  if (file) {
    int width = fb->width;
    int height = fb->height;
    int rowSize = (3 * width + 3) & ~3;

    lgfx::bitmap_header_t bmpheader;
    bmpheader.bfType = 0x4D42;
    bmpheader.bfSize = rowSize * height + sizeof(bmpheader);
    bmpheader.bfOffBits = sizeof(bmpheader);
    bmpheader.biSize = 40;
    bmpheader.biWidth = width;
    bmpheader.biHeight = height;
    bmpheader.biPlanes = 1;
    bmpheader.biBitCount = 24;
    bmpheader.biCompression = 0;
    bmpheader.biSizeImage = 0;
    bmpheader.biXPelsPerMeter = 2835;
    bmpheader.biYPelsPerMeter = 2835;
    bmpheader.biClrUsed = 0;
    bmpheader.biClrImportant = 0;

    file.write((std::uint8_t*)&bmpheader, sizeof(bmpheader));
    std::uint8_t buffer[rowSize];
    memset(&buffer[rowSize - 4], 0, 4);
    for (int y = height - 1; y >= 0; y--) {
      for (int x = 0; x < width; x++) {

        // グレイデータを読み出す
        int i_gray = y * width + x;
        uint8_t gray = graydata[i_gray];

        // カラーパレットから色を取得
        uint32_t newColor = ColorPalettes[currentPalettelndex][gray];
        uint8_t r = (newColor >> 16) & 0xFF;
        uint8_t g = (newColor >> 8) & 0xFF;
        uint8_t b = newColor & 0xFF;

        // バッファに書き込み BGRの順になる
        int i_buffer = x * 3;
        buffer[i_buffer] = b;
        buffer[i_buffer + 1] = g;
        buffer[i_buffer + 2] = r;
      }
      file.write(buffer, rowSize);
    }
    file.close();
  } else {
    error_lamp();
  }
}

void saveGraylevel_fb() {
  uint8_t* fb_data = fb->buf;
  int width = fb->width;
  int height = fb->height;
  int i = 0;

  for (int y = 0; y < height; y++) {
    for (int x = 0; x < (width * 2); x = x + 2) {

      // 各ピクセルの色を取得
      uint32_t rgb565Color = (fb_data[y * width * 2 + x] << 8) | fb_data[y * width * 2 + x + 1];

      // RGB565からRGB888へ変換
      uint32_t rgb888Color = canvas0.color16to24(rgb565Color);
      uint8_t r = (rgb888Color >> 16) & 0xFF;
      uint8_t g = (rgb888Color >> 8) & 0xFF;
      uint8_t b = rgb888Color & 0xFF;

      // 輝度の計算 BT.709の係数を使用
      uint16_t luminance = (uint16_t)(0.2126 * r + 0.7152 * g + 0.0722 * b);

      // 輝度を16階調のグレースケールに変換
      uint8_t grayLevel = luminance / 32;  // 256/32 = 8

      // 輝度情報を保存
      graydata[i] = grayLevel;
      i++;
    }
  }
}

void saveGraylevel_canvas(M5Canvas& srcSprite) {
  int width = srcSprite.width();
  int height = srcSprite.height();
  int i = 0;

  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {

      // 各ピクセルの色を取得
      uint32_t rgb565Color = srcSprite.readPixel(x, y);

      // RGB565からRGB888へ変換
      uint32_t rgb888Color = srcSprite.color16to24(rgb565Color);
      uint8_t r = (rgb888Color >> 16) & 0xFF;
      uint8_t g = (rgb888Color >> 8) & 0xFF;
      uint8_t b = rgb888Color & 0xFF;

      // 輝度の計算 BT.709の係数を使用
      uint16_t luminance = (uint16_t)(0.2126 * r + 0.7152 * g + 0.0722 * b);

      // 輝度を16階調のグレースケールに変換
      uint8_t grayLevel = luminance / 32;  // 256/32 = 8

      // 輝度情報を保存
      graydata[i] = grayLevel;
      i++;
    }
  }
}


void convertColor_canvas(M5Canvas& srcSprite, M5Canvas& dstSprite) {

  int width = srcSprite.width();
  int height = srcSprite.height();

  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width; x++) {

      // 各ピクセルの色を取得
      uint32_t rgb565Color = srcSprite.readPixel(x, y);

      // RGB565からRGB888へ変換
      uint32_t rgb888Color = srcSprite.color16to24(rgb565Color);
      uint8_t r = (rgb888Color >> 16) & 0xFF;
      uint8_t g = (rgb888Color >> 8) & 0xFF;
      uint8_t b = rgb888Color & 0xFF;

      // 輝度の計算 BT.709の係数を使用
      uint16_t luminance = (uint16_t)(0.2126 * r + 0.7152 * g + 0.0722 * b);

      // 輝度を16階調のグレースケールに変換
      uint8_t grayLevel = luminance / 32;  // 256/32 = 8

      // カラーパレットから色を取得
      uint32_t newColor = ColorPalettes[currentPalettelndex][grayLevel];

      // 取得した色を書込
      dstSprite.drawPixel(x, y, dstSprite.color24to16(newColor));
    }
  }
  // delay(0);
}

void error_lamp() {
  encoder.setLEDColor(0, 0x400000);  //赤
}

void applyColorBayerDither4x4(M5Canvas& srcSprite, M5Canvas& dstSprite, int levelsPerChannel) {

  // Bayerマトリックス
  static const uint8_t bayer4x4[4][4] = {
    { 0, 8, 2, 10 }, { 12, 4, 14, 6 }, { 3, 11, 1, 9 }, { 15, 7, 13, 5 }
  };
  static const float bayerDivisor = 16.0f;

  int width = dstSprite.width();
  int height = dstSprite.height();
  float step = 255.0f / (float)(levelsPerChannel - 1);

  for (int y = 0; y < height; ++y) {
    for (int x = 0; x < width; ++x) {

      if (x >= srcSprite.width() || y >= srcSprite.height()) {
        dstSprite.drawPixel(x, y, TFT_BLACK);
        continue;
      }

      // 元画像のピクセル色を取得
      uint32_t rgb565Color = srcSprite.readPixel(x, y);
      uint32_t originalColorValue = srcSprite.color16to24(rgb565Color);
      uint8_t r_src = (originalColorValue >> 16) & 0xFF;
      uint8_t g_src = (originalColorValue >> 8) & 0xFF;
      uint8_t b_src = originalColorValue & 0xFF;

      // 各チャンネルに対してディザリングを適用
      uint8_t r_dst, g_dst, b_dst;
      uint8_t channels_src[3] = { r_src, g_src, b_src };
      uint8_t channels_dst[3];
      float bayerThreshold = (float)bayer4x4[y % 4][x % 4] / bayerDivisor;

      for (int ch = 0; ch < 3; ++ch) {
        uint8_t val_src = channels_src[ch];
        int level_index = floor((float)val_src / step);
        if (level_index >= levelsPerChannel - 1) { level_index = levelsPerChannel - 2; }
        float level_low = (float)level_index * step;
        float error = (float)val_src - level_low;
        float normalized_error = (step > 0) ? (error / step) : 0.0f;
        if (normalized_error < 0.0f) normalized_error = 0.0f;
        if (normalized_error > 1.0f) normalized_error = 1.0f;

        uint8_t val_dst;
        if (normalized_error >= bayerThreshold) {
          val_dst = (uint8_t)round(((float)level_index + 1.0f) * step);
        } else {
          val_dst = (uint8_t)round(level_low);
        }
        channels_dst[ch] = std::max(0, std::min(255, (int)val_dst));
      }

      r_dst = channels_dst[0];
      g_dst = channels_dst[1];
      b_dst = channels_dst[2];

      // 新しいRGB値で出力先スプライトに描画
      uint16_t ditheredColor = dstSprite.color565(r_dst, g_dst, b_dst);
      dstSprite.drawPixel(x, y, ditheredColor);
    }
    // delay(0);
  }
}


void setup() {
  encoder.begin(&Wire, 0x40, 2, 1);
  display.init(2, 1);
  display.setTextScroll(true);
  display.setRotation(3);
  display.setColorDepth(2);
  display.setTextFont(2);
  display.setBrightness(255);
  canvas0.createSprite(240, 176);  // カメラ画像
  canvas1.createSprite(240, 176);  // 変換後画像1
  Serial.begin(115200);
  pinMode(POWER_GPIO_NUM, OUTPUT);
  digitalWrite(POWER_GPIO_NUM, LOW);
  delay(500);

  if (psramFound()) {
    size_t psram_size = esp_spiram_get_size() / 1048576;
    camera_config.pixel_format = PIXFORMAT_RGB565;
    camera_config.fb_location = CAMERA_FB_IN_PSRAM;
    camera_config.fb_count = 2;
  } else {
    Serial.println("PSRAM not found!");
    display.println(" PSRAM not found!");
    delay(500);
  }
  Serial.begin();
  if (!CameraBegin()) {
    Serial.println("Camera initialization failed!");
    display.println(" Camera initialization failed!");
    delay(1000);
    ESP.restart();
  }
  Serial.println("Camera initialized...");
  display.println(" Camera initialized...");

  // 一度SDカードをマウントして確認
  SPI.begin(7, 8, 6, -1);
  if (!SD.begin(15, SPI, 80000000)) {
    error_lamp();
    Serial.println("SD Card initialization failed!");
    display.println(" SD Card initialization failed!");
    delay(500);
    return;
  } else {
    Serial.println("SD Card initialized...");
    display.println(" SD Card initialized...");

    // パレット0から7までループ
    for (int i = 0; i < 8; i++) {
      if (loadPaletteFromSD(i)) {
        Serial.printf("Palette %d loaded from SD.\n", i);
        display.printf(" Palette %d loaded from SD.\n", i);
      } else {
        Serial.printf("Palette %d use default.\n", i);
        display.printf(" Palette %d use default.\n", i);
      }
      delay(100);
    }
  }
  SD.end();  // 一旦ENDしておく

  // ボタン押しながら起動で夜間モード
  if (!encoder.getButtonStatus()) {
    sensor_t* s = esp_camera_sensor_get();
    s->set_brightness(s, 1);  // -3 to 3 あまり大きくするとノイジーに
    s->set_aec2(s, 1);        // ナイトモード？ 効いてるか微妙
    Serial.println("Night Mode");
    display.println(" Night Mode");
  }

  delay(500);
  encoder.setLEDColor(1, 0x000040);  // 青
  Serial.println("System initialized...");
  display.println(" System initialized...");
  delay(500);
  encoder.setLEDColor(1, 0x004000);  // 緑
}


void loop() {
  newEncoderValue = encoder.getEncoderValue();
  bool btn_stauts = encoder.getButtonStatus();

  int diff = newEncoderValue - lastEncoderValue;

  // 値に変化があった場合のみ処理
  if (diff != 0) {
    keyOnTime = millis();
    accumulatedChange += diff;           // 変化量を累積
    lastEncoderValue = newEncoderValue;  // 今回の値を次回のために保存

    // 累積変化量が+2以上になったかチェック
    while (accumulatedChange >= 2) {
      mode++;  // モードを+1
      if (mode > modeMax) {
        mode = modeMax;  //maxで止まる
      }
      accumulatedChange -= 2;  // 累積変化量から2を引く
    }
    // 累積変化量が-2以下になったかチェック
    while (accumulatedChange <= -2) {
      mode--;  // モードを-1
      if (mode < -1) {
        mode = -1;  //minで止まる
        if (dither == 0) {
          dither = 1;
          display.drawString("Dither:ON", 170, 120);
        } else {
          dither = 0;
          display.drawString("Dither:OFF", 170, 120);
        }
      }
      accumulatedChange += 2;  // 累積変化量に2を足す
    }
    if (mode == -1) {
      sprintf(currentmode, "MODE:ALL");
    } else {
      sprintf(currentmode, "MODE:%01d", mode);
      currentPalettelndex = mode;
    }
    display.drawString(currentmode, 10, 120);
  }

  if (!btn_stauts) {
    keyOnTime = millis();              // 最後にボタン操作した時間
    encoder.setLEDColor(0, 0x000000);  // 一度消灯
    encoder.setLEDColor(2, 0x284000);  // オレンジ

    CameraGet();  // 撮影

    SD.end();  // 念のため一旦END
    delay(100);
    SD.begin(15, SPI, 80000000);  // 保存失敗するときは速度を下げる

    saveToSD_DisplayBMP();  // ディスプレイの画像保存
    sprintf(filesaveprogress, "%04d_Display.bmp", filecounter);
    display.drawCenterString(filesaveprogress, 120, 60);

    saveToSD_OriginalBMP();  // 変換前の画像保存
    sprintf(filesaveprogress, "%04d_Original.bmp", filecounter);
    display.drawCenterString(filesaveprogress, 120, 60);

    if (dither == 0) {
      saveGraylevel_fb();  // 輝度情報の保存
    } else {
      canvas0.pushImage(0, 0, 240, 176, (uint16_t*)fb->buf);  // (x, y, w, h, *data)
      applyColorBayerDither4x4(canvas0, canvas1, levels);
      saveGraylevel_canvas(canvas1);
    }

    if (mode == -1) {
      // 全パレット分変換
      for (int i = 0; i < maxPalettelndex; i++) {
        currentPalettelndex = i;
        sprintf(filesaveprogress, "%04d_Palette%01d.bmp", filecounter, currentPalettelndex);
        display.drawCenterString(filesaveprogress, 120, 60);
        saveToSD_ConvertBMP();  // 変換後の画像保存
      }
    } else {
      // モード指定したパレットだけ変換
      sprintf(filesaveprogress, "%04d_Palette%01d.bmp", filecounter, currentPalettelndex);
      display.drawCenterString(filesaveprogress, 120, 60);
      saveToSD_ConvertBMP();  // 変換後の画像保存
    }

    CameraFree();   // フレームバッファを解放
    filecounter++;  // 連番を更新
    SD.end();
    encoder.setLEDColor(0, 0x000000);  // 一度消灯
    encoder.setLEDColor(1, 0x004000);  // 緑
  }

  if ((millis() - keyOnTime >= LED_ON_DURATION)) {
    encoder.setLEDColor(0, 0x000000);  // LED消灯
    display.fillScreen(TFT_BLACK);     // ディスプレイ消す
  } else {
    display.setBrightness(160);  // カメラからフレームを取得して表示
    if (CameraGet()) {
      canvas0.pushImage(0, 0, 240, 176, (uint16_t*)fb->buf);  // (x, y, w, h, *data)
      CameraFree();                                           // 取得したフレームを解放
    }
    if (mode == -1) {
      if (dither == 0) {
        canvas0.pushSprite(&display, 0, -16);
      } else {
        applyColorBayerDither4x4(canvas0, canvas1, levels);
        canvas1.pushSprite(&display, 0, -16);
      }
    } else {
      if (dither == 0) {
        convertColor_canvas(canvas0, canvas1);
      } else {
        applyColorBayerDither4x4(canvas0, canvas1, levels);
        convertColor_canvas(canvas1, canvas1);
      }
      canvas1.pushSprite(&display, 0, -16);
    }
  }
  delay(2);
}