#include "tc_system.h"
#include "ast_XH.h"
#include "modbus_TCP_HW.h"
#include "goose_HW.h"
#include "stm32f103.h"
#include "tc_thread.h"
#include "config.h"


static ast_xh_t ccuASTXH;
static stm32f103_t stm32F103;
static modbus_TCP_t modbusHW;
static goose_HW_t gooseGUNA;
static goose_HW_t gooseGUNB;
static tc_thread_t threadTCU[5];


void ccu_Update_serialID(const char* serialID)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    char* buff = (char*)&dev->regs_0x1000[0x16];
    pthread_mutex_unlock(&dev->mutex);
    sprintf(buff, "%s", serialID);
}

void ccu_Update_UTC(void)
{
    modbus_TCP_t* dev = &modbusHW;
    struct timeval tv;
    gettimeofday(&tv, NULL);
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1000[0x24] = tv.tv_sec&0xFFFF;
    dev->regs_0x1000[0x25] = (tv.tv_sec>>16)&0xFFFF;
    pthread_mutex_unlock(&dev->mutex);
}

void ccu_Update_GUNA_Type(uint16_t type)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1100[0x4] = type;
    pthread_mutex_unlock(&dev->mutex);
    // printd("0x1104:%d\n", dev->regs_0x1100[0x4]);
    modbus_TCP_report_registers(&modbusHW, 0x1104, &dev->regs_0x1100[0x4], 1);
}

void ccu_Update_GUNB_Type(uint16_t type)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1D00[0x4] = type;
    pthread_mutex_unlock(&dev->mutex);
    // printd("0x1D04:%d\n", dev->regs_0x1D00[0x4]);
    modbus_TCP_report_registers(&modbusHW, 0x1D04, &dev->regs_0x1D00[0x4], 1);
}

void ccu_Update_GUNA_IVE(uint16_t maxI, uint16_t currI, uint16_t currV, uint32_t totalE, uint32_t currE)
{
    uint32_t value[2];
    float *pvalue = (float*)&value[0];
    double *pdouble = (double*)&value[0];
    modbus_TCP_t* dev = &modbusHW;

    pthread_mutex_lock(&dev->mutex);
    pvalue[0] = ((float)maxI) / 10.0;
    dev->regs_0x1100[0x6] = value[0] & 0xFFFF;
    dev->regs_0x1100[0x5] = (value[0]>>16) & 0xFFFF;
    // printi("---0x1105:1106:%04x %04x,   maxI float:%f\n", dev->regs_0x1100[0x5], dev->regs_0x1100[0x6], pvalue[0]);

    pvalue[0] = ((float)currV) / 10.0;
    dev->regs_0x1100[0x8] = value[0] & 0xFFFF;
    dev->regs_0x1100[0x7] = (value[0]>>16) & 0xFFFF;
    // printi("---0x1107:1108:%04x %04x,   currV float:%f\n", dev->regs_0x1100[0x7], dev->regs_0x1100[0x8], pvalue[0]);

    pvalue[0] = ((float)currI) / 10.0;
    dev->regs_0x1100[0xA] = value[0] & 0xFFFF;
    dev->regs_0x1100[0x9] = (value[0]>>16) & 0xFFFF;
    // printi("---0x1109:110A:%04x %04x,   currI float:%f\n", dev->regs_0x1100[0x9], dev->regs_0x1100[0xA], pvalue[0]);

    pdouble[0] = (double)totalE/1000;
    dev->regs_0x1100[0x3] = value[0] & 0xFFFF;
    dev->regs_0x1100[0x2] = (value[0]>>16) & 0xFFFF;
    dev->regs_0x1100[0x1] = value[1] & 0xFFFF;
    dev->regs_0x1100[0x0] = (value[1]>>16) & 0xFFFF;
    // printi("---0x1100:1103:%04x %04x %04x %04x,   double:%f\n", dev->regs_0x1100[0x0], dev->regs_0x1100[0x1], dev->regs_0x1100[0x2], dev->regs_0x1100[0x3], pdouble[0]);

    pvalue[0] = ((float)currV) / 10.0;
    dev->regs_0x1100[0x14] = (value[0]>>0) & 0xFFFF;
    dev->regs_0x1100[0x13] = (value[0]>>16) & 0xFFFF;
    pthread_mutex_unlock(&dev->mutex);
    //printd("---[0x114 0x1113]:%04x %04x;  %f\n", dev->regs_0x1100[0x14], dev->regs_0x1100[0x13] ,pvalue[0]);
    modbus_TCP_report_registers(&modbusHW, 0x1113, &dev->regs_0x1100[0x13], 2);
}

