#include "device_init.h"
#include "devcontrol_common.h"
#include "common.h"
#include "gpio_init.h"
#include "car0107_idle.h"

int g_device_delay_back_count  =0;//设备计时，比如坦克打击倒退逻辑

int g_device_type =0; //设备详细类型，让其他地方判断，比如tank0202、car0101

static const deviceconfig_t s_device_configs[] = {
    // car0101配置
    {
        .device_id = DEVICE_CAR0101,
        .device_name = "car0101",
        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26, 27, -1},
        .gpio_pwms = {-1},
        .gpio_inputs={-1},
        .device_pwm_init =  physics_pwm_init,
        .device_control_stop = car0101_middle_pwm,
        .emergency_code = 101
    },
    // car0102配置
    {
        .device_id = DEVICE_CAR0102,
        .device_name = "car0102",
        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26, 27, -1},
        .gpio_pwms = {5, 7, -1},
        .gpio_inputs={-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop = car0102_speed_stop,
        .emergency_code = 102
    },
    // car0103配置（根据您的需求补充）
    {
        .device_id = DEVICE_CAR0103,
        .device_name = "car0103",
        .gpio_pins = {5, 6, 7, 9 , 10, 16, 20, 22, 23, 24, 25, 26, 27,-1},/* 补充GPIO引脚 */
        .gpio_pwms = {-1},
        .gpio_inputs={11,12,14,17,18,20,-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop = car0103_middle,/* 补充速度控制函数 */
        .emergency_code = 103
    },
    // car0104配置（根据您的需求补充）
    {
        .device_id = DEVICE_CAR0104,
        .device_name = "car0104",
        .gpio_pins = {5, 6, 7, 9,10, 16, 20, 22, 23, 24, 25, 26, 27,-1},/* 补充GPIO引脚 */
        .gpio_pwms = {-1},
        .gpio_inputs={11,12,14,20,-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  car0104_stop,/* 补充速度控制函数 */
        .emergency_code = 104
    },
    // car0105配置（根据您的需求补充）
    {
        .device_id = DEVICE_CAR0105,
        .device_name = "car0105",
        .gpio_pins = {5, 6, 7, 9,10, 16, 20, 22, 23, 24, 25, 26, -1},/* 补充GPIO引脚 */
        .gpio_pwms = {27,-1},
        .gpio_inputs={-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  car0101_middle_pwm,/* 补充速度控制函数 */
        .emergency_code = 105
    },
    // car0106 F1赛车（双电机大板）: 软PWM 5/7/24/26 对应前后左右
    {
        .device_id = DEVICE_CAR0106,
        .device_name = "car0106",
        .gpio_pins = {-1},
        .gpio_pwms = {5, 7, 24, 26, -1},
        .gpio_inputs={-1},
        .device_pwm_init = car0106_pwm_init_empty,
        .device_control_stop = car0106_stop,
        .emergency_code = 106
    },
    // car0107 大众POLO R（与0101相同硬件，前后方向相反）
    {
        .device_id = DEVICE_CAR0107,
        .device_name = "car0107",
        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26, 27, -1},
        .gpio_pwms = {-1},
        .gpio_inputs={-1},
        .device_pwm_init =  physics_pwm_init,
        .device_control_stop = car0107_middle_pwm,
        .emergency_code = 107
    },
     // tank0202配置（根据您的需求补充）
    {
        .device_id = DEVICE_TANK0202,
        .device_name = "tank0202",
        .gpio_pins = {6, 16, 20, 22,-1},/* 补充GPIO引脚 */
        .gpio_pwms = {5 , 7 ,24,26, 27,-1},
        .gpio_inputs={12,-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  tank0202_middle,/* 补充速度控制函数 */
        .emergency_code = 202
    }, 
    {
        .device_id = DEVICE_TANK0203,
        .device_name = "tank0203",
        .gpio_pins = {6, 16, 20, 22,-1},/* 补充GPIO引脚 */
        .gpio_pwms = {5 , 7 ,24, 26, 27,-1},
        .gpio_inputs={12,-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  tank0203_middle,/* 补充速度控制函数 */
        .emergency_code = 203
    },
    {
        .device_id = DEVICE_TANK0204,
        .device_name = "tank0204",
        .gpio_pins = {6, 16, 20, 22,-1},/* 补充GPIO引脚 */
        .gpio_pwms = {5 , 7 ,24, 26, 27,-1},
        .gpio_inputs={12,-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  tank0204_stop,/* 补充速度控制函数 */
        .emergency_code = 204
    },
    {
        .device_id = DEVICE_TANK0206,
        .device_name = "tank0206",
        .gpio_pins = {6, 16, 20, 22, 23,-1},/* 补充GPIO引脚 */
        .gpio_pwms = {5 , 7 ,24,26, 27,-1},
        .gpio_inputs={-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  tank0206_middle,/* 补充速度控制函数 */
        .emergency_code = 206
    },
    {
        .device_id = DEVICE_TANK0207,
        .device_name = "tank0207",
        .gpio_pins = {6, 16, 20, 22, 23,-1},/* 补充GPIO引脚 */
        .gpio_pwms = {5 , 7 ,24,26, 27,-1},
        .gpio_inputs={-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  tank0207_middle,/* 补充速度控制函数 */
        .emergency_code = 207
    },
    {
        .device_id = DEVICE_SHIP0301,
        .device_name = "ship0301",
        .gpio_pins = {6, 16, 20, 22, 23,-1},/* 补充GPIO引脚 */
        .gpio_pwms = {5 , 7 ,24,26, 27,-1},
        .gpio_inputs={-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  ship0301_stop,/* 补充速度控制函数 */
        .emergency_code = 301
    },
    {
        .device_id = DEVICE_SHIP0302,
        .device_name = "ship0302",
        .gpio_pins = {6, 16, 20, 22, 23,-1},
        .gpio_pwms = {5 , 7 ,24,26, 27,-1},
        .gpio_inputs={-1},
        .device_pwm_init = device_ship0302_init,
        .device_control_stop =  ship0302_stop,
        .emergency_code = 302
    },
    {
        .device_id = DEVICE_PAO_PTZ0401,
        .device_name = "ptz0401",
        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26,27,-1},/* 补充GPIO引脚 */
        .gpio_pwms = { -1},
        .gpio_inputs={-1},
        .device_pwm_init = ptz_driver_pwm_hz,
        .device_control_stop = ptz_driver_init,
        .emergency_code = 401
    },

    {
        .device_id = DEVICE_PAO_PTZ0404,
        .device_name = "ptz0404",
        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26,27,-1},/* 补充GPIO引脚 */
        .gpio_pwms = { -1},
        .gpio_inputs={-1},
        .device_pwm_init = ptz_driver_pwm_hz,
        .device_control_stop = ptz_driver_init,
        .emergency_code = 404
    },

    {
        .device_id = DEVICE_PAO_PTZ0405,
        .device_name = "ptz0405",
        .gpio_pins = {5, 7, 10, 16, 20, 22, 23, 24, 25, 26,27,-1},
        .gpio_pwms = {-1},
        .gpio_inputs={-1},
        .device_pwm_init = ptz_driver_pwm_hz,
        .device_control_stop = ptz_driver_init,
        .emergency_code = 405
    },
    {
        .device_id = DEVICE_PAO_PTZ0406,
        .device_name = "ptz0406",
        .gpio_pins = {5, 7, 10, 16, 20, 22, 23, 24, 25, 26,27,-1},
        .gpio_pwms = {-1},
        .gpio_inputs={-1},
        .device_pwm_init = ptz_driver_pwm_hz,
        .device_control_stop = ptz_driver_init,
        .emergency_code = 406
    },

     {
        .device_id = DEVICE_PG_GPS0403,
        .device_name = "gps0403",
        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26,-1},/* 补充GPIO引脚 */
        .gpio_pwms = { 27,-1},
        .gpio_inputs={-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  car0101_middle_pwm,/* 补充速度控制函数 */
        .emergency_code = 403
    },
    {
        .device_id = DEVICE_ROBOT_DOG0501,
        .device_name = "dog0501",
        .gpio_pins = {6,  10, 16, 20, 22, 23,  25,-1},/* 补充GPIO引脚 */
        .gpio_pwms = { 5, 7,24, 26,27,-1},
        .gpio_inputs={-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop =  car0101_middle_pwm,/* 补充速度控制函数 */
        .emergency_code = 501
    },
    {
        .device_id = DEVICE_ROBOT_ARM0502,
        .device_name = "arm0502",
        .gpio_pins = {5, 6, 7, 10, 16, 20, 22, 23, 24, 25, 26,-1},
        .gpio_pwms = { 27,-1},
        .gpio_inputs={-1},
        .device_pwm_init = physics_pwm_init,
        .device_control_stop = arm0502_control_stop_init,
        .emergency_code = 502
    },
    // 结束标记
    { .device_id = -1 }
};

bool get_array_length(const int* arr) {
    if(arr[0] != -1) {  // 遇到-1停止计数
        return TRUE;
    }
    return FALSE;
}

void device_init(int device_id) {
    const deviceconfig_t *config = NULL;
    
    // 查找设备配置
    for(int i = 0; s_device_configs[i].device_id != -1; i++) {
        if(s_device_configs[i].device_id == device_id) {
            config = &s_device_configs[i];
            break;
        }
    }
    
    if(!config) {
        my_zlog_error("Error: Device ID %d not found!", device_id);
        return;
    }
    
    // 执行初始化流程
    if(get_array_length(config->gpio_pins)==TRUE)init_gpiowpi(config->gpio_pins);  // GPIO初始化
    if(get_array_length(config->gpio_pwms)==TRUE)init_gpiopwm(config->gpio_pwms);  // GPIOsoft_pwm初始化
    if(get_array_length(config->gpio_inputs)==TRUE) init_gpio_input(config->gpio_inputs);
    g_device_type =config->emergency_code;
    config->device_pwm_init();               // PWM初始化
    config->device_control_stop();          // 速度控制初始化

    if (device_id == DEVICE_CAR0102) {
        car0102_pin27_startup(device_id);
    }
    if (device_id == DEVICE_CAR0107) {
        car0107_idle_startup(device_id);
    }

    
    my_zlog_debug("%s initialized successfully!", config->device_name);
}