#include <AP_HAL/AP_HAL.h>
#include "AP_BattMonitor_Serial_BattGo.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS.h>
#define MAXSONAR_SERIAL_LV_BAUD_RATE 115200
extern const AP_HAL::HAL &hal;

AP_BattMonitor_Serial_BattGo::AP_BattMonitor_Serial_BattGo(AP_BattMonitor &mon,
                                                           AP_BattMonitor::BattMonitor_State &mon_state,
                                                           AP_BattMonitor_Params &params,
                                                           uint8_t serial_instance) : AP_BattMonitor_Backend(mon, mon_state, params)
{
  
    const AP_SerialManager &serial_manager = AP::serialmanager();
    uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Battery, serial_instance);
    if (uart != nullptr)
    {
        uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Battery, serial_instance));   
    }
      last_rev_batt_time = 0;
};

/* 
   detect if a battGo  is connected. We'll detect by
   trying to take a reading on Serial. If we get a result the sensor is
   there.
*/
bool AP_BattMonitor_Serial_BattGo::detect(uint8_t serial_instance) //how to used
{
    return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Battery, serial_instance) != nullptr;
}

void AP_BattMonitor_Serial_BattGo::init(void)
{
    if (uart != nullptr)
    {
        requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA);
    //    requestBattData(BATTGO_CMD_REQUEST_INFO);
    //    requestBattData(BATTGO_CMD_REQUEST_HISTORY);
    }
}
bool isparse = false;
bool AP_BattMonitor_Serial_BattGo::get_reading() //TODO need to Refactor
{

    if (uart == nullptr)
    {
        return false;
    }
    int16_t numc;
    numc = uart->available();
    uint8_t data;
   // if (numc == 0)
  //      gcs().send_text(MAV_SEVERITY_INFO, "uasart num:%d", numc);
    isparse = false;
    for (int16_t i = 0; i < numc; i++)
    {
        data = uart->read();
        rxProcess(data);
        if (isparse)
        {
            isparse = true;
            return true;
        }
    }
    return isparse;
}

void AP_BattMonitor_Serial_BattGo::requestBattData(uint8_t data)
{
    if (uart == nullptr)
    {
        return ;
    }
    //发送data主要   0x40  0x42  0x44  地址0x21  及计算crc 均不会出现0x55，故不做0x55处理
    uint8_t send_data[13];
    send_data[0] = 0x55;
    send_data[1] = 0x12;
    send_data[2] = 0x08;
    send_data[3] = data;
    send_data[4] = 0;
    send_data[5] = 0;
    send_data[6] = 0;
    send_data[7] = 0;
    send_data[8] = 0;
    send_data[9] = 0;
    send_data[10] = 0;
    uint16_t crc = send_data[1] + send_data[2] + send_data[3]; //Notice :other data  is  0 ,so no need to  add to culcalate crc
    send_data[11] = crc & 0xff;                                //64
    send_data[12] = crc >> 8 & 0xff;                           //00
    if (uart->txspace() > sizeof(send_data))
    {
        uart->write(&send_data[0], sizeof(send_data));
    }
}