void ccu_Update_GUNB_IVE(uint16_t maxI, uint16_t currI, uint16_t currV, uint32_t totalE, uint32_t currE)
{
    uint32_t value[2];
    float *pvalue = (float*)&value[0];
    double *pdouble = (double*)&value[0];
    modbus_TCP_t* dev = &modbusHW;

    pthread_mutex_lock(&dev->mutex);

    pvalue[0] = ((float)maxI) / 10.0;
    dev->regs_0x1D00[0x6] = value[0] & 0xFFFF;
    dev->regs_0x1D00[0x5] = (value[0]>>16) & 0xFFFF;
    // printi("---0x1D05:1D06:%04x %04x,   maxI float:%f\n", dev->regs_0x1D00[0x5], dev->regs_0x1D00[0x6], pvalue[0]);

    pvalue[0] = ((float)currV) / 10.0;
    dev->regs_0x1D00[0x8] = value[0] & 0xFFFF;
    dev->regs_0x1D00[0x7] = (value[0]>>16) & 0xFFFF;
    // printi("---0x1D07:1D08:%04x %04x,   currV float:%f\n", dev->regs_0x1D00[0x7], dev->regs_0x1D00[0x8], pvalue[0]);

    pvalue[0] = ((float)currI) / 10.0;
    dev->regs_0x1D00[0xA] = value[0] & 0xFFFF;
    dev->regs_0x1D00[0x9] = (value[0]>>16) & 0xFFFF;
    // printi("---0x1D09:1D0A:%04x %04x,   currI float:%f\n", dev->regs_0x1D00[0x9], dev->regs_0x1D00[0xA], pvalue[0]);

    pdouble[0] = (double)totalE/1000;
    dev->regs_0x1D00[0x3] = value[0] & 0xFFFF;
    dev->regs_0x1D00[0x2] = (value[0]>>16) & 0xFFFF;
    dev->regs_0x1D00[0x1] = value[1] & 0xFFFF;
    dev->regs_0x1D00[0x0] = (value[1]>>16) & 0xFFFF;
    // printi("---0x1D00:1D03:%04x %04x %04x %04x,   double:%f\n", dev->regs_0x1D00[0x0], dev->regs_0x1D00[0x1], dev->regs_0x1D00[0x2], dev->regs_0x1D00[0x3], pdouble[0]);

    pvalue[0] = ((float)currV) / 10.0;
    dev->regs_0x1D00[0x14] = (value[0]>>0) & 0xFFFF;
    dev->regs_0x1D00[0x13] = (value[0]>>16) & 0xFFFF;
    pthread_mutex_unlock(&dev->mutex);
    // printd("---[0x114 0x1D13]:%04x %04x;  %f\n", dev->regs_0x1D00[0x14], dev->regs_0x1D00[0x13] ,pvalue[0]);
    modbus_TCP_report_registers(&modbusHW, 0x1D13, &dev->regs_0x1D00[0x13], 2);
}

void ccu_Update_GUNA_WorkState(uint16_t state)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1100[0xB] = state;
    pthread_mutex_unlock(&dev->mutex);
    printd("0x110B:%d\n", dev->regs_0x1100[0xB]);
    modbus_TCP_report_registers(&modbusHW, 0x110B, &dev->regs_0x1100[0xB], 1);
}

