#include "kit_time.h"
#include "kit_data.h"
#include "kit_debug.h"

#include "iso_check.h"
#include "gpio_manager.h"
#include "eeprom_manager.h"
#include "signal_manager.h"
#include "hv_adc_manager.h"
#include "run_status.h"

#define ISO_SAMPLE_CNT          (20u)
#define ISO_RES_BUF_CNT         (3u)       //sfj 原本6 暂改3
#define DIVIDER_RES_VALUE       (5100u)     // 档位kΩ 半桥电阻
#define ISO_STABLE_AD_RATE      (3u)
#define ISO_SAMPLE_TIME_MAX     (10000u)  //sfj 7.25 原本60000 因为采样平均值通不过
#define ISO_SAMPLE_TIME_MIN     (3000u)

typedef enum
{
    kIsoStep_None = 0,
	kIsoStep_TotalVolt,
	kIsoStep_IsoVolt,
    kIsoStep_PosVolt,
	kIsoStep_NegVolt,
    kIsoStep_End,         
}IsoStep;

typedef struct 
{
	int32_t v0;
	int32_t v1;
	int32_t v2;
	int32_t v3;
    uint16_t iso_bat_rate;
    uint8_t sample_pos[kIsoVolt_End];
    uint8_t res_buf_pos;
    
    IsoCtrl ctrl;
    IsoCtrl bsu_ctrl;
	IsoStep step;
    uint16_t dly;
    int32_t ad_buf[kIsoVolt_End][ISO_SAMPLE_CNT];
    uint16_t pos_res_buf[ISO_SAMPLE_CNT];
    uint16_t neg_res_buf[ISO_SAMPLE_CNT];
    uint16_t data[kIsoData_End];
}IsoItem;

IsoItem iso_item;

#define ISO_S1_ON()  drv_gpio_set_pin_status(kGpioType_IsoS1, kGpioStatus_High)
#define ISO_S1_OFF() drv_gpio_set_pin_status(kGpioType_IsoS1, kGpioStatus_Low)
#define ISO_S2_ON()  drv_gpio_set_pin_status(kGpioType_IsoS2, kGpioStatus_High)
#define ISO_S2_OFF() drv_gpio_set_pin_status(kGpioType_IsoS2, kGpioStatus_Low)
#define ISO_S3_ON()  drv_gpio_set_pin_status(kGpioType_IsoS3, kGpioStatus_High)
#define ISO_S3_OFF() drv_gpio_set_pin_status(kGpioType_IsoS3, kGpioStatus_Low)

uint16_t bms_get_iso_data(IsoData idx)
{
    uint16_t tmp = 0;
    if(idx < kIsoData_End)
    {
        tmp = iso_item.data[idx];
    }
    return tmp;
}

void bms_set_iso_ctrl(IsoCtrl ctrl)
{
    if(ctrl < kIsoCtrl_End)
    {
        iso_item.ctrl = ctrl;
    }
}

void bms_set_iso_volt_ad(IsoVolt idx, int32_t ad)
{
    if(idx < kIsoVolt_End)
    {
        iso_item.ad_buf[idx][iso_item.sample_pos[idx]] = ad;
        if(++iso_item.sample_pos[idx] >= ISO_SAMPLE_CNT)
        {
            iso_item.sample_pos[idx] = 0;
        }
    }
}


//aim_rate 是最大最小差值和最小值比例
static bool is_volt_ad_stable(int32_t *buf, uint16_t len, uint16_t aim_rate)
{
	bool res = false;
    int32_t i, max, min;

    max = min = buf[0];
    for(i = 0; i < len; i++)
    {
        if(max < buf[i])
        {
            max = buf[i];
        }
        else if(min > buf[i])
        {
            min = buf[i];
        }
    }
    //KIT_DEBUG_PRINTF("iso ad %d %d %d\r\n", max, min, (max - min) * 10000 / min);
    if((min > 0) && (((int64_t)(max - min) * 10000 / min) < aim_rate))
    {
        res = true;
    }
	return res;
}