mavlink_battgo_info_t battgo_info_t;
mavlink_battgo_history_t battgo_history_t;
bool AP_BattMonitor_Serial_BattGo::parseBattGo()
{
     uint8_t offset =0;
    if (data_buf[0] == 0x43)
    {   //电池实时数据
        uint32_t tnow = AP_HAL::micros();
        uint32_t dt = tnow - _interim_state.last_time_micros;
        _interim_state.voltage = 0;
        for (uint8_t i = 0; i < 6; i++)
        {
            uint16_t valtemp = ((data_buf[4 + 2 * i] << 8) & 0xff00) + data_buf[3 + 2 * i];
            _interim_state.cell_voltages.cells[i] = valtemp;
            _interim_state.voltage += valtemp / 1000.0;
            _has_cell_voltages = true;
        }
        _interim_state.temperature = (int8_t)data_buf[15];
        _interim_state.temperature_time = AP_HAL::millis();
        _interim_state.current_amps = (data_buf[16] + (data_buf[17] << 8) + (data_buf[18] << 16) + (data_buf[19] << 24)) / 1000.0;
        // update total current drawn since startup
        if (_interim_state.last_time_micros != 0 && dt < 2000000)
        {
            // .0002778 is 1/3600 (conversion to hours)
            float mah = (float)((double)_interim_state.current_amps * (double)dt * (double)0.0000002778f);
            _interim_state.consumed_mah += mah;
            _interim_state.consumed_wh += 0.001f * mah * _interim_state.voltage;
        }
       
         battgo_history_t.batt_cycled_time = (data_buf[25]<<8)+data_buf[24] ;
         battgo_history_t.batt_abnormal_time =  (data_buf[27]<<8)+data_buf[26] ;
         battgo_history_t.temp_abnormal_time = (data_buf[29]<<8)+data_buf[28] ;
        battgo_history_t.over_vol_abnormal_time=  (data_buf[31]<<8)+data_buf[30] ;
         offset = 32;
         battgo_history_t.under_vol_abnormal_time = (data_buf[offset+1]<<8)+data_buf[offset] ;
         offset =  offset+2;
         battgo_history_t.soft_crashes_time = data_buf[offset];
         offset++;
         battgo_history_t.charging_current_setted  =  (data_buf[offset+3] + (data_buf[offset+2] << 8) + (data_buf[offset+1] << 16) + (data_buf[offset] << 24)) ;
         offset = offset+4;
         battgo_history_t.storage_vol_setted =  (data_buf[offset+1]<<8)+data_buf[offset] ;
         offset+=2;
         battgo_history_t.charging_full_vol_setted = (data_buf[offset+1]<<8)+data_buf[offset] ;
         offset+=2;
         battgo_history_t.self_discharge_times_setted = data_buf[offset];
         //battgo_history_t.set
        // record time
        _interim_state.last_time_micros = tnow;
        _interim_state.healthy = true;

     

        return true;
    }
    else if (data_buf[0] == 0x41)
    {
        //电池参数
        //gcs().send_text(MAV_SEVERITY_INFO, "into  battgo  0x41  battgo info");
        battgo_info_t.channal_num = data_buf[1];
        battgo_info_t.connect_status = data_buf[2];
        battgo_info_t.sub_hardware_ver = data_buf[3];
        battgo_info_t.main_hardware_ver = data_buf[4];
        battgo_info_t.sub_software_ver = data_buf[5];
        battgo_info_t.main_software_ver = data_buf[6];
        for (int i = 0; i < 8; i++)
        {
            battgo_info_t.equipment_id[i] = data_buf[7 + i];
        }
        for (int i = 0; i < 10; i++)
        {
            battgo_info_t.slave_id[i] = data_buf[15 + i];
        }
        battgo_info_t.station_id = data_buf[25];
        battgo_info_t.customer_id = data_buf[27] << 8 | data_buf[26];
        battgo_info_t.battery_id = data_buf[31] << 24 | data_buf[30] << 16 | data_buf[29] << 8 | data_buf[28];

        uint8_t sec = data_buf[32] & 0x3f;
        uint8_t min = ((data_buf[32] & 0xc0) >> 6) | ((data_buf[33] & 0x0F) << 2);
        uint8_t hour = ((data_buf[33] & 0xF0) >> 4) | ((data_buf[34] & 0x01) << 5);
        uint8_t day = (data_buf[34] & 0x3e) >> 1;
        uint8_t month = ((data_buf[34] & 0xc0) >> 6) | ((data_buf[35] & 0x03) << 2);
        uint16_t year = ((data_buf[35] & 0xFC) >> 2) + 2017;

        battgo_info_t.production_time_year = year;
        battgo_info_t.production_time_mon = month;
        battgo_info_t.production_time_mday = day;
        battgo_info_t.production_time_hour = hour;
        battgo_info_t.production_time_min = min;
        battgo_info_t.production_time_sec = sec;
        for (int i = 0; i < 16; i++)
        {
            battgo_info_t.battery_brand[i] = data_buf[i + 36];
        }
        battgo_info_t.type = data_buf[52];
        battgo_info_t.core_amount = data_buf[53];
        battgo_info_t.over_discharge_vol = (data_buf[55] << 8) | data_buf[54];
        battgo_info_t.warning_vol = (data_buf[57] << 8) | data_buf[56];
        battgo_info_t.storage_vol = (data_buf[59] << 8) | data_buf[58];
        battgo_info_t.full_charge_vol = (data_buf[61] << 8) | data_buf[60];
        battgo_info_t.battery_capacity = (data_buf[65] << 24) | (data_buf[64] << 16) | (data_buf[63] << 8) | data_buf[62];
        battgo_info_t.charge_c_value = (data_buf[67] << 8) | data_buf[66];
        battgo_info_t.discharge_c_value = (data_buf[69] << 8) | data_buf[68];
        battgo_info_t.temp_using_min = data_buf[70];
        battgo_info_t.temp_using_max = data_buf[71];
        battgo_info_t.temp_store_nin = data_buf[72];
        battgo_info_t.temp_store_max = data_buf[73];
        battgo_info_t.enable_auto_store = data_buf[74];
        battgo_info_t.request_tag = 0; //reset the request
        gcs().update_serial_battgo_info(&battgo_info_t);
        return true;
    }
    else if (data_buf[0] == 0x45) //
    {
       // gcs().send_text(MAV_SEVERITY_INFO, "into  battgo  0x45  battgo history");
        battgo_history_t.channal_num = data_buf[1];
        battgo_history_t.channal_num = data_buf[2];
        battgo_history_t.last_charge_cap = (data_buf[6] << 24) | (data_buf[5] << 16) | (data_buf[4] << 8) | data_buf[3];

        for (int i = 0; i < 6; i++)
        {
            battgo_history_t.battery_core_vol[i] = (data_buf[7 + 2 * i + 1] << 8) | data_buf[7 + 2 * i];
        }

        for (int i = 0; i < 6; i++)
        {
            battgo_history_t.Internal_resistance[i] = (data_buf[19 + 2 * i + 1] << 8) | data_buf[19 + 2 * i];
        }
        battgo_history_t.charge_state = data_buf[31];

        gcs().update_serial_battgo_history(&battgo_history_t);
        return true;
    }
    return false;
}