void ccu_Update_GUNB_WorkState(uint16_t state)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1D00[0xB] = state;
    pthread_mutex_unlock(&dev->mutex);
    printd("0x1D0B:%d\n", dev->regs_0x1D00[0xB]);
    modbus_TCP_report_registers(&modbusHW, 0x1D0B, &dev->regs_0x1D00[0xB], 1);
}

void ccu_Update_GUNA_LinkState(uint16_t state)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1100[0xD] = state;
    pthread_mutex_unlock(&dev->mutex);
    printd("0x110D:%d\n", dev->regs_0x1100[0xD]);
    modbus_TCP_report_registers(&modbusHW, 0x110D, &dev->regs_0x1100[0xD], 1);
}

void ccu_Update_GUNB_LinkState(uint16_t state)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1D00[0xD] = state;
    pthread_mutex_unlock(&dev->mutex);
    printd("0x1D0D:%d\n", dev->regs_0x1D00[0xD]);
    modbus_TCP_report_registers(&modbusHW, 0x1D0D, &dev->regs_0x1D00[0xD], 1);
}

void ccu_Update_GUNA_K1K2Voltage(uint32_t vol)
{
    uint32_t value[2];
    float *pvalue = (float*)&value[0];
    pvalue[0] = ((float)vol);
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1100[0x14] = (value[0]>>0) & 0xFFFF;
    dev->regs_0x1100[0x13] = (value[0]>>16) & 0xFFFF;
    pthread_mutex_unlock(&dev->mutex);
    printd("---[0x114 0x1113]:%04x %04x;  %f\n", dev->regs_0x1100[0x14], dev->regs_0x1100[0x13] ,pvalue[0]);
    modbus_TCP_report_registers(&modbusHW, 0x1113, &dev->regs_0x1100[0x13], 2);
}

void ccu_Update_GUNB_K1K2Voltage(uint32_t vol)
{
    uint32_t value[2];
    float *pvalue = (float*)&value[0];
    pvalue[0] = ((float)vol);
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1D00[0x14] = (value[0]>>0) & 0xFFFF;
    dev->regs_0x1D00[0x13] = (value[0]>>16) & 0xFFFF;
    pthread_mutex_unlock(&dev->mutex);
    printd("---[0x1D14 0x1D13]:%04x %04x;  %f\n", dev->regs_0x1D00[0x14], dev->regs_0x1D00[0x13] ,pvalue[0]);
	modbus_TCP_report_registers(&modbusHW, 0x1D13, &dev->regs_0x1D00[0x13], 2);
}

void ccu_Update_GUNA_ContactorState(uint8_t K1K2)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1100[0x54] = K1K2;
    pthread_mutex_unlock(&dev->mutex);
    // printd("0x1154:%04x\n", dev->regs_0x1154[0]);
    modbus_TCP_report_registers(&modbusHW, 0x1154, &dev->regs_0x1100[0x54], 1);
}

void ccu_Update_GUNB_ContactorState(uint8_t K1K2)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1D00[0x54] = K1K2;
    pthread_mutex_unlock(&dev->mutex);
    // printd("0x1D54:%04x\n", dev->regs_0x1D54[0]);
    modbus_TCP_report_registers(&modbusHW, 0x1D54, &dev->regs_0x1D00[0x54], 1);
}

void ccu_Update_GUNA_ElecLockState(uint8_t state)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1100[0x56] = state;
    pthread_mutex_unlock(&dev->mutex);
    modbus_TCP_report_registers(&modbusHW, 0x1156, &dev->regs_0x1100[0x56], 1);
}

void ccu_Update_GUNB_ElecLockState(uint8_t state)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1D00[0x56] = state;
    pthread_mutex_unlock(&dev->mutex);
    modbus_TCP_report_registers(&modbusHW, 0x1D56, &dev->regs_0x1D00[0x56], 1);
}

void ccu_Update_GUNA_PowerOn(uint8_t state)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1100[0x7E] = state;
    pthread_mutex_unlock(&dev->mutex);
    modbus_TCP_report_registers(&modbusHW, 0x117E, &dev->regs_0x1100[0x7E], 1);
}