/********************************绝缘情况分析********************************************
1.当绝缘无穷大时，理论上V0 = V1
2.当S2闭合时，正极处并上电阻导致分压减小，进而V2 > V1
3.当S3闭合时，负极处并上电阻导致分压减小，进而V3 < V1
****************************************************************************************/
void insu_sample_calc_insu_res(void)
{
    uint16_t total_volt;
    float tmp, v0v1, v0v2, v0v3, v1v2, v1v3;
    
    if(iso_item.v1 > 10)
    {
        if (iso_item.v2 <= iso_item.v1)
        {
            KIT_DEBUG_PRINTF("iso err v1 %d v2 %d\r\n", iso_item.v1, iso_item.v2);
            iso_item.v2 = iso_item.v1 + 1;
        }
        
        if (iso_item.v3 >= iso_item.v1)
        {
            KIT_DEBUG_PRINTF("iso err v1 %d v3 %d\r\n", iso_item.v1, iso_item.v3);
            iso_item.v3 = iso_item.v1 - 1;
        }
        
        v0v1 = (float)iso_item.v0 * iso_item.v1;
        v0v2 = (float)iso_item.v0 * iso_item.v2;
        v0v3 = (float)iso_item.v0 * iso_item.v3;
        v1v2 = (float)iso_item.v1 * iso_item.v2;
        v1v3 = (float)iso_item.v1 * iso_item.v3;
        total_volt = bms_get_high_volt(kHvType_Bat);
        
        tmp = 4 * v0v1 - 2 * v0v2 - v1v2;
        if(tmp > 0)
        {
            tmp = (v0v2 - v0v1) * 2 * DIVIDER_RES_VALUE / tmp;
            //iso_item.data[kIsoData_PosRes] = tmp;
            //iso_item.data[kIsoData_PosValue] = tmp * 10000 / total_volt;
            iso_item.pos_res_buf[iso_item.res_buf_pos] = tmp;
        }
        else
        {
            KIT_DEBUG_PRINTF("iso pos neg\r\n");
        }
        
        tmp = 4 * v0v3 - 2 * v0v1 - v1v3;
        if(tmp > 0)
        {
            tmp = (v0v1 - v0v3) * 2 * DIVIDER_RES_VALUE / tmp;
            //iso_item.data[kIsoData_NegRes] = tmp;
            //iso_item.data[kIsoData_NegValue] = tmp * 10000 / total_volt;
            iso_item.neg_res_buf[iso_item.res_buf_pos] = tmp;          
        }
        else
        {
            KIT_DEBUG_PRINTF("iso neg neg\r\n");
        }
        
//		iso_item.data[kIsoData_Res] = (iso_item.data[kIsoData_PosRes] > iso_item.data[kIsoData_NegRes]) ? iso_item.data[kIsoData_NegRes] : iso_item.data[kIsoData_PosRes];
//		iso_item.data[kIsoData_Value] = (iso_item.data[kIsoData_PosValue] > iso_item.data[kIsoData_NegValue]) ? iso_item.data[kIsoData_NegValue] : iso_item.data[kIsoData_PosValue];
        if(++iso_item.res_buf_pos >= ISO_RES_BUF_CNT)
        {
            iso_item.res_buf_pos = 0;
        
			iso_item.data[kIsoData_PosRes] = kit_get_unsign_avg_filter_max_min(iso_item.pos_res_buf, ISO_RES_BUF_CNT);
			iso_item.data[kIsoData_NegRes] = kit_get_unsign_avg_filter_max_min(iso_item.neg_res_buf, ISO_RES_BUF_CNT);
			
			tmp = (uint32_t)iso_item.data[kIsoData_PosRes] * 10000 / total_volt;
			iso_item.data[kIsoData_PosValue] = (tmp > UINT16_MAX) ? UINT16_MAX : tmp;
			
			tmp = (uint32_t)iso_item.data[kIsoData_NegRes] * 10000 / total_volt;
			iso_item.data[kIsoData_NegValue] = (tmp > UINT16_MAX) ? UINT16_MAX : tmp;

			iso_item.data[kIsoData_Res] = (iso_item.data[kIsoData_PosRes] > iso_item.data[kIsoData_NegRes]) ? iso_item.data[kIsoData_NegRes] : iso_item.data[kIsoData_PosRes];
			iso_item.data[kIsoData_Value] = (iso_item.data[kIsoData_PosValue] > iso_item.data[kIsoData_NegValue]) ? iso_item.data[kIsoData_NegValue] : iso_item.data[kIsoData_PosValue];
			
			KIT_DEBUG_PRINTF("iso %d %d %d %d\r\n", iso_item.v0, iso_item.v1, iso_item.v2, iso_item.v3);
			KIT_DEBUG_PRINTF("iso pos %d neg %d\r\n", iso_item.data[kIsoData_PosRes], iso_item.data[kIsoData_NegRes]);
		}        
    }
}

