#include "sw/device/lib/camera_hm01b0.h"

#include "hw/top_matcha/sw/autogen/top_matcha.h"
#include "sw/device/lib/camera_hm01b0_regs.h"
#include "sw/device/lib/dif/dif_base.h"
#include "sw/device/lib/dif/dif_i2c.h"
#include "sw/device/lib/dif/dif_rv_plic.h"
#include "sw/device/lib/testing/test_framework/check.h"

// Camera address and registers
#define HM01B0_ADDR (0x24 << 1)

// Expected values of Chip ID registers
#define HM01B0_MODEL_ID_H_EXPECTED (0x01)
#define HM01B0_MODEL_ID_L_EXPECTED (0xB0)

// I2C R/W bits
static const uint8_t kI2cWrite = 0;
static const uint8_t kI2cRead = 1;

// IRQ statuses
static volatile bool fmt_irq_seen = false;
static volatile bool rx_irq_seen = false;
static volatile bool done_irq_seen = false;

// Device handles
static dif_i2c_t cam_i2c;
static dif_rv_plic_t plic;

static dif_result_t camera_hm01b0_write_reg(uint16_t reg, uint8_t val) {
  CHECK(!done_irq_seen);
  // Cam address + start
  CHECK_DIF_OK(dif_i2c_write_byte(&cam_i2c, HM01B0_ADDR | kI2cWrite,
                                  kDifI2cFmtStart, true));
  // addr hi
  CHECK_DIF_OK(dif_i2c_write_byte(&cam_i2c, (reg >> 8), kDifI2cFmtTx, true));
  // addr lo
  CHECK_DIF_OK(dif_i2c_write_byte(&cam_i2c, (reg & 0xFF), kDifI2cFmtTx, true));
  // val + stop
  CHECK_DIF_OK(dif_i2c_write_byte(&cam_i2c, val, kDifI2cFmtTxStop, true));

  while (!done_irq_seen) {
    asm volatile("wfi");
  }
  CHECK(done_irq_seen);
  done_irq_seen = false;

  return kDifOk;
}

static dif_result_t camera_hm01b0_read_reg(uint16_t reg, uint8_t* val_out) {
  if (!val_out) return kDifBadArg;

  CHECK(!done_irq_seen);
  // Cam address + start
  CHECK_DIF_OK(dif_i2c_write_byte(&cam_i2c, HM01B0_ADDR | kI2cWrite,
                                  kDifI2cFmtStart, true));
  // addr hi
  CHECK_DIF_OK(dif_i2c_write_byte(&cam_i2c, (reg >> 8), kDifI2cFmtTx, true));
  // addr lo + stop
  CHECK_DIF_OK(
      dif_i2c_write_byte(&cam_i2c, (reg & 0xFF), kDifI2cFmtTxStop, true));

  while (!done_irq_seen) {
    asm volatile("wfi");
  }
  CHECK(done_irq_seen);
  done_irq_seen = false;

  // start + id
  CHECK_DIF_OK(dif_i2c_write_byte(&cam_i2c, HM01B0_ADDR | kI2cRead,
                                  kDifI2cFmtStart, true));
  // rx
  CHECK_DIF_OK(dif_i2c_write_byte(&cam_i2c, 1, kDifI2cFmtRxStop, false));
  while (!done_irq_seen) {
    asm volatile("wfi");
  }
  CHECK(done_irq_seen);
  done_irq_seen = false;

  CHECK_DIF_OK(dif_i2c_read_byte(&cam_i2c, val_out));

  return kDifOk;
}

dif_result_t camera_hm01b0_init(void) {
  CHECK_DIF_OK(dif_i2c_init(mmio_region_from_addr(TOP_MATCHA_CAM_I2C_BASE_ADDR),
                            &cam_i2c));
  dif_i2c_config_t cam_i2c_config;
  dif_i2c_timing_config_t cam_i2c_timing_config = {
      .lowest_target_device_speed = kDifI2cSpeedStandard,
      .clock_period_nanos = 400,
      .sda_rise_nanos = 800,
      .sda_fall_nanos = 600,
      .scl_period_nanos = 1000000 / 100000,
  };
  CHECK_DIF_OK(dif_i2c_compute_timing(cam_i2c_timing_config, &cam_i2c_config));
  CHECK_DIF_OK(dif_i2c_configure(&cam_i2c, cam_i2c_config));
  CHECK_DIF_OK(
      dif_i2c_set_watermarks(&cam_i2c, kDifI2cLevel1Byte, kDifI2cLevel1Byte));
  CHECK_DIF_OK(dif_i2c_irq_set_enabled(&cam_i2c, kDifI2cIrqFmtThreshold,
                                       kDifToggleEnabled));
  CHECK_DIF_OK(dif_i2c_irq_set_enabled(&cam_i2c, kDifI2cIrqRxThreshold,
                                       kDifToggleEnabled));
  CHECK_DIF_OK(dif_i2c_irq_set_enabled(&cam_i2c, kDifI2cIrqCmdComplete,
                                       kDifToggleEnabled));
  CHECK_DIF_OK(dif_i2c_host_set_enabled(&cam_i2c, kDifToggleEnabled));

  uint8_t id_h, id_l;
  CHECK_DIF_OK(camera_hm01b0_read_reg(HM01B0_MODEL_ID_H, &id_h));
  CHECK_DIF_OK(camera_hm01b0_read_reg(HM01B0_MODEL_ID_L, &id_l));
  CHECK(id_h == HM01B0_MODEL_ID_H_EXPECTED);
  CHECK(id_l == HM01B0_MODEL_ID_L_EXPECTED);
  return kDifOk;
}

