/*********************************飞轮接口说明***********************************************************
GNC服务下达控制指令：
指令格式：id + mode + 速度 + 力矩
id：1/2/3...
mode：0速度模式 1滑行模式
速度：-6200~6200
力矩：0~2

GNC服务下达取遥测指令：
指令格式：id + 指令码
id：1/2/3...
指令码：0xFF
**********************************************************************************************************/

#include <iostream>
#include <cstring>
#include <cstdlib>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <pthread.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/select.h>
#include <errno.h>
#include <signal.h>
#include <string>
#include <vector>
#include <fstream>
#include <jsoncpp/json/json.h>

#include "FlywheelHS.h"
#include "SimMsg.h"

using namespace std;

/* ------------------ 全局变量 ------------------ */
SimMsg* FlywheelHS_part = nullptr;
string servername = "Flywheel_Hardware_Service";
string topic_name_cmd = "Command";
string topic_name_tlm = "Telemetry";

Flywheel_Device flywheels[MAX_FLYWHEELS] = {0};
uint8_t flywheel_count = 0;
pthread_mutex_t cache_mutex = PTHREAD_MUTEX_INITIALIZER;
Flywheel_Cmd_Cache cmd_cache[MAX_FLYWHEELS] = {0};

/* ------------------ 工具函数 ------------------ */
uint32_t bytes_to_uint32(const uint8_t *a) {
    return (((uint32_t)a[0] << 24) & 0xFF000000) |
           (((uint32_t)a[1] << 16) & 0x00FF0000) |
           (((uint32_t)a[2] << 8) & 0x0000FF00) |
           ((uint32_t)a[3] & 0x000000FF);
}

void cal_cmder_para(double k, double exp, uint8_t *a) {
    unsigned long t;
    if (k < 0) t = (unsigned long)(WHEEL_LIMIT4 + k * exp);
    else t = (unsigned long)(k * exp);
    a[0] = (uint8_t)(t >> 24) & 0xFF;
    a[1] = (uint8_t)(t >> 16) & 0xFF;
    a[2] = (uint8_t)(t >> 8) & 0xFF;
    a[3] = (uint8_t)t & 0xFF;
}

double cal_message_para(const uint8_t *data, int exp) {
    double result = 0;
    uint32_t k = bytes_to_uint32(data);
    if (k > 0x7FFFFFFF) {
        result = (double)k - WHEEL_LIMIT4;
    } else {
        result = (double)k;
    }
    return result / exp;
}

int checksum_verify(const uint8_t *data, int len) {
    if (len <= 1 || data == NULL) return SAT_ERR_DATA;
    uint8_t checksum = 0;
    for (int i = 1; i < len - 2; ++i) checksum += data[i];
    if (checksum == data[len - 1]) return SAT_ERR_OK;
    cerr << "[ERROR] Checksum error: calculated=0x" << hex << (int)checksum 
         << ", received=0x" << (int)data[len - 1] << dec << endl;
    return SAT_ERR_CHECKSUM;
}

void update_fault_count(Flywheel_Device *wheel) {
    wheel->fault_cnt = wheel->header_err_cnt + wheel->check_err_cnt + 
                       wheel->length_err_cnt + wheel->uart_reset_cnt;
}

const char *flywheel_strerror(int err) {
    switch (err) {
        case SAT_ERR_OK: return "OK";
        case SAT_ERR_SERIAL: return "Serial not open or invalid";
        case SAT_ERR_SEND_CMD: return "Failed to send command";
        case SAT_ERR_NO_RESPONSE: return "No response after sending command";
        case SAT_ERR_SELECT: return "Select() system call failed";
        case SAT_ERR_HEADER: return "Invalid frame header";
        case SAT_ERR_DATA: return "Failed to parse data";
        case SAT_ERR_UNKNOWN: return "Unknown error";
        case SAT_ERR_LEN: return "Frame length error";
        case SAT_ERR_CHECKSUM: return "Checksum error";
        case SAT_ERR_TIMEOUT: return "Timeout";
        case E_wheel_ID: return "Invalid wheel ID";
        default: return "Unrecognized error";
    }
}