void ccu_Update_GUNB_PowerOn(uint8_t state)
{
    modbus_TCP_t* dev = &modbusHW;
    pthread_mutex_lock(&dev->mutex);
    dev->regs_0x1D00[0x7E] = state;
    pthread_mutex_unlock(&dev->mutex);
    modbus_TCP_report_registers(&modbusHW, 0x1D7E, &dev->regs_0x1D00[0x7E], 1);
}

void ccu_Update_GUNA_mac_addr(void)
{
    pthread_mutex_lock(&gooseGUNA.mutex);
    dumpReg2String(gooseGUNA.dst_mac, &modbusHW.regs_0x2100[0x11], 3);
    pthread_mutex_unlock(&gooseGUNA.mutex);
    
    pthread_mutex_lock(&gooseGUNB.mutex);
    dumpReg2String(gooseGUNB.dst_mac, &modbusHW.regs_0x2100[0x11], 3);
    pthread_mutex_unlock(&gooseGUNB.mutex);
	// memcpy(gooseGUNA.dst_mac, &modbusHW.regs_0x2100[0x11], 6);
    // memcpy(gooseGUNB.dst_mac, &modbusHW.regs_0x2100[0x11], 6);
    // printi("---recv HOST macA: %02x %02x %02x %02x %02x %02x\n", gooseGUNA.dst_mac[0], gooseGUNA.dst_mac[1], gooseGUNA.dst_mac[2], gooseGUNA.dst_mac[3], gooseGUNA.dst_mac[4], gooseGUNA.dst_mac[5]);
}

void ccu_Update_GUNB_mac_addr(void)
{
    pthread_mutex_lock(&gooseGUNA.mutex);
    dumpReg2String(gooseGUNA.dst_mac, &modbusHW.regs_0x2D00[0x11], 3);
    pthread_mutex_unlock(&gooseGUNA.mutex);

    pthread_mutex_lock(&gooseGUNB.mutex);
    dumpReg2String(gooseGUNB.dst_mac, &modbusHW.regs_0x2D00[0x11], 3);
    pthread_mutex_unlock(&gooseGUNB.mutex);
    // memcpy(gooseGUNA.dst_mac, &modbusHW.regs_0x2D00[0x11], 6);
	// memcpy(gooseGUNB.dst_mac, &modbusHW.regs_0x2D00[0x11], 6);
    // printi("---recv HOST macB: %02x %02x %02x %02x %02x %02x\n", gooseGUNB.dst_mac[0], gooseGUNB.dst_mac[1], gooseGUNB.dst_mac[2], gooseGUNB.dst_mac[3], gooseGUNB.dst_mac[4], gooseGUNB.dst_mac[5]);

}

void ccu_Update_GUNA_HMAC(void)
{
    pthread_mutex_lock(&gooseGUNA.mutex);
    dumpReg2String(gooseGUNA.HMAC, &modbusHW.regs_0x2100[0x15], 24);
    // memcpy(gooseGUNA.HMAC, &modbusHW.regs_0x2100[0x15], 48);
    pthread_mutex_unlock(&gooseGUNA.mutex);
}

void ccu_Update_GUNB_HMAC(void)
{
    pthread_mutex_lock(&gooseGUNB.mutex);
    dumpReg2String(gooseGUNB.HMAC, &modbusHW.regs_0x2D00[0x15], 24);
    // memcpy(gooseGUNB.HMAC, &modbusHW.regs_0x2D00[0x15], 48);
    pthread_mutex_unlock(&gooseGUNB.mutex);
}

//----------------------------------------------------------------------------------