void bms_poll_iso(uint32_t base_time)
{
    bool is_iso_enable = true;
    iso_item.dly += base_time;
    if((iso_item.ctrl == kIsoCtrl_Disable) || (iso_item.bsu_ctrl == kIsoCtrl_Disable) 
    || (iso_item.ctrl == kIsoCtrl_SelfCheck && bms_get_circuit_status(kCircuitType_Main) == kCircuitStatus_On))
    {
        iso_item.dly = 0;
        is_iso_enable = false;
        iso_item.step = kIsoStep_None;
        kit_set_buf(iso_item.data, kIsoData_End * 2, 0xFF);
    }
    bms_set_signal(kSignalIdx_BsuCtrlIso, (SignalStatus)is_iso_enable);
	switch(iso_item.step)
	{
		case kIsoStep_None:
			ISO_S1_OFF();
            ISO_S2_OFF();
			ISO_S3_OFF();
            if(is_iso_enable == true)
            {
                iso_item.dly = 0;
                iso_item.step = kIsoStep_TotalVolt;
                KIT_DEBUG_PRINTF("iso tv \r\n");
            }
			break;
        case kIsoStep_TotalVolt:
            if ((is_volt_ad_stable(iso_item.ad_buf[kIsoVolt_TotalVolt], ISO_SAMPLE_CNT, ISO_STABLE_AD_RATE) == true) || (iso_item.dly >= ISO_SAMPLE_TIME_MAX))
            {                
                KIT_DEBUG_PRINTF("iso tv %d\r\n", iso_item.dly);
                ISO_S1_ON();
                iso_item.dly = 0;
                iso_item.step = kIsoStep_IsoVolt;
                iso_item.v0 = (int64_t)kit_get_int32_avg_filter_max_min(iso_item.ad_buf[kIsoVolt_TotalVolt], ISO_SAMPLE_CNT) * iso_item.iso_bat_rate / 1000;
            }
			break;
        case kIsoStep_IsoVolt:
            if (((is_volt_ad_stable(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT, ISO_STABLE_AD_RATE) == true) || (iso_item.dly >= ISO_SAMPLE_TIME_MAX))
             && (iso_item.dly >= ISO_SAMPLE_TIME_MIN))
            {                
                KIT_DEBUG_PRINTF("iso iv %d\r\n", iso_item.dly);
                ISO_S2_ON();
                iso_item.dly = 0;
                iso_item.step = kIsoStep_PosVolt;
                iso_item.v1 = kit_get_int32_avg_filter_max_min(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT);
            }
			break;
		case kIsoStep_PosVolt:
            if (((is_volt_ad_stable(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT, ISO_STABLE_AD_RATE) == true) || (iso_item.dly >= ISO_SAMPLE_TIME_MAX))
             && (iso_item.dly >= ISO_SAMPLE_TIME_MIN))
            {                
                KIT_DEBUG_PRINTF("iso pv %d\r\n", iso_item.dly);
                ISO_S2_OFF();
                ISO_S3_ON();
                iso_item.dly = 0;
                iso_item.step = kIsoStep_NegVolt;
                iso_item.v2 = kit_get_int32_avg_filter_max_min(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT);
            }
			break;

		case kIsoStep_NegVolt:
            if (((is_volt_ad_stable(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT, ISO_STABLE_AD_RATE) == true) || (iso_item.dly >= ISO_SAMPLE_TIME_MAX))
             && (iso_item.dly >= ISO_SAMPLE_TIME_MIN))
            {              
                KIT_DEBUG_PRINTF("iso nv %d\r\n", iso_item.dly);
                ISO_S3_OFF();
                iso_item.dly = 0;
                iso_item.v3 = kit_get_int32_avg_filter_max_min(iso_item.ad_buf[kIsoVolt_Other], ISO_SAMPLE_CNT);  
                insu_sample_calc_insu_res();
                if((iso_item.ctrl == kIsoCtrl_Once) || (iso_item.bsu_ctrl == kIsoCtrl_Disable))
                {
                    iso_item.step = kIsoStep_None;
                    iso_item.ctrl = kIsoCtrl_Disable;
                }
                else
                {
                    //由于总压有独立通道采集，不需要关闭S1
                    iso_item.step = kIsoStep_TotalVolt;
                }
            }
			break;

		default:
			break;
	}
}

void bms_set_iso_bsu_ctrl(IsoCtrl ctrl)
{
    iso_item.bsu_ctrl = ctrl;
}

void bms_init_iso(void)
{
    iso_item.iso_bat_rate = 1000;
    
    kit_set_buf(iso_item.data, kIsoData_End * 2, 0xFF);
    iso_item.ctrl = (IsoCtrl)get_eeprom_data(kEep_InsuPeriod_Enable, kEepromDataType_Low);
}
