#include "Underwater_Sonar.h"

#include <AP_Logger/AP_Logger.h>

#define MY_USE_DEBUG 1
#if MY_USE_DEBUG

# define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_CRITICAL,fmt, ## args)
#else 
//  
 # define Debug(fmt, args ...) 
#endif

Underwater_Sonar::Underwater_Sonar(void):
    ucCRCHi(0),
    ucCRCLo(0),
    iIndex(0)
{
    _singleton = this;
}

bool Underwater_Sonar::init(const AP_SerialManager& serial_manager)
{
	_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Underwater_Sonar,0);
	if (_uart != nullptr) {
	        _uart->begin(4800U);

            return true;
	        // _uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
	}
    return false;    
}

bool Underwater_Sonar::read(void)
{
    if (_uart == nullptr) {
        return false;
    }
    // Debug("read");
    bool ret = false;


    uint32_t nbytes = _uart->available();
    uint32_t data_len = nbytes;
    while(nbytes-->0)
    {
        uint8_t temp = _uart->read();
        sonar_data[data_len-nbytes] = temp;
        // Debug("read %d:%d",nbytes,temp);
        if(_decode(temp)){

            // write(&temp,1);
            for(int j = 0; j < 8; j++){
                // radar_data = _sonar[j];
                // memcpy(*radar_data,_sonar[j],sizeof(_sonar[j]));
                radar_data[j] = atof(_term[j]);
                
                // cout << j << " : " << radar_data[j] << endl;
            }
            // data_index +=1;
            //     Debug("%d: %d,%.2f,%d,%d,%d,%.2f,%.d,%d ",data_index,(int)radar_data[0],radar_data[1],(int)radar_data[2],(int)radar_data[3], \
            //                         (int)radar_data[4],radar_data[5],(int)radar_data[6],(int)radar_data[7]);
            ret = true;
        }
    }
    // Debug("read %s",sonar_data);
    return ret;
}

bool Underwater_Sonar::_decode(char c)
{
    bool valid_sentence = false;
    // _term[_term_num][_term_offset];  
    switch (c) {
        case '\r':
        case '\n':
            _term_offset = 0;
            _term_num = 0;
            valid_sentence = true;
            return valid_sentence;
        case ' ':
            _term_num+=1;
            _term_offset = 0;
            return valid_sentence;

    }
    if (_term_offset < sizeof(_term[_term_num]) - 1)
        _term[_term_num][_term_offset++] = c;
    return valid_sentence;
}


void Underwater_Sonar::write(const uint8_t *data, uint16_t len)
{

    if (_uart == nullptr) {
        return;
    }
    if (_uart->txspace() > len) {
        _uart->write(data, len);
    } else {
        // Debug("FLOW: Not enough TXSPACE");
    }
}

void Underwater_Sonar::update(const AP_SerialManager& serial_manager, const struct Location &_loc){
    if (_uart == nullptr) {
        _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Underwater_Sonar,0);
        if (_uart != nullptr) {
                _uart->begin(4800U);
            Debug("---------Underwater_Sonar: ok\n");
        }
    }
    if(read()){
        AP_Logger *logger = AP_Logger::get_singleton();
        if (logger == nullptr) {
            Debug("log error");
            return;
        }
        logger->Write_Underwater_Sonar(_loc);
        
        Debug("%d: %d,%.2f,%d,%d,%d,%.2f,%.d,%d ",data_index,(int)radar_data[0],radar_data[1],(int)radar_data[2],(int)radar_data[3], \
                            (int)radar_data[4],radar_data[5],(int)radar_data[6],(int)radar_data[7]);
        
    }

}
// singleton instance
Underwater_Sonar *Underwater_Sonar::_singleton;

namespace AP {

    Underwater_Sonar &underwater_sonar()
    {
        return *Underwater_Sonar::get_singleton();
    }

}