uint8_t hardware_id_to_logical_id(uint8_t hardware_id, uint8_t default_id) {
    // 硬件ID转换: 0x11->1, 0x22->2, 0x33->3...
    if (hardware_id >= HARDWARE_ID_BASE && hardware_id <= 0x88 && 
        (hardware_id % HARDWARE_ID_BASE == 0)) {
        return hardware_id / HARDWARE_ID_BASE;
    } else {
        // 如果硬件ID不符合预期格式，使用默认ID
        return default_id;
    }
}

/* ------------------ 配置文件解析 ------------------ */
int parse_config_file(const char* filename, FlywheelConfig flywheel_configs[], int max_flywheels) {
    ifstream config_file(filename);
    if (!config_file.is_open()) {
        cerr << "[ERROR] Cannot open config file: " << filename << endl;
        return -1;
    }
    
    Json::CharReaderBuilder reader_builder;
    Json::Value root;
    string errs;
    
    if (!Json::parseFromStream(reader_builder, config_file, &root, &errs)) {
        cerr << "[ERROR] Failed to parse JSON: " << errs << endl;
        return -1;
    }
    
    int wheel_count = 0;
    
    if (root.isMember("flywheels") && root["flywheels"].isArray()) {
        const Json::Value& wheels = root["flywheels"];
        
        for (unsigned int i = 0; i < wheels.size() && wheel_count < max_flywheels; i++) {
            const Json::Value& wheel = wheels[i];
            
            FlywheelConfig config;
            
            if (!wheel.isMember("id")) {
                cerr << "[WARNING] Flywheel " << i << " has no ID specified, skipping" << endl;
                continue;
            }
            config.id = wheel["id"].asInt();
            
            if (!wheel.isMember("device")) {
                cerr << "[WARNING] Flywheel " << config.id << " has no device specified, skipping" << endl;
                continue;
            }
            config.device = wheel["device"].asString();
            
            config.baudrate = wheel.isMember("baudrate") ? wheel["baudrate"].asInt() : 115200;
            config.hardware_id = wheel.isMember("hardware_id") ? wheel["hardware_id"].asInt() : 
                                ((config.id + 1) * HARDWARE_ID_BASE);  // 默认硬件ID
            
            flywheel_configs[wheel_count] = config;
            wheel_count++;
        }
    } else {
        cerr << "[ERROR] No 'flywheels' array found in config file" << endl;
        return -1;
    }
    
    return wheel_count;
}

/* ------------------ FastDDS相关 ------------------ */
void FlywheelHSWriteLog(const std::string &msg) {
    std::cout << "[FlywheelHS] " << msg << std::endl;
}

void Fastdds_init(uint8_t domainid, string appname) {
    vector<string> parameters;
    string expression = "dest = '"+ servername + "'";
    
    if (nullptr == FlywheelHS_part) {
        FlywheelHS_part = new SimMsg(domainid, 3000, appname, FlywheelHSWriteLog);
        FlywheelHS_part->create_pub(topic_name_tlm);
        FlywheelHS_part->create_sub(topic_name_cmd, command_callback, expression, parameters);
        FlywheelHS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
    }
}

void Flywheel_telemetry_Pub(uint8_t* data, const string& dest, uint16_t len) {
    FlywheelHS_part->publish(topic_name_tlm, servername, dest, "telemetry", data, len);
}