void ccu_GUNA_PowerRequest(uint16_t type, uint16_t mode, float voltageV, float currentA)
{
    goose_HW_t* dev = (goose_HW_t*)&gooseGUNA;
    pthread_mutex_lock(&dev->mutex);
    dev->power_request.connect_id = modbusHW.regs_0x1100[0xE];
    dev->power_request.serialnumb = 0xFFFF;
    dev->power_request.type = type;
    dev->power_request.mode = mode;
    dev->power_request.requestV = voltageV;
    dev->power_request.requestI = currentA;
    dev->power_request.resv0 = 0xFFFF;
    dev->power_request.resv1 = 0xFFFF;
    pthread_mutex_unlock(&dev->mutex);

    showRegs(&modbusHW);
    printd("type:%hd\n", type);
    printd("mac = %02x%02x%02x%02x%02x%02x\n",gooseGUNA.dst_mac[0],gooseGUNA.dst_mac[1],gooseGUNA.dst_mac[2],\
        gooseGUNA.dst_mac[3],gooseGUNA.dst_mac[4],gooseGUNA.dst_mac[5]);
    goose_HW_PowerRequest(&gooseGUNA);
}

void ccu_GUNB_PowerRequest(uint16_t type, uint16_t mode, float voltageV, float currentA)
{
    goose_HW_t* dev = (goose_HW_t*)&gooseGUNB;
    pthread_mutex_lock(&dev->mutex);
    dev->power_request.connect_id = modbusHW.regs_0x1D00[0xE];
    dev->power_request.serialnumb = 0xFFFF;
    dev->power_request.type = type;
    dev->power_request.mode = mode;
    dev->power_request.requestV = voltageV;
    dev->power_request.requestI = currentA;
    dev->power_request.resv0 = 0xFFFF;
    dev->power_request.resv1 = 0xFFFF;
    pthread_mutex_unlock(&dev->mutex);

    showRegs(&modbusHW);
    printd("type:%hd\n", type);

    goose_HW_PowerRequest(&gooseGUNB);
}

void ccu_GUNA_ShuntdownRequest(uint16_t reason)
{
    goose_HW_t* dev = (goose_HW_t*)&gooseGUNA;
    pthread_mutex_lock(&dev->mutex);
    dev->shutdown_request.connect_id = modbusHW.regs_0x1100[0xE];
    dev->shutdown_request.serialnumb = 0xFFFF;
    dev->shutdown_request.reason = reason;
    dev->shutdown_request.resv0 = 0xFFFF;
    dev->shutdown_request.resv1 = 0xFFFF;
    pthread_mutex_unlock(&dev->mutex);

    goose_HW_ShuntdownRequest(&gooseGUNA);
}

void ccu_GUNB_ShuntdownRequest(uint16_t reason)
{
    goose_HW_t* dev = (goose_HW_t*)&gooseGUNB;
    pthread_mutex_lock(&dev->mutex);
    dev->shutdown_request.connect_id = modbusHW.regs_0x1D00[0xE];
    dev->shutdown_request.serialnumb = 0xFFFF;
    dev->shutdown_request.reason = reason;
    dev->shutdown_request.resv0 = 0xFFFF;
    dev->shutdown_request.resv1 = 0xFFFF;
    pthread_mutex_unlock(&dev->mutex);
    
    goose_HW_ShuntdownRequest(&gooseGUNB);
}

//-----------------------------------------------

void ccu_GUNA_Ctrl_K1(int close)
{
    if (close==0)
    {
        modbusHW.regs_0x1100[0xB] = 1;
        modbus_TCP_report_registers(&modbusHW, 0x110B, &modbusHW.regs_0x1100[0xB], 1);
    }
    ccu_STM32_ControlContactor(&stm32F103, 0, close);
}

void ccu_GUNA_Ctrl_K2(int close)
{
    ccu_STM32_ControlContactor(&stm32F103, 1, close);
}

void ccu_GUNB_Ctrl_K1(int close)
{
    if (close==0)
    {
        modbusHW.regs_0x1D00[0xB] = 1;
        modbus_TCP_report_registers(&modbusHW, 0x1D0B, &modbusHW.regs_0x1D00[0xB], 1);
    }
    ccu_STM32_ControlContactor(&stm32F103, 2, close);
}

void ccu_GUNB_Ctrl_K2(int close)
{
    ccu_STM32_ControlContactor(&stm32F103, 3, close);
}

