// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: Copyright (c) 2026 Ashwin Whitchurch, Protocentral Electronics

//////////////////////////////////////////////////////////////////////////////////////////
//
//  Arduino library for the ProtoCentral FDC2214 capacitive sensor breakout board
//
//  Copyright (c) 2026 Ashwin Whitchurch, Protocentral Electronics
//  Email: info@protocentral.com
//
//  This software is licensed under the MIT License (http://opensource.org/licenses/MIT).
//
/////////////////////////////////////////////////////////////////////////////////////////

#include "Protocentral_FDC2214.h"

FDC2214::FDC2214(uint8_t address, TwoWire* wire)
    : _addr(address),
      _wire(wire),
      _f_ref_hz(FDC2214_INTERNAL_OSC_HZ),
      _config(0),
      _mux_config(0)
{
    for (uint8_t i = 0; i < 4; i++) {
        _clock_div[i] = FDC2214_FIN_SEL_HIGH_FREQ | 0x0001;
    }
}

// ============================================================================
// Lifecycle
// ============================================================================

bool FDC2214::begin()
{
    if (!isConnected()) return false;
    reset();
    delay(2);
    return isConnected();
}

bool FDC2214::isConnected()
{
    return (getManufacturerID() == FDC2214_MANUFACTURER_ID) &&
           (getDeviceID()       == FDC2214_DEVICE_ID);
}

void FDC2214::reset()
{
    // RESET_DEV bit 15 = soft reset. The bit auto-clears.
    writeRegister(FDC2214_REG_RESET_DEV, 0x8000);
}

uint16_t FDC2214::getDeviceID()
{
    return readRegister(FDC2214_REG_DEVICE_ID);
}

uint16_t FDC2214::getManufacturerID()
{
    return readRegister(FDC2214_REG_MANUFACTURER_ID);
}

// ============================================================================
// Channel configuration
// ============================================================================

void FDC2214::setReferenceCount(fdc2214_channel_t ch, uint16_t rcount)
{
    writeRegister(FDC2214_REG_RCOUNT_CH0 + ch, rcount);
}

void FDC2214::setSettleCount(fdc2214_channel_t ch, uint16_t scount)
{
    writeRegister(FDC2214_REG_SETTLECOUNT_CH0 + ch, scount);
}

void FDC2214::setOffset(fdc2214_channel_t ch, uint16_t offset)
{
    writeRegister(FDC2214_REG_OFFSET_CH0 + ch, offset);
}

void FDC2214::setClockDividers(fdc2214_channel_t ch, uint16_t fin_sel, uint16_t fref_div)
{
    // CLOCK_DIVIDERS layout:
    //   bits [13:12] = CHx_FIN_SEL (1 = sensor < 8.75 MHz, 2 = high-freq path /2)
    //   bits [9:0]   = CHx_FREF_DIVIDER (must be >= 1)
    if (fref_div == 0) fref_div = 1;
    uint16_t v = (fin_sel & 0x3000) | (fref_div & 0x03FF);
    _clock_div[ch] = v;
    writeRegister(FDC2214_REG_CLOCK_DIV_CH0 + ch, v);
}

void FDC2214::setDriveCurrent(fdc2214_channel_t ch, uint8_t idrive_5bit)
{
    // DRIVE_CURRENT_CHx layout:
    //   bits [15:11] = IDRIVE (0..31)
    //   bits [10:6]  = INIT_IDRIVE (auto-calibrated, leave 0 for user-set)
    uint16_t v = ((uint16_t)(idrive_5bit & 0x1F)) << 11;
    writeRegister(FDC2214_REG_DRIVE_CURRENT_CH0 + ch, v);
}

// ============================================================================
// Device-wide configuration
// ============================================================================

void FDC2214::setActiveChannel(fdc2214_channel_t ch)
{
    _config &= ~0xC000;
    _config |= ((uint16_t)ch) << 14;
    writeConfig();
}

void FDC2214::setAutoscan(bool enable, uint16_t rr_sequence)
{
    _mux_config &= ~(0x8000 | 0x6000);
    if (enable) {
        _mux_config |= FDC2214_AUTOSCAN_ENABLED;
        _mux_config |= (rr_sequence & 0x6000);
    }
    writeMuxConfig();
}

void FDC2214::setDeglitch(uint16_t deglitch_field)
{
    _mux_config &= ~0x0007;
    _mux_config |= (deglitch_field & 0x0007);
    writeMuxConfig();
}

void FDC2214::setReferenceClockSource(bool external)
{
    if (external) _config |= FDC2214_CONFIG_REF_CLK_EXT;
    else          _config &= ~FDC2214_CONFIG_REF_CLK_EXT;
    writeConfig();
}

void FDC2214::setHighCurrentDrive(bool enable)
{
    if (enable) _config |= FDC2214_CONFIG_HIGH_CURRENT;
    else        _config &= ~FDC2214_CONFIG_HIGH_CURRENT;
    writeConfig();
}

void FDC2214::setSleep(bool sleep)
{
    if (sleep) _config |= FDC2214_CONFIG_SLEEP_EN;
    else       _config &= ~FDC2214_CONFIG_SLEEP_EN;
    writeConfig();
}

// ============================================================================
// One-shot sensible defaults
// ============================================================================

