#include "temperature.h"

#include <OneWireNg_CurrentPlatform.h>
#include <drivers/DSTherm.h>
#include <HardwareSerial.h>

constexpr uint8_t TEMPERATURE_RESOLUTION = 12;
constexpr int8_t TEMPERATURE_ALARM_LIMIT_LOW = -40;
constexpr int8_t TEMPERATURE_ALARM_LIMIT_HIGH = 80;

#define INVALID_SENSOR_INDEX 255

constexpr uint8_t TEMPERATURE_SENSOR_ERROR_THRESHOLD = 3;

constexpr bool TEMPERATURE_SENSOR_PARASITE_POWER = false;

// Indicator if the temperature sensors have been configured
RTC_DATA_ATTR bool didConfigureTemperatureSensors = false;

OneWireNg_CurrentPlatform ow(TEMPERATURE_SENSOR_PIN, TEMPERATURE_SENSOR_PARASITE_POWER);
DSTherm sensorInterface = DSTherm(ow);
ALLOC_ALIGNED uint8_t scratchpadBuffer[sizeof(DSTherm::Scratchpad)];

struct Sensor {
    
    uint8_t address[TEMPERATURE_SENSOR_ADDRESS_SIZE];

    bool isSet;

    bool foundDuringCurrentUpdate;
};

RTC_DATA_ATTR Sensor sensors[TEMPERATURE_SENSOR_MAX_COUNT];
RTC_DATA_ATTR uint8_t availableSensorCount = 0;

bool hasSensorAtIndex(uint8_t sensorIndex) {
    return sensors[sensorIndex].isSet;
}

bool hasIdAtIndex(uint8_t sensorIndex, const OneWireNg::Id& id) {
    if (!hasSensorAtIndex(sensorIndex)) {
        return false;
    }
    for (uint8_t pos = 0; pos < TEMPERATURE_SENSOR_ADDRESS_SIZE; pos += 1) {
        if (id[pos] != sensors[sensorIndex].address[pos]) {
            return false;
        }
    }
    return true;
}

uint8_t getSensorIndex(const OneWireNg::Id& id) {
    for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
        if (hasIdAtIndex(sensorIndex, id)) {
            return sensorIndex;
        }
    }
    return INVALID_SENSOR_INDEX;
}

uint8_t getIndexOfFirstEmptySensorSlot() {
    for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
        if (!sensors[sensorIndex].isSet) {
            return sensorIndex;
        }
    }
    return INVALID_SENSOR_INDEX;
}

uint8_t addNewSensor(const OneWireNg::Id& id) {
    if (availableSensorCount == TEMPERATURE_SENSOR_MAX_COUNT) {
        return INVALID_SENSOR_INDEX;
    }
    uint8_t sensorIndex = getIndexOfFirstEmptySensorSlot();
    if (sensorIndex == INVALID_SENSOR_INDEX) {
        return INVALID_SENSOR_INDEX;
    }
    for (uint8_t pos = 0; pos < TEMPERATURE_SENSOR_ADDRESS_SIZE; pos += 1) {
        sensors[sensorIndex].address[pos] = id[pos];
    }
    sensors[sensorIndex].isSet = true;
    sensors[sensorIndex].foundDuringCurrentUpdate = true;
    availableSensorCount += 1;
    return sensorIndex;
}

void removeSensorAtIndex(uint8_t sensorIndex) {
    sensors[sensorIndex].isSet = false;
    availableSensorCount -= 1;
}

uint8_t* getSensorAddress(uint8_t index) {
    return sensors[index].address;
}

void copySensorAddress(uint8_t index, uint8_t* buffer) {
    memcpy(buffer, sensors[index].address, TEMPERATURE_SENSOR_ADDRESS_SIZE);
}

void printSensorAddress(const OneWireNg::Id& id) {
    for (uint8_t i = 0; i < TEMPERATURE_SENSOR_ADDRESS_SIZE; i += 1) {
        Serial.print(" 0x");
        Serial.print(id[i], HEX);
    }
    Serial.println();
}