int ccu_GUNA_Query_K1_State(void)
{
    pthread_mutex_lock(&stm32F103.mutex);
    int ret = stm32F103.state[0];
    pthread_mutex_unlock(&stm32F103.mutex);
    return ret;
}

int ccu_GUNA_Query_K2_State(void)
{
    pthread_mutex_lock(&stm32F103.mutex);
    int ret = stm32F103.state[1];
    pthread_mutex_unlock(&stm32F103.mutex);
    return ret;
}

int ccu_GUNB_Query_K1_State(void)
{
    pthread_mutex_lock(&stm32F103.mutex);
    int ret = stm32F103.state[2];
    pthread_mutex_unlock(&stm32F103.mutex);
    return ret;
}

int ccu_GUNB_Query_K2_State(void)
{
    pthread_mutex_lock(&stm32F103.mutex);
    int ret = stm32F103.state[3];
    pthread_mutex_unlock(&stm32F103.mutex);
    return ret;
}


uint32_t ccu_GUNA_Query_OutputVoltage(void)
{
    pthread_mutex_lock(&stm32F103.mutex);
    uint32_t ret = stm32F103.voltage[0];
    pthread_mutex_unlock(&stm32F103.mutex);
    return ret;
}

uint32_t ccu_GUNB_Query_OutputVoltage(void)
{
    pthread_mutex_lock(&stm32F103.mutex);
    uint32_t ret = stm32F103.voltage[2];
    pthread_mutex_unlock(&stm32F103.mutex);
    return ret;
}

void ccu_GUNA_LED_OnOff(int enable)
{
    // pthread_mutex_lock(&stm32F103.mutex);
    if (enable) {
        if (stm32F103.led[0] == 0) {ccu_STM32_ControlLED(&stm32F103, 6, 1); stm32F103.led[0] = 1;}//Green on
        if (stm32F103.led[1] == 1) {ccu_STM32_ControlLED(&stm32F103, 7, 0); stm32F103.led[1] = 0;}//Red off
    }else{
        if (stm32F103.led[0] == 1) {ccu_STM32_ControlLED(&stm32F103, 6, 0); stm32F103.led[0] = 0;}//Green off
        if (stm32F103.led[1] == 0) {ccu_STM32_ControlLED(&stm32F103, 7, 1); stm32F103.led[1] = 1;}//Red on
    }
    // pthread_mutex_unlock(&stm32F103.mutex);
    // printi("LED:%d,%d,%d,%d\n", stm32F103.led[0], stm32F103.led[1],stm32F103.led[2],stm32F103.led[3]);
}

void ccu_GUNA_LED_Toggle(void)
{
    // pthread_mutex_lock(&stm32F103.mutex);
    if (stm32F103.led[0]==0) { ccu_STM32_ControlLED(&stm32F103, 6, 1); stm32F103.led[0] = 1; }else{ ccu_STM32_ControlLED(&stm32F103, 6, 0); stm32F103.led[0] = 0; }
    if (stm32F103.led[1]==1) { ccu_STM32_ControlLED(&stm32F103, 7, 0); stm32F103.led[1] = 0; }
    // pthread_mutex_unlock(&stm32F103.mutex);
    // printi("LED:%d,%d,%d,%d\n", stm32F103.led[0], stm32F103.led[1],stm32F103.led[2],stm32F103.led[3]);
}

void ccu_GUNB_LED_OnOff(int enable)
{
    // pthread_mutex_lock(&stm32F103.mutex);
    if (enable) {
        if (stm32F103.led[2] == 1) {ccu_STM32_ControlLED(&stm32F103, 8, 0); stm32F103.led[2] = 0;}//Red off
        if (stm32F103.led[3] == 0) {ccu_STM32_ControlLED(&stm32F103, 9, 1); stm32F103.led[3] = 1;}//Green on
    }else{
        if (stm32F103.led[2] == 0) {ccu_STM32_ControlLED(&stm32F103, 8, 1); stm32F103.led[2] = 1;}//Red on
        if (stm32F103.led[3] == 1) {ccu_STM32_ControlLED(&stm32F103, 9, 0); stm32F103.led[3] = 0;}//Green off
    }
    // pthread_mutex_unlock(&stm32F103.mutex);
    // printi("LED:%d,%d,%d,%d\n", stm32F103.led[0], stm32F103.led[1],stm32F103.led[2],stm32F103.led[3]);
}