bool FDC2214::configureDefaults()
{
    // Per-channel: maximum reference count for best resolution, generous settle
    // count, high-frequency input path (sensor up to 10 MHz), reference divider 1,
    // mid-range drive current.
    for (uint8_t i = 0; i < 4; i++) {
        fdc2214_channel_t ch = (fdc2214_channel_t)i;
        setReferenceCount(ch, 0xFFFF);
        setSettleCount(ch, 0x000A);
        setOffset(ch, 0x0000);
        setClockDividers(ch, FDC2214_FIN_SEL_HIGH_FREQ, 0x0001);
        setDriveCurrent(ch, 0x0F);   // ~0.146 mA, safe default for typical LC tanks
    }

    // Disable per-channel error reporting on data outputs (kept clean by default).
    writeRegister(FDC2214_REG_ERROR_CONFIG, 0x0000);

    // MUX_CONFIG: autoscan disabled by default, single CH0, 10 MHz deglitch.
    _mux_config = FDC2214_AUTOSCAN_DISABLED | FDC2214_MUX_RESERVED | FDC2214_DEGLITCH_10MHZ;
    writeMuxConfig();

    // CONFIG: hold device in sleep until start() is called. Internal oscillator,
    // INTB enabled, full-current sensor activation, low-current drive.
    _config = FDC2214_CONFIG_RESERVED | FDC2214_CONFIG_ACTIVE_CH0 | FDC2214_CONFIG_SLEEP_EN;
    writeConfig();

    return true;
}

void FDC2214::start()
{
    _config &= ~FDC2214_CONFIG_SLEEP_EN;
    writeConfig();
}

void FDC2214::stop()
{
    _config |= FDC2214_CONFIG_SLEEP_EN;
    writeConfig();
}

// ============================================================================
// Data access
// ============================================================================

bool FDC2214::isDataReady(fdc2214_channel_t ch)
{
    uint16_t status = readStatus();
    uint16_t mask;
    switch (ch) {
        case FDC2214_CH0: mask = FDC2214_STATUS_CH0_UNREAD; break;
        case FDC2214_CH1: mask = FDC2214_STATUS_CH1_UNREAD; break;
        case FDC2214_CH2: mask = FDC2214_STATUS_CH2_UNREAD; break;
        case FDC2214_CH3: mask = FDC2214_STATUS_CH3_UNREAD; break;
        default: return false;
    }
    return (status & mask) != 0;
}

uint32_t FDC2214::readRawChannel(fdc2214_channel_t ch)
{
    uint16_t msb = readRegister(FDC2214_REG_DATA_MSB_CH0 + (ch * 2));
    uint16_t lsb = readRegister(FDC2214_REG_DATA_LSB_CH0 + (ch * 2));

    // Top 4 bits are error flags; data is 28 bits.
    uint32_t raw = ((uint32_t)(msb & 0x0FFF) << 16) | lsb;
    return raw & FDC2214_DATA_MASK_28BIT;
}

float FDC2214::readFrequencyHz(fdc2214_channel_t ch)
{
    uint32_t raw = readRawChannel(ch);
    if (raw == 0) return 0.0f;

    uint16_t cdiv     = _clock_div[ch];
    uint16_t fref_div = cdiv & 0x03FF;
    if (fref_div == 0) fref_div = 1;

    // CH_FIN_DIVIDER is 1 for low-freq sensor mode, 2 for high-freq mode.
    float ch_fin_divider = ((cdiv & 0x3000) == FDC2214_FIN_SEL_HIGH_FREQ) ? 2.0f : 1.0f;
    float f_ref          = _f_ref_hz / (float)fref_div;

    // f_sensor = (DATA / 2^28) * f_REF * CH_FIN_DIVIDER
    return ((float)raw / 268435456.0f) * f_ref * ch_fin_divider;
}

float FDC2214::readCapacitancePf(fdc2214_channel_t ch, float inductance_uH)
{
    float f = readFrequencyHz(ch);
    if (f <= 0.0f || inductance_uH <= 0.0f) return 0.0f;

    // C = 1 / (L * (2*pi*f)^2). Result in farads.
    float L = inductance_uH * 1.0e-6f;
    float w = 2.0f * (float)PI * f;
    float C = 1.0f / (L * w * w);

    return C * 1.0e12f; // farads -> picofarads
}

// ============================================================================
// Status / raw register access
// ============================================================================

uint16_t FDC2214::readStatus()
{
    return readRegister(FDC2214_REG_STATUS);
}

uint16_t FDC2214::readErrorConfig()
{
    return readRegister(FDC2214_REG_ERROR_CONFIG);
}

uint16_t FDC2214::readRegister(uint8_t reg)
{
    _wire->beginTransmission(_addr);
    _wire->write(reg);
    if (_wire->endTransmission(false) != 0) {
        _wire->endTransmission();
        return 0;
    }

    if (_wire->requestFrom(_addr, (uint8_t)2) != 2) return 0;
    uint16_t hi = _wire->read();
    uint16_t lo = _wire->read();
    return (hi << 8) | lo;
}

void FDC2214::writeRegister(uint8_t reg, uint16_t value)
{
    _wire->beginTransmission(_addr);
    _wire->write(reg);
    _wire->write((uint8_t)(value >> 8));
    _wire->write((uint8_t)(value & 0xFF));
    _wire->endTransmission();
}

// ============================================================================
// Reference clock
// ============================================================================

void FDC2214::setReferenceClockHz(float hz)
{
    if (hz > 0.0f) _f_ref_hz = hz;
}

// ============================================================================
// Private
// ============================================================================

void FDC2214::writeConfig()
{
    writeRegister(FDC2214_REG_CONFIG, _config);
}

void FDC2214::writeMuxConfig()
{
    writeRegister(FDC2214_REG_MUX_CONFIG, _mux_config);
}