/* ------------------ 串口初始化 ------------------ */
int flywheel_uart_init(uint8_t id, const char *dev, int baudrate, uint8_t hardware_id) {
    if (id >= MAX_FLYWHEELS) {
        cerr << "[ERROR] Invalid flywheel id: " << (int)id << endl;
        return SAT_ERR_DATA;
    }
    
    Flywheel_Device *wheel = &flywheels[id];
    
    if (wheel->fd > 0) {
        close(wheel->fd);
        wheel->fd = -1;
    }
    
    wheel->fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (wheel->fd < 0) {
        perror("[ERROR] Open flywheel serial failed");
        return SAT_ERR_SERIAL;
    }
    
    struct termios options;
    if (tcgetattr(wheel->fd, &options) < 0) {
        perror("[ERROR] Get serial attributes failed");
        close(wheel->fd);
        wheel->fd = -1;
        return SAT_ERR_SERIAL;
    }
    
    speed_t baud = B115200;
    switch (baudrate) {
        case 9600: baud = B9600; break;
        case 19200: baud = B19200; break;
        case 38400: baud = B38400; break;
        case 57600: baud = B57600; break;
        case 115200: baud = B115200; break;
        case 230400: baud = B230400; break;
        case 460800: baud = B460800; break;
        case 921600: baud = B921600; break;
        default: baud = B115200;
    }
    cfsetispeed(&options, baud);
    cfsetospeed(&options, baud);
    
    options.c_cflag |= PARENB;
    options.c_cflag |= PARODD;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;
    options.c_cflag |= CREAD | CLOCAL;
    options.c_iflag &= ~(IXON | IXOFF | IXANY);
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;
    options.c_cc[VMIN] = 0;
    options.c_cc[VTIME] = 10;
    
    if (tcsetattr(wheel->fd, TCSANOW, &options) < 0) {
        perror("[ERROR] Set serial attributes failed");
        close(wheel->fd);
        wheel->fd = -1;
        return SAT_ERR_SERIAL;
    }
    
    tcflush(wheel->fd, TCIOFLUSH);
    
    wheel->dev = dev;
    wheel->baudrate = baudrate;
    wheel->hardware_id = hardware_id;
    wheel->id = id;
    wheel->is_data_valid = 0;
    
    // 初始化错误计数
    wheel->header_err_cnt = 0;
    wheel->check_err_cnt = 0;
    wheel->length_err_cnt = 0;
    wheel->uart_reset_cnt = 0;
    wheel->fault_cnt = 0;
    wheel->send_cmd_cnt = 0;
    
    cout << "[OK] Flywheel " << (int)id << " UART init: dev=" << dev 
         << ", baud=" << baudrate << ", hw_id=0x" << hex << (int)hardware_id << dec << endl;
    
    return SAT_ERR_OK;
}

/* ------------------ 命令回调函数 ------------------ */
void command_callback(string src, string dest, string type,
                      string reserve1, string reserve2,
                      vector<uint8_t>& data) {
    cout << "[INFO] Command received from " << src << " to " << dest << endl;
    
    if (data.size() >= 10) {  // 10字节格式
        uint8_t wheel_id = data[0];       // 飞轮逻辑ID (1,2,3...)
        uint8_t mode_flag = data[1];      // 模式标志 (0=速度,1=力矩)
        
        float speed, torque;
        memcpy(&speed, &data[2], sizeof(float));   // 速度指令 (rpm)
        memcpy(&torque, &data[6], sizeof(float));  // 力矩指令 (mNm)
        
        // 验证飞轮ID
        if (wheel_id == 0 || wheel_id > flywheel_count) {
            cerr << "[ERROR] Invalid wheel_id: " << (int)wheel_id 
                 << ", valid range: 1-" << flywheel_count << endl;
            return;
        }
        
        // 转换为硬件索引 (0-based)
        uint8_t hw_index = wheel_id - 1;
        
        // 转换为硬件模式
        uint8_t hw_mode;
        if (mode_flag == 0) {
            hw_mode = 0x58;  // 速度模式
        } else if (mode_flag == 1) {
            hw_mode = 0x59;  // 力矩模式
        } else {
            cerr << "[ERROR] Invalid mode_flag: " << (int)mode_flag 
                 << ", must be 0(speed) or 1(torque)" << endl;
            return;
        }
        
        // 获取硬件ID
        Flywheel_Device *wheel = &flywheels[hw_index];
        uint8_t hw_id = wheel->hardware_id;
        
        // 转换为11字节硬件指令
        uint8_t raw_command[11];
        
        // 字节0: 模式
        raw_command[0] = hw_mode;
        
        // 字节1-4: 速度指令（编码）
        unsigned long speed_encoded;
        if (speed < 0) {
            speed_encoded = (unsigned long)(WHEEL_LIMIT4 + speed * WHEEL_EXP);
        } else {
            speed_encoded = (unsigned long)(speed * WHEEL_EXP);
        }
        raw_command[1] = (speed_encoded >> 24) & 0xFF;
        raw_command[2] = (speed_encoded >> 16) & 0xFF;
        raw_command[3] = (speed_encoded >> 8) & 0xFF;
        raw_command[4] = speed_encoded & 0xFF;
        
        // 字节5-8: 力矩指令（编码）
        unsigned long torque_encoded;
        if (torque < 0) {
            torque_encoded = (unsigned long)(WHEEL_LIMIT4 + torque * WHEEL_EXP);
        } else {
            torque_encoded = (unsigned long)(torque * WHEEL_EXP);
        }
        raw_command[5] = (torque_encoded >> 24) & 0xFF;
        raw_command[6] = (torque_encoded >> 16) & 0xFF;
        raw_command[7] = (torque_encoded >> 8) & 0xFF;
        raw_command[8] = torque_encoded & 0xFF;
        
        // 字节9: 硬件ID
        raw_command[9] = hw_id;
        
        // 字节10: 校验和（前10字节累加和取低8位）
        raw_command[10] = 0;
        for (int i = 0; i < 10; i++) {
            raw_command[10] += raw_command[i];
        }
        
        // 发送给硬件
        if (wheel->fd < 0) {
            cerr << "[ERROR] Serial not open for Flywheel " << (int)wheel_id << endl;
            return;
        }
        
        // 缓存指令
        pthread_mutex_lock(&cache_mutex);
        cmd_cache[hw_index].mode = hw_mode;
        cmd_cache[hw_index].speed_cmd = speed;
        cmd_cache[hw_index].torque_cmd = torque;
        cmd_cache[hw_index].has_cmd = 1;
        pthread_mutex_unlock(&cache_mutex);
        
        // 立即发送指令
        int bytes_written = write(wheel->fd, raw_command, sizeof(raw_command));
        if (bytes_written == sizeof(raw_command)) {
            cout << "[OK] Command sent to hardware" << endl;
        } else {
            cerr << "[ERROR] Failed to send command to hardware" << endl;
        }
    } else {
        cerr << "[ERROR] Invalid command data size: " << data.size() 
             << ", expected at least 10 bytes" << endl;
    }
}