void AP_BattMonitor_Serial_BattGo::read()
{
    requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA);
    uint32_t tnow = AP_HAL::micros();
    static uint32_t last_prt;
    if (get_reading())
    {
        if (parseBattGo())
        {     
             last_rev_batt_time =tnow ;
            // timeout after 5 seconds
            if ((tnow - _interim_state.last_time_micros) > AP_BATTMONITOR_SERIAL_TIMEOUT_MICROS)
            {
                _interim_state.healthy = false;
            }
            
            // Copy over relevant states over to main state
            //   WITH_SEMAPHORE(_sem_battmon);

            _state.cell_voltages = _interim_state.cell_voltages;
            _state.voltage = _interim_state.voltage;
            _state.current_amps = _interim_state.current_amps;
            _state.consumed_mah = _interim_state.consumed_mah;
            _state.consumed_wh = _interim_state.consumed_wh;
            _state.last_time_micros = _interim_state.last_time_micros;
            //...
            _state.temperature = _interim_state.temperature;
            _state.temperature_time = _interim_state.temperature_time;
            //...
            _state.healthy = _interim_state.healthy;
        }else{            
            if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){
                      _interim_state.healthy = false;
                      gcs().send_text(MAV_SEVERITY_WARNING,"BATT_ERR:已经%d秒没有接收到智能电池的串口数据！",(tnow-last_rev_batt_time)/1000000);
                last_prt = tnow;
            }
        }
    }else{
            if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){
                      _interim_state.healthy = false;
                      gcs().send_text(MAV_SEVERITY_WARNING,"BATT_ERR:已经%d秒没有接收到智能电池的串口数据！",(tnow-last_rev_batt_time)/1000000);
                last_prt = tnow;
            }
    }
}

void AP_BattMonitor_Serial_BattGo::notice_to_send_battgo_param()
{
    gcs().send_message(MSG_BATTGAO_INFO);
}

void AP_BattMonitor_Serial_BattGo::request_info()
{
    requestBattData(BATTGO_CMD_REQUEST_INFO);
}

void AP_BattMonitor_Serial_BattGo::request_history()
{
    requestBattData(BATTGO_CMD_REQUEST_HISTORY);
}

void AP_BattMonitor_Serial_BattGo::rxProcess(uint8_t data)
{
    if (data == 0x55)
    {
        m_SyncByteCount++;
        if (m_SyncByteCount & 0x01)
            return;
    }
    else
    {
        if (m_SyncByteCount & 0x01)
            m_eRxStatus = rsWaitAddres;
        m_SyncByteCount = 0;
    }

    switch (m_eRxStatus)
    {
    case rsWaitSync:
        break;

    case rsWaitAddres:
    {
        if (data != 0x21)
        {
            m_eRxStatus = rsWaitSync;
            break;
        }
        check_sum = data;
        m_eRxStatus = rsWaitLength;
    }
    break;

    case rsWaitLength:
    {
        if (data < 2)
        {
            m_eRxStatus = rsWaitSync;
            break;
        }
        m_u08RXCnt = 0;
        payload_length = data;
        check_sum += data;
        m_eRxStatus = rsWaitData;
    }
    break;

    case rsWaitData:
    {
        data_buf[m_u08RXCnt] = data;
        m_u08RXCnt++;
        check_sum += data;
        if (m_u08RXCnt >= payload_length)
            m_eRxStatus = rsWaitChkSumL;
    }
    break;

    case rsWaitChkSumL:
    {

        if (data == (check_sum & 0xFF))
            m_eRxStatus = rsWaitChkSumH;
        else
            m_eRxStatus = rsWaitSync;
    }
    break;

    case rsWaitChkSumH:
    {
        if (data == ((check_sum >> 8) & 0xFF))
        {
            //m_bMessege = TRUE;
            isparse = true;
        }
        m_eRxStatus = rsWaitSync;
    }
    break;

    default:
    {
        m_eRxStatus = rsWaitSync;
    }
    break;
    }
}