dif_result_t camera_hm01b0_set_default_registers(
    camera_hm01b0_resolution_t resolution,
    camera_hm01b0_frame_rate_t frame_rate) {
  // These settings are derived from
  // Himax's HM01B0_RAW8_QVGA_8bits_lsb_1fps.txt.
  // Any registers without a name are labeled as RESERVED in the
  // datasheet.

  uint8_t qvga_win_en;
  if (resolution == kCameraHm01b0Resolution324x324) {
    qvga_win_en = 0;
  } else if (resolution == kCameraHm01b0Resolution320x240) {
    qvga_win_en = 1;
  } else {
    return kDifBadArg;
  }

  uint8_t intg_h, intg_l, frame_length_h, frame_length_l;
  switch (frame_rate) {
    case kCameraHm01b0FrameRate1Fps:
      intg_h = 0x3E;
      intg_l = 0x58;
      frame_length_h = 0x3E;
      frame_length_l = 0x5A;
      break;
    case kCameraHm01b0FrameRate60Fps:
      intg_h = 0x01;
      intg_l = 0x0A;
      frame_length_h = 0x01;
      frame_length_l = 0x0C;
      break;
    default:
      return kDifBadArg;
  }

  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_SW_RESET, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MODE_SELECT, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_IMAGE_ORIENTATION, 0x03));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_BLC_TGT, 0x08));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_BLC2_TGT, 0x08));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3044, 0x0A));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3045, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3047, 0x0A));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3050, 0xC0));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3051, 0x42));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3052, 0x50));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3053, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3054, 0x03));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3055, 0xF7));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3056, 0xF8));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3057, 0x29));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x3058, 0x1F));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_BIT_CONTROL, 0x1E));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_SYNC_EN, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_OUTPUT_PIN_STATUS_CTRL, 0x04));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_BLC_CFG, 0x43));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x1001, 0x40));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x1002, 0x32));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x0350, 0x7F));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_BLC2_EN, 0x01));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x1008, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x1009, 0xA0));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x100A, 0x60));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x100B, 0x90));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x100C, 0x40));
  CHECK_DIF_OK(
      camera_hm01b0_write_reg(HM01B0_VSYNC_HSYNC_PIXEL_SHIFT_EN, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_STATISTIC_CTRL, 0x07));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x2003, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x2004, 0x1C));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x2007, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x2008, 0x58));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x200B, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x200C, 0x7A));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x200F, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(0x2010, 0xB8));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MD_LROI_Y_START_H, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MD_LROI_Y_START_L, 0x58));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MD_LROI_X_END_H, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MD_LROI_X_END_L, 0x9B));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_AE_CTRL, 0x01));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_CONVERGE_OUT_TH, 0x07));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MAX_INTG_H, intg_h));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MAX_INTG_L, intg_l));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MAX_AGAIN_FULL, 0x03));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MAX_AGAIN_BIN2, 0x03));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MAX_DGAIN, 0x80));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_FS_60HZ_H, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_FS_60HZ_L, 0x85));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_FS_50HZ_H, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_FS_50HZ_L, 0xA0));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MD_CTRL, 0x03));
  CHECK_DIF_OK(
      camera_hm01b0_write_reg(HM01B0_FRAME_LENGTH_LINES_H, frame_length_h));
  CHECK_DIF_OK(
      camera_hm01b0_write_reg(HM01B0_FRAME_LENGTH_LINES_L, frame_length_l));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_LINE_LENGTH_PCK_H, 0x01));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_LINE_LENGTH_PCK_L, 0x78));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_QVGA_WIN_EN, qvga_win_en));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_READOUT_X, 0x01));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_READOUT_Y, 0x01));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_BINNING_MODE, 0x00));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_SIX_BIT_MODE_EN, 0x70));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_BIT_CONTROL, 0x02));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_OSC_CLK_DIV, 0x01));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_GRP_PARAM_HOLD, 0x01));
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MODE_SELECT, 0x05));
  return kDifOk;
}

dif_result_t camera_hm01b0_set_test_pattern(
    camera_hm01b0_test_pattern_t pattern) {
  if (pattern == kCameraHm01b0TestPatternNone) {
    camera_hm01b0_write_reg(HM01B0_AE_CTRL, 0x01);
    camera_hm01b0_write_reg(HM01B0_BLC_CFG, 0x43);
    camera_hm01b0_write_reg(HM01B0_TEST_PATTERN_MODE, 0x00);
  } else if (pattern == kCameraHm01b0TestPatternWalkingOnes ||
             pattern == kCameraHm01b0TestPatternColorBar) {
    camera_hm01b0_write_reg(HM01B0_AE_CTRL, 0x00);
    camera_hm01b0_write_reg(HM01B0_BLC_CFG, 0x00);
    camera_hm01b0_write_reg(HM01B0_TEST_PATTERN_MODE, pattern);
  } else {
    return kDifBadArg;
  }
  return kDifOk;
}

