#include "LinkerHandApi.h"
#include "ErrorCode.h"

#include <algorithm>
#include <atomic>
#include <chrono>
#include <cstdint>
#include <exception>
#include <iomanip>
#include <iostream>
#include <mutex>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

namespace {

using ForceData = std::vector<std::vector<std::vector<uint8_t>>>;
using Clock = std::chrono::steady_clock;

constexpr std::chrono::milliseconds kPollInterval(100);
constexpr std::chrono::milliseconds kPrintInterval(200);
constexpr std::chrono::milliseconds kStaleAfter(600);

struct O6Snapshot {
    uint64_t sequence = 0;
    bool ready = false;
    Clock::time_point updated_at{};
    std::vector<uint8_t> state;
    std::vector<uint8_t> torque;
    std::vector<uint8_t> temperature;
    std::vector<uint8_t> fault_code;
    ForceData force;
    std::string last_error;
};

std::string bytesToString(const std::vector<uint8_t> &values) {
    if (values.empty()) {
        return "[]";
    }

    std::ostringstream oss;
    oss << "[";
    for (size_t i = 0; i < values.size(); ++i) {
        if (i != 0) {
            oss << ' ';
        }
        oss << static_cast<int>(values[i]);
    }
    oss << "]";
    return oss.str();
}

void appendError(O6Snapshot &snapshot, const std::string &label, const std::string &message) {
    if (!snapshot.last_error.empty()) {
        snapshot.last_error += " | ";
    }
    snapshot.last_error += label + ": " + message;
}

uint8_t maxSensorValue(const ForceData &force_data, size_t finger_index, size_t sensor_type_index) {
    if (finger_index >= force_data.size()) {
        return 0;
    }
    if (sensor_type_index >= force_data[finger_index].size()) {
        return 0;
    }

    const auto &sensor_values = force_data[finger_index][sensor_type_index];
    if (sensor_values.empty()) {
        return 0;
    }

    return *std::max_element(sensor_values.begin(), sensor_values.end());
}

class O6AsyncReader {
public:
    O6AsyncReader(HAND_TYPE hand_type,
                  std::string can_channel,
                  int baudrate,
                  std::chrono::milliseconds poll_interval)
        : hand_(LINKER_HAND::O6, hand_type, can_channel, baudrate),
          poll_interval_(poll_interval) {}

    ~O6AsyncReader() {
        stop();
    }

    void start() {
        if (running_.exchange(true)) {
            return;
        }
        worker_ = std::thread(&O6AsyncReader::pollLoop, this);
    }

    void stop() {
        if (!running_.exchange(false)) {
            return;
        }
        if (worker_.joinable()) {
            worker_.join();
        }
    }

    O6Snapshot snapshot() const {
        std::lock_guard<std::mutex> lock(snapshot_mutex_);
        return snapshot_;
    }

private:
    template <typename Fn>
    void readField(O6Snapshot &next, const char *name, Fn &&fn) {
        try {
            fn();
        } catch (const linkerhand::UnsupportedFeatureException &e) {
            appendError(next, name, std::string("unsupported - ") + e.what());
        } catch (const std::exception &e) {
            appendError(next, name, e.what());
        }
    }

    void pollLoop() {
        uint64_t sequence = 0;

        while (running_.load()) {
            const auto cycle_started = Clock::now();

            O6Snapshot next;
            next.sequence = ++sequence;
            next.ready = true;
            next.updated_at = cycle_started;

            readField(next, "getState", [&]() { next.state = hand_.getState(); });
            readField(next, "getTorque", [&]() { next.torque = hand_.getTorque(); });
            readField(next, "getTemperature", [&]() { next.temperature = hand_.getTemperature(); });
            readField(next, "getFaultCode", [&]() { next.fault_code = hand_.getFaultCode(); });
            readField(next, "getForce", [&]() { next.force = hand_.getForce(); });

            {
                std::lock_guard<std::mutex> lock(snapshot_mutex_);
                snapshot_ = std::move(next);
            }

            std::this_thread::sleep_until(cycle_started + poll_interval_);
        }
    }

    LinkerHandApi hand_;
    std::chrono::milliseconds poll_interval_;
    std::atomic<bool> running_{false};
    std::thread worker_;

    mutable std::mutex snapshot_mutex_;
    O6Snapshot snapshot_;
};

void printSnapshot(const O6Snapshot &snapshot) {
    if (!snapshot.ready) {
        std::cout << "waiting for first O6 snapshot..." << std::endl;
        return;
    }

    const auto age = std::chrono::duration_cast<std::chrono::milliseconds>(
        Clock::now() - snapshot.updated_at);
    const bool stale = age > kStaleAfter;

    const uint8_t thumb_force = maxSensorValue(snapshot.force, 0, 0);
    const uint8_t index_force = maxSensorValue(snapshot.force, 1, 0);

    std::cout << "seq=" << snapshot.sequence
              << " age=" << age.count() << "ms";
    if (stale) {
        std::cout << " STALE";
    }
    std::cout << '\n';

    std::cout << "state       : " << bytesToString(snapshot.state) << '\n';
    std::cout << "torque      : " << bytesToString(snapshot.torque) << '\n';
    std::cout << "temperature : " << bytesToString(snapshot.temperature) << '\n';
    std::cout << "fault       : " << bytesToString(snapshot.fault_code) << '\n';
    std::cout << "thumb_force : " << static_cast<int>(thumb_force)
              << ", index_force: " << static_cast<int>(index_force) << '\n';

    if (!snapshot.last_error.empty()) {
        std::cout << "last_error  : " << snapshot.last_error << '\n';
    }
}

} // namespace

int main() {
    try {
        // 读取使用独立 SDK 实例和独立线程。
        // 如果你的控制线程因为 fingerMove 等调用卡住，主线程和这个读取线程不会一起被抢占。
        // 若你还需要控制，请为控制路径再创建一个独立 LinkerHandApi 实例，不要和读取复用同一个对象。
        O6AsyncReader reader(HAND_TYPE::RIGHT, "can0", 1000000, kPollInterval);
        reader.start();

        std::cout << "O6 async reader started." << std::endl;
        std::cout << "Main thread only consumes cached snapshots." << std::endl;
        std::cout << "Press Ctrl+C to stop." << std::endl;

        while (true) {
            const O6Snapshot snapshot = reader.snapshot();
            printSnapshot(snapshot);
            std::cout << "------------------------------" << std::endl;
            std::this_thread::sleep_for(kPrintInterval);
        }
    } catch (const std::exception &e) {
        std::cerr << "Failed to start O6 async reader: " << e.what() << std::endl;
        return 1;
    }
}