void ccu_GUNB_LED_Toggle(void)
{
    // pthread_mutex_lock(&stm32F103.mutex);
    if (stm32F103.led[2]==1) { ccu_STM32_ControlLED(&stm32F103, 8, 0); stm32F103.led[2] = 0; }
    if (stm32F103.led[3]==0) { ccu_STM32_ControlLED(&stm32F103, 9, 1); stm32F103.led[3] = 1; }else{ ccu_STM32_ControlLED(&stm32F103, 9, 0); stm32F103.led[3] = 0; }
    // pthread_mutex_unlock(&stm32F103.mutex);
    // printi("LED:%d,%d,%d,%d\n", stm32F103.led[0], stm32F103.led[1],stm32F103.led[2],stm32F103.led[3]);
}

void ccu_GUNA_LED_State(uint8_t state)
{
    pthread_mutex_lock(&stm32F103.mutex);
    stm32F103.GUNA_State = state;
    pthread_mutex_unlock(&stm32F103.mutex);
}

void ccu_GUNB_LED_State(uint8_t state)
{
    pthread_mutex_lock(&stm32F103.mutex);
    stm32F103.GUNB_State = state;
    pthread_mutex_unlock(&stm32F103.mutex);
}

int ccu_TCU_init(void)
{
    modbus_TCP_init(&modbusHW, tcu_config.modbus_hw.serverip, tcu_config.modbus_hw.port, tcu_config.modbus_hw.enable_ssl, tcu_config.tcu_id);
    goose_HW_init(&gooseGUNA, tcu_config.ifname, tcu_config.tcu_id*2+1);
    goose_HW_init(&gooseGUNB, tcu_config.ifname, tcu_config.tcu_id*2+2);

    dumpString2Reg(&modbusHW.regs_0x1100[0x4D], gooseGUNA.src_mac, 6);
    dumpString2Reg(&modbusHW.regs_0x1D00[0x4D], gooseGUNB.src_mac, 6);

    ccu_STM32_init(&stm32F103, "can0", 500000, 1);
    ccu_AST_init(&ccuASTXH, tcu_config.ast_tty);

    return 0;
}

void ccu_TCU_run(int quit)
{
    // while (quit==0)
    // {
    //     // ccu_AST_loop(&ccuASTXH);
    //     // modbus_TCP_loop(&modbusHW);
    //     // goose_HW_loop(&gooseGUNA);
    // }

    tc_thread_init(&threadTCU[0], modbus_TCP_loop, &modbusHW);
    tc_thread_init(&threadTCU[1], goose_HW_loop,   &gooseGUNA);
    tc_thread_init(&threadTCU[2], goose_HW_loop,   &gooseGUNB);
    tc_thread_init(&threadTCU[3], ccu_STM32_loop,  &stm32F103);
    tc_thread_init(&threadTCU[4], ccu_AST_loop,    &ccuASTXH);
    while(quit==0);
}

void ccu_TCU_exit(void)
{
    stm32F103.quit = 1;
    gooseGUNB.quit = 1;
    gooseGUNA.quit = 1;
    ccuASTXH.quit = 1;
    modbusHW.quit = 1;

    tc_thread_exit(&threadTCU[4]);
    tc_thread_exit(&threadTCU[3]);
    tc_thread_exit(&threadTCU[2]);
    tc_thread_exit(&threadTCU[1]);
    tc_thread_exit(&threadTCU[0]);

    ccu_AST_exit(&ccuASTXH);
    ccu_STM32_exit(&stm32F103);
    goose_HW_exit(&gooseGUNB);
    goose_HW_exit(&gooseGUNA);
    modbus_TCP_exit(&modbusHW);
}