dif_result_t camera_hm01b0_set_streaming_mode(
    camera_hm01b0_streaming_mode_t mode) {
  CHECK_DIF_OK(camera_hm01b0_write_reg(HM01B0_MODE_SELECT, mode));
  return kDifOk;
}

dif_result_t camera_hm01b0_set_frame_count(uint16_t count) {
  CHECK_DIF_OK(
      camera_hm01b0_write_reg(HM01B0_PMU_PROGRAMMABLE_FRAMECNT, count));
  return kDifOk;
}

dif_result_t camera_hm01b0_irq_init(const dif_rv_plic_t* plic,
                                    const top_matcha_plic_target_smc_t target) {
  CHECK_DIF_OK(dif_rv_plic_irq_set_enabled(
      plic, kTopMatchaPlicIrqIdCamI2cFmtThreshold, kTopMatchaPlicTargetIbex0Smc,
      kDifToggleEnabled));
  CHECK_DIF_OK(dif_rv_plic_irq_set_enabled(
      plic, kTopMatchaPlicIrqIdCamI2cRxThreshold, kTopMatchaPlicTargetIbex0Smc,
      kDifToggleEnabled));
  CHECK_DIF_OK(dif_rv_plic_irq_set_enabled(
      plic, kTopMatchaPlicIrqIdCamI2cCmdComplete, kTopMatchaPlicTargetIbex0Smc,
      kDifToggleEnabled));
  CHECK_DIF_OK(dif_rv_plic_irq_set_priority(
      plic, kTopMatchaPlicIrqIdCamI2cFmtThreshold, kDifRvPlicMaxPriority));
  CHECK_DIF_OK(dif_rv_plic_irq_set_priority(
      plic, kTopMatchaPlicIrqIdCamI2cRxThreshold, kDifRvPlicMaxPriority));
  CHECK_DIF_OK(dif_rv_plic_irq_set_priority(
      plic, kTopMatchaPlicIrqIdCamI2cCmdComplete, kDifRvPlicMaxPriority));
  return kDifOk;
}

dif_result_t camera_hm01b0_irq_handler(dif_rv_plic_irq_id_t interrupt_id) {
  dif_i2c_irq_t i2c_irq =
      (dif_i2c_irq_t)(interrupt_id - kTopMatchaPlicIrqIdCamI2cFmtThreshold);
  CHECK_DIF_OK(dif_i2c_irq_acknowledge(&cam_i2c, i2c_irq));
  switch (i2c_irq) {
    case kDifI2cIrqFmtThreshold:
      fmt_irq_seen = true;
      break;
    case kDifI2cIrqRxThreshold:
      rx_irq_seen = true;
      break;
    case kDifI2cIrqCmdComplete:
      done_irq_seen = true;
      break;
    default:
      LOG_ERROR("Unexpected interrupt (at I2C): %d\r\n", i2c_irq);
      return kDifError;
  }
  return kDifOk;
}

static uint8_t walking_ones_next(uint8_t in) {
  uint8_t out;
  if (in == 0) {
    out = 1;
  } else {
    out = in << 1;
  }
  return out;
}

dif_result_t camera_hm01b0_validate_test_pattern(
    const uint8_t* addr, camera_hm01b0_test_pattern_t pattern,
    camera_hm01b0_resolution_t resolution) {
  if (pattern != kCameraHm01b0TestPatternWalkingOnes) {
    return kDifBadArg;
  }

  if (resolution == kCameraHm01b0Resolution324x324) {
    size_t size = 324 * 324;
    uint8_t expected = 0;
    for (size_t i = 0; i < size; ++i) {
      if (addr[i] != expected) {
        LOG_ERROR("Mismatch at %d: 0x%x != 0x%x", i, addr[i], expected);
        return kDifError;
      }
      expected = walking_ones_next(expected);
    }
    return kDifOk;
  } else if (resolution == kCameraHm01b0Resolution320x240) {
    size_t width = 320, height = 240;
    uint8_t expected = 0;
    for (size_t y = 0; y < height; ++y) {
      // In this mode, we start 2 pixels in.
      expected = walking_ones_next(expected);
      expected = walking_ones_next(expected);
      for (size_t x = 0; x < width; ++x) {
        size_t offset = (y * 320) + x;
        if (addr[offset] != expected) {
          LOG_ERROR("Mismatch at %d: 0x%x != 0x%x", offset, addr[offset], expected);
          return kDifError;
        }
        expected = walking_ones_next(expected);
      }
      // We also lose the last two pixels.
      expected = walking_ones_next(expected);
      expected = walking_ones_next(expected);
    }
    return kDifOk;
  } else {
    return kDifBadArg;
  }
}