#include "devcontrol_common.h"
#include "modules_common.h"
#include "gpio_common.h"

static const device_didrive s_didrive_control_config[]={
        {
        .device_id = DEVICE_CAR0101,
        .device_didrive_control = car0101_control_change
    },
        {
        .device_id = DEVICE_CAR0102,
        .device_didrive_control = car0102_speed_change
    },
        {
        .device_id = DEVICE_CAR0103,
        .device_didrive_control = car0103_change
    },
        {
        .device_id = DEVICE_CAR0104,
        .device_didrive_control =  car0104_change
    },
         {
        .device_id = DEVICE_CAR0105,
        .device_didrive_control =  car0105_control_change
    },
        {
        .device_id = DEVICE_CAR0106,
        .device_didrive_control =  car0106_control_change
    },
        {
        .device_id = DEVICE_TANK0202,
        .device_didrive_control = tank0202_change
    },

    {
        .device_id = DEVICE_TANK0203,
        .device_didrive_control = tank0203_change
    },
    {
        .device_id = DEVICE_TANK0204,
        .device_didrive_control = tank0204_change
    },
    {
        .device_id = DEVICE_TANK0206,
        .device_didrive_control = tank0206_change
    },
    {
        .device_id = DEVICE_TANK0207,
        .device_didrive_control = tank0207_change
    },
    {
        .device_id = DEVICE_SHIP0301,
        .device_didrive_control = ship0301_change
    },
    {
        .device_id = DEVICE_SHIP0302,
        .device_didrive_control = ship0302_change
    },
        {
        .device_id = DEVICE_PAO_PTZ0401,
        .device_didrive_control = ptz_driver_change
    },
        {
        .device_id = DEVICE_PAO_PTZ0404,
        .device_didrive_control = ptz_driver_change
    },
        {
        .device_id = DEVICE_PAO_PTZ0405,
        .device_didrive_control = ptz_driver_change
    },
        {
        .device_id = DEVICE_PAO_PTZ0406,
        .device_didrive_control = ptz_driver_change
    },
    {
        .device_id = DEVICE_ROBOT_ARM0502,
        .device_didrive_control = arm0502_control_change
    },
    // 结束标记
    { .device_id = -1 }
};

static const device_abnormal_close_t s_devcontrol_config[]= {
    {
        .device_id = DEVICE_CAR0101,
        .device_abnormal_stop = car0101_middle_pwm,
        .device_close = NULL,                 // TANK0206没有单独的关闭函数
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_CAR0102,
        .device_abnormal_stop = car0102_speed_stop,
        .device_close = NULL,                 // TANK0206没有单独的关闭函数
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_CAR0103,
        .device_abnormal_stop = car0103_middle,
        .device_close = NULL,                 // TANK0206没有单独的关闭函数
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_CAR0104,
        .device_abnormal_stop = car0104_stop,
        .device_close = NULL,                 // TANK0206没有单独的关闭函数
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

        {
        .device_id = DEVICE_CAR0105,
        .device_abnormal_stop = car0105_serial_stop,
        .device_close = NULL,                 // TANK0206没有单独的关闭函数
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

        {
        .device_id = DEVICE_CAR0106,
        .device_abnormal_stop = car0106_stop,
        .device_close = NULL,
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_TANK0202,
        .device_abnormal_stop = tank0202_middle,
        .device_close = device_poilthread_close,
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_TANK0203,
        .device_abnormal_stop = tank0203_middle,
        .device_close = device_poilthread_close,
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_TANK0204,
        .device_abnormal_stop = tank0204_stop,
        .device_close = device_poilthread_close,
        .gpio_pin_pulled=tank0204_pin_all_default,
        .gpio_pwm_pulled=tankpwm_default
    },

    {
        .device_id = DEVICE_TANK0206,
        .device_abnormal_stop = tank0206_middle,
        .device_close = NULL,                 // TANK0206没有单独的关闭函数
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_TANK0207,
        .device_abnormal_stop = tank0207_middle,
        .device_close = NULL,                 // TANK0206没有单独的关闭函数
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_SHIP0301,
        .device_abnormal_stop = ship0301_stop,
        .device_close = NULL,                 // TANK0206没有单独的关闭函数
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_SHIP0302,
        .device_abnormal_stop = ship0302_stop,
        .device_close = NULL,
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_PAO_PTZ0401,
        .device_abnormal_stop = ptz_driver_stop,
        .device_close = device_poilthread_close,
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_PAO_PTZ0404,
        .device_abnormal_stop = ptz_driver_stop,
        .device_close = device_poilthread_close,
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_PAO_PTZ0405,
        .device_abnormal_stop = ptz_driver_stop,
        .device_close = device_poilthread_close,
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_PAO_PTZ0406,
        .device_abnormal_stop = ptz_driver_stop,
        .device_close = device_poilthread_close,
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_ROBOT_DOG0501,
        .device_abnormal_stop = car0101_middle_pwm,
        .device_close = NULL,                 // TANK0206没有单独的关闭函数
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },

    {
        .device_id = DEVICE_ROBOT_ARM0502,
        .device_abnormal_stop = arm0502_serial_stop_all,
        .device_close = NULL,
        .gpio_pin_pulled=pin_all_default,
        .gpio_pwm_pulled=pwm_all_default
    },
    // 结束标记
    { .device_id = -1 }
};

void device_stop(int device_id) {
    const device_abnormal_close_t *config = NULL;
    
    // 查找设备配置
    for(int i = 0; s_devcontrol_config[i].device_id != -1; i++) {
        if(s_devcontrol_config[i].device_id == device_id) {
            config = &s_devcontrol_config[i];
            break;
        }
    }
    
    if(!config) {
        my_zlog_error("Error: Device gpio stop is low ID %d not found!", device_id);
        return;
    }
    
    config->device_abnormal_stop(); 
    config->gpio_pin_pulled();
    config->gpio_pwm_pulled();         

}

void device_end_close(int device_id) {
    const device_abnormal_close_t *config = NULL;
    
    // 查找设备配置
    for(int i = 0; s_devcontrol_config[i].device_id != -1; i++) {
        if(s_devcontrol_config[i].device_id == device_id) {
            config = &s_devcontrol_config[i];
            break;
        }
    }
    
    if(!config) {
        my_zlog_error("Error: Device pthread close  ID %d not found!", device_id);
        return;
    }

    if(config->device_close != NULL) {
        my_zlog_info("Calling device_close for ID %d", device_id);
        config->device_close();
    }      

}


void device_walk_control(int device_id,int *valt) {
    const device_didrive *config = NULL;
    
    // 查找设备配置
    for(int i = 0; s_didrive_control_config[i].device_id != -1; i++) {
        if(s_didrive_control_config[i].device_id == device_id) {
            config = &s_didrive_control_config[i];
            break;
        }
    }
    
    if(!config) {
        my_zlog_error("Error: Device walk control ID %d not found!", device_id);
        return;
    }
    
    config->device_didrive_control(valt);          // 速度控制初始化

}

/*
* 串口设备启动
*/
void device_serial_exit_run(){
    if(g_device_type==DEVICE_CAR0105){
        car0105_serial_run(); 
    }else if(g_device_type==DEVICE_PG_GPS0403){
        pg0403_serial_run();
    }else if(g_device_type==DEVICE_ROBOT_ARM0502){
        (void)arm0502_serial_run();
    }else return;
}