void printSensorAddressAtIndex(uint8_t sensorIndex) {
    for (uint8_t i = 0; i < TEMPERATURE_SENSOR_ADDRESS_SIZE; i += 1) {
        Serial.print(" 0x");
        Serial.print(sensors[sensorIndex].address[i], HEX);
    }
    Serial.println();
}

void clearFoundIndicatorForAllSensors() {
    for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
        sensors[sensorIndex].foundDuringCurrentUpdate = false;
    }
}

DSTherm::Scratchpad* getScratchpad() {
    return reinterpret_cast<DSTherm::Scratchpad*>(&scratchpadBuffer[0]);
}

void temperatureConfigure() {
    if (didConfigureTemperatureSensors) {
        // Only determine and configure sensors once per power cycle
        return;
    }

    DSTherm::Scratchpad* scratchpad = getScratchpad();

    // Clear discovery flags
    clearFoundIndicatorForAllSensors();

    // Read sensors and determine unknown ones
    for (const auto& id: (OneWireNg&) ow) {
        uint8_t sensorIndex = getSensorIndex(id);
        if (sensorIndex == INVALID_SENSOR_INDEX) {
            sensorIndex = addNewSensor(id);
            Serial.print("Sensor ");
            Serial.print(sensorIndex);
            Serial.print(": ");
            printSensorAddress(id);
            continue;
        }
        // Mark existing sensor
        sensors[sensorIndex].foundDuringCurrentUpdate = true;
        Serial.print("Discovered sensor ");
        printSensorAddress(id);
        Serial.print(" at index ");
        Serial.println(sensorIndex);
    }

    // Remove missing sensors
    for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
        if (!hasSensorAtIndex(sensorIndex)) {
            continue;
        }
        if (sensors[sensorIndex].foundDuringCurrentUpdate) {
            continue;
        }
        Serial.print("Removed sensor ");
        printSensorAddressAtIndex(sensorIndex);
        Serial.println(sensorIndex);
        removeSensorAtIndex(sensorIndex);
    }
    
    if (availableSensorCount == 0) {
        Serial.println("No sensors available");
        return;
    }

    // Write sensor resolution and set limits
    if (sensorInterface.writeScratchpadAll(
        TEMPERATURE_ALARM_LIMIT_HIGH, 
        TEMPERATURE_ALARM_LIMIT_LOW, 
        TEMPERATURE_RESOLUTION) != OneWireNg::ErrorCode::EC_SUCCESS) {
        Serial.println("Failed to set temperature limits and resolution");
        return;
    }

    // Do an update once, because otherwise the temperature will be 85° for some reason
    Temperature samples[TEMPERATURE_SENSOR_MAX_COUNT];
    delay(500); // Delay seems to prevent missing sensors on first measurement
    temperaturePerformUpdate(samples);
    didConfigureTemperatureSensors = true;
}

void temperaturePerformUpdate(Temperature* temperatures) {
    clearFoundIndicatorForAllSensors();

    // Convert temperature on all sensors connected
    sensorInterface.convertTempAll(DSTherm::SCAN_BUS, TEMPERATURE_SENSOR_PARASITE_POWER);
    DSTherm::Scratchpad* scratchpad = getScratchpad();

    // Read sensors one by one
    for (const auto& id: (OneWireNg&) ow) {
        uint8_t sensorIndex = getSensorIndex(id);
        if (sensorIndex == INVALID_SENSOR_INDEX) {
            Serial.println("Invalid sensor");
            // Invalid sensor id
            continue;
        }
        sensors[sensorIndex].foundDuringCurrentUpdate = true;
        if (sensorInterface.readScratchpad(id, scratchpad) != OneWireNg::EC_SUCCESS) {
            // Invalid CRC
            temperatures[sensorIndex].status = TemperatureStatus::sensorError;
            Serial.println("Invalid CRC");
            continue;
        }
        temperatures[sensorIndex].value = scratchpad->getTemp(); // In millidegrees Celcius
        temperatures[sensorIndex].status = TemperatureStatus::temperatureIsValid;
        Serial.print(temperatures[sensorIndex].value);
        Serial.println("°C");
    }

    // Update missing sensors
    for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
        if (!sensors[sensorIndex].foundDuringCurrentUpdate) {
            temperatures[sensorIndex].status = TemperatureStatus::sensorNotFound;
        }
    }
}