/* ------------------ 遥测回调函数 ------------------ */
void telemetry_callback(string src, string dest, string type,
                        string reserve1, string reserve2,
                        vector<uint8_t>& data) {
    
    if (data.size() >= 2 && type == "command") {
        uint8_t wheel_id = data[0];    // 请求的飞轮ID (1,2,3...)
        uint8_t req_type = data[1];    // 请求类型
        
        // 注意：传入的wheel_id是1-based，需要转换为0-based索引
        if (wheel_id == 0 || wheel_id > flywheel_count) {
            cerr << "[ERROR] Invalid wheel_id in telemetry request: " 
                 << (int)wheel_id << ", valid range: 1-" << (int)flywheel_count << endl;
            
            // 构造错误响应帧
            Flywheel_Frame error_frame;
            memset(&error_frame, 0, sizeof(Flywheel_Frame));
            error_frame.wheel_id = wheel_id;  // 返回请求的ID
            
            Flywheel_telemetry_Pub((uint8_t *)&error_frame, src, sizeof(Flywheel_Frame));
            return;
        }
        
        uint8_t wheel_index = wheel_id - 1;
        Flywheel_Frame response_frame;
        int result = get_telemetry_from_cache(wheel_index, &response_frame);
        
        if (result == SAT_ERR_OK) {
            // 确保ID正确
            response_frame.wheel_id = wheel_id;
            Flywheel_telemetry_Pub((uint8_t *)&response_frame, src, sizeof(Flywheel_Frame));
            cout << "[TELEMETRY] Sent data for Flywheel " << (int)wheel_id 
                 << " to " << src << endl;
        } else {
            // 构造错误响应帧
            Flywheel_Frame error_frame;
            memset(&error_frame, 0, sizeof(Flywheel_Frame));
            error_frame.wheel_id = wheel_id;
            
            Flywheel_telemetry_Pub((uint8_t *)&error_frame, src, sizeof(Flywheel_Frame));
            cerr << "[ERROR] No valid telemetry data for Flywheel " 
                 << (int)wheel_id << endl;
        }
    } else {
        cerr << "[ERROR] Invalid telemetry request data size: " << data.size() 
             << " or type: " << type << endl;
    }
}

/* ------------------ 串口接收函数 ------------------ */
int check_and_receive_serial_data(void) {
    fd_set readfds;
    struct timeval timeout;
    int max_fd = -1;
    int frames_received = 0;
    
    FD_ZERO(&readfds);
    
    // 设置要监听的串口文件描述符
    for (uint8_t i = 0; i < flywheel_count; i++) {
        if (flywheels[i].fd > 0) {
            FD_SET(flywheels[i].fd, &readfds);
            if (flywheels[i].fd > max_fd) {
                max_fd = flywheels[i].fd;
            }
        }
    }
    
    if (max_fd < 0) {
        return 0;  // 没有有效的串口
    }
    
    // 设置超时为0，立即返回（非阻塞）
    timeout.tv_sec = 0;
    timeout.tv_usec = 0;
    
    // 检查哪些串口有数据可读
    int result = select(max_fd + 1, &readfds, NULL, NULL, &timeout);
    
    if (result > 0) {
        // 有数据可读的串口
        for (uint8_t i = 0; i < flywheel_count; i++) {
            if (flywheels[i].fd > 0 && FD_ISSET(flywheels[i].fd, &readfds)) {
                uint8_t buffer[WHEEL_FRAME_LENGTH];
                int bytes_read = read(flywheels[i].fd, buffer, WHEEL_FRAME_LENGTH);
                
                if (bytes_read == WHEEL_FRAME_LENGTH) {
                    Flywheel_Frame frame;
                    int parse_result = parse_flywheel_frame(i, buffer, &frame);
                    
                    if (parse_result == SAT_ERR_OK) {
                        pthread_mutex_lock(&cache_mutex);
                        flywheels[i].last_frame = frame;
                        flywheels[i].is_data_valid = 1;
                        pthread_mutex_unlock(&cache_mutex);
                        
                        frames_received++;
                        cout << "[DATA] Flywheel " << (int)frame.wheel_id 
                             << " received: speed=" << frame.rotate_speed 
                             << " rpm, current=" << frame.current 
                             << ", torque=" << frame.acc_torque << endl;
                    }
                } else if (bytes_read > 0) {
                    cerr << "[WARN] Flywheel " << (int)i << " incomplete frame: " 
                         << bytes_read << "/" << WHEEL_FRAME_LENGTH << " bytes" << endl;
                    flywheels[i].length_err_cnt++;
                    update_fault_count(&flywheels[i]);
                }
            }
        }
    } else if (result < 0 && errno != EINTR) {
        perror("[ERROR] Select failed");
        return -1;
    }
    
    return frames_received;
}

/* ------------------ 解析飞轮数据帧 ------------------ */
int parse_flywheel_frame(uint8_t wheel_index, const uint8_t* buffer, Flywheel_Frame* frame) {
    Flywheel_Device *wheel = &flywheels[wheel_index];
    
    // 检查帧头
    if (buffer[0] != 0xFF) {
        wheel->header_err_cnt++;
        update_fault_count(wheel);
        return SAT_ERR_HEADER;
    }
    
    // 检查校验和
    if (checksum_verify(buffer, WHEEL_FRAME_LENGTH) != SAT_ERR_OK) {
        wheel->check_err_cnt++;
        update_fault_count(wheel);
        return SAT_ERR_CHECKSUM;
    }
    
    // 获取硬件ID并转换为逻辑ID
    uint8_t hardware_id = buffer[17];
    uint8_t logical_id = hardware_id_to_logical_id(hardware_id, wheel_index + 1);
    
    // 解析数据
    float rotate_speed = cal_message_para(&buffer[1], WHEEL_EXP);
    float current = cal_message_para(&buffer[5], WHEEL_EXP);
    float acc_torque = cal_message_para(&buffer[13], WHEEL_EXP);
    
    // 填充数据帧，只保留需要的字段
    memset(frame, 0, sizeof(Flywheel_Frame));
    
    // 设备标识 - 传转换后的逻辑ID (1,2,3...)
    frame->wheel_id = logical_id;
    
    // 状态数据
    frame->rotate_speed = rotate_speed;
    frame->current = current;
    frame->acc_torque = acc_torque;
    
    // 错误统计
    frame->header_err_cnt = wheel->header_err_cnt;
    frame->check_err_cnt = wheel->check_err_cnt;
    frame->length_err_cnt = wheel->length_err_cnt;
    frame->uart_reset_cnt = wheel->uart_reset_cnt;
    frame->fault_cnt = wheel->fault_cnt;
    
    return SAT_ERR_OK;
}

/* ------------------ 发送控制指令 ------------------ */
int send_flywheel_command(uint8_t wheel_index) {
    Flywheel_Device *wheel = &flywheels[wheel_index];
    
    if (wheel->fd < 0) {
        return SAT_ERR_SERIAL;
    }
    
    pthread_mutex_lock(&cache_mutex);
    Flywheel_Cmd_Cache cmd = cmd_cache[wheel_index];
    pthread_mutex_unlock(&cache_mutex);
    
    if (!cmd.has_cmd) {
        return SAT_ERR_OK;  // 没有指令要发送
    }
    
    // 构建11字节硬件指令
    uint8_t raw_command[11];
    
    // 硬件ID
    uint8_t hw_id = wheel->hardware_id;
    
    // 字节0: 模式
    raw_command[0] = cmd.mode;
    
    // 字节1-4: 速度指令（编码）
    unsigned long speed_encoded;
    double speed_cmd = cmd.speed_cmd;
    if (speed_cmd < 0) {
        speed_encoded = (unsigned long)(WHEEL_LIMIT4 + speed_cmd * WHEEL_EXP);
    } else {
        speed_encoded = (unsigned long)(speed_cmd * WHEEL_EXP);
    }
    raw_command[1] = (speed_encoded >> 24) & 0xFF;
    raw_command[2] = (speed_encoded >> 16) & 0xFF;
    raw_command[3] = (speed_encoded >> 8) & 0xFF;
    raw_command[4] = speed_encoded & 0xFF;
    
    // 字节5-8: 力矩指令（编码）
    unsigned long torque_encoded;
    double torque_cmd = cmd.torque_cmd;
    if (torque_cmd < 0) {
        torque_encoded = (unsigned long)(WHEEL_LIMIT4 + torque_cmd * WHEEL_EXP);
    } else {
        torque_encoded = (unsigned long)(torque_cmd * WHEEL_EXP);
    }
    raw_command[5] = (torque_encoded >> 24) & 0xFF;
    raw_command[6] = (torque_encoded >> 16) & 0xFF;
    raw_command[7] = (torque_encoded >> 8) & 0xFF;
    raw_command[8] = torque_encoded & 0xFF;
    
    // 字节9: 硬件ID
    raw_command[9] = hw_id;
    
    // 字节10: 校验和
    raw_command[10] = 0;
    for (int i = 0; i < 10; i++) {
        raw_command[10] += raw_command[i];
    }
    
    // 发送指令
    int bytes_written = write(wheel->fd, raw_command, sizeof(raw_command));
    if (bytes_written != sizeof(raw_command)) {
        cerr << "[Flywheel" << (int)wheel_index << "] Send cmd failed: " 
             << strerror(errno) << endl;
        wheel->send_cmd_cnt++;
        return SAT_ERR_SEND_CMD;
    }
    
    wheel->send_cmd_cnt = 0;
    
    return SAT_ERR_OK;
}

/* ------------------ 从缓存获取遥测数据 ------------------ */
int get_telemetry_from_cache(uint8_t wheel_index, Flywheel_Frame* frame) {
    if (wheel_index >= flywheel_count || !frame) {
        return E_wheel_ID;
    }
    
    if (!flywheels[wheel_index].is_data_valid) {
        return SAT_ERR_NO_RESPONSE;
    }
    
    pthread_mutex_lock(&cache_mutex);
    *frame = flywheels[wheel_index].last_frame;
    pthread_mutex_unlock(&cache_mutex);
    
    return SAT_ERR_OK;
}

/* ------------------ 清理函数 ------------------ */
void FlywheelHS_cleanup(void) {
    
    if (FlywheelHS_part != nullptr) {
        delete FlywheelHS_part;
        FlywheelHS_part = nullptr;
    }
    
    for (uint8_t i = 0; i < MAX_FLYWHEELS; i++) {
        if (flywheels[i].fd > 0) {
            close(flywheels[i].fd);
            flywheels[i].fd = -1;
        }
    }
    
    pthread_mutex_destroy(&cache_mutex);
    
}