/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#include <AP_HAL/AP_HAL.h>
#include "AP_EFI_Serial_MS.h"

#if HAL_EFI_ENABLED
#include <AP_Math/AP_Math.h>
#include <AP_SerialManager/AP_SerialManager.h>

extern const AP_HAL::HAL &hal;

AP_EFI_Serial_MS::AP_EFI_Serial_MS(AP_EFI &_frontend):
    AP_EFI_Backend(_frontend)
{
    internal_state.estimated_consumed_fuel_volume_cm3 = 0; // Just to be sure
    port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_EFI, 0);
}


void AP_EFI_Serial_MS::update()
{
    if (!port) {
        return;
    }

    uint32_t now = AP_HAL::millis();

    const uint32_t expected_bytes = 2 + (RT_LAST_OFFSET - RT_FIRST_OFFSET) + 4;
    if (port->available() >= expected_bytes && read_incoming_realtime_data()) {
        last_response_ms = now;
        copy_to_frontend();
    }

    if (port->available() == 0 || now - last_response_ms > 200) {
        port->discard_input();
        // Request an update from the realtime table (7).
        // The data we need start at offset 6 and ends at 129
        send_request(7, RT_FIRST_OFFSET, RT_LAST_OFFSET);
    }
}

bool AP_EFI_Serial_MS::read_incoming_realtime_data() 
{
    // Data is parsed directly from the buffer, otherwise we would need to allocate
    // several hundred bytes for the entire realtime data table or request every
    // value individiually
    uint16_t message_length = 0;

    // reset checksum before reading new data
    checksum = 0;
    
    // Message length field begins the message (16 bits, excluded from CRC calculation)
    // Message length value excludes the message length and CRC bytes 
    message_length = port->read() << 8;
    message_length += port->read();

    if (message_length >= 256) {
        // don't process invalid messages
        // hal.console->printf("message_length: %u\n", message_length);
        return false;
    }

    // Response Flag (see "response_codes" enum)
    response_flag = read_byte_CRC32();
    if (response_flag != RESPONSE_WRITE_OK) {
        // abort read if we did not receive the correct response code;
        return false;
    }
    
    // Iterate over the payload bytes 
    for ( uint8_t offset=RT_FIRST_OFFSET; offset < (RT_FIRST_OFFSET + message_length - 1); offset++) {
        uint8_t data = read_byte_CRC32();
        float temp_float;
        switch (offset) {
            case PW1_MSB:
                internal_state.cylinder_status[0].injection_time_ms = (float)((data << 8) + read_byte_CRC32())/1000.0f;
                offset++;  // increment the counter because we read a byte in the previous line
                break;
            case RPM_MSB:
                // Read 16 bit RPM
                internal_state.engine_speed_rpm = (data << 8) + read_byte_CRC32();
                offset++;
                break;
            case ADVANCE_MSB:
                internal_state.cylinder_status[0].ignition_timing_deg = (float)((data << 8) + read_byte_CRC32())/10.0f;
                offset++;
                break;
            case ENGINE_BM:
                break;
            case BAROMETER_MSB:
                internal_state.atmospheric_pressure_kpa = (float)((data << 8) + read_byte_CRC32())/10.0f;
                offset++;
                break;
            case MAP_MSB:
                internal_state.intake_manifold_pressure_kpa = (float)((data << 8) + read_byte_CRC32())/10.0f;
                offset++;
                break;
            case MAT_MSB:
                temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f;
                offset++;
                internal_state.intake_manifold_temperature = degF_to_Kelvin(temp_float);
                break;
            case CHT_MSB:
                temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f;
                offset++;
                internal_state.cylinder_status[0].cylinder_head_temperature = degF_to_Kelvin(temp_float);
                break;
            case TPS_MSB:
                temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f;
                offset++;
                internal_state.throttle_position_percent = roundf(temp_float);
                break;
            case AFR1_MSB:
                temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f;
                offset++;
                internal_state.cylinder_status[0].lambda_coefficient = temp_float;
                break;
            case DWELL_MSB:
                temp_float = (float)((data << 8) + read_byte_CRC32())/10.0f;
                internal_state.spark_dwell_time_ms = temp_float;
                offset++;
                break;
            case LOAD:
                internal_state.engine_load_percent = data;
                break;
            case FUEL_PRESSURE_MSB:
                // MS Fuel Pressure is unitless, store as KPA anyway
                temp_float = (float)((data << 8) + read_byte_CRC32());
                internal_state.fuel_pressure = temp_float;
                offset++;
                break;   
                
        }
    }
    
    // Read the four CRC bytes
    uint32_t received_CRC;
    received_CRC = port->read() << 24;
    received_CRC += port->read() << 16;
    received_CRC += port->read() << 8;
    received_CRC += port->read();
                        
    if (received_CRC != checksum) {
        // hal.console->printf("EFI CRC: 0x%08x 0x%08x\n", received_CRC, checksum);
        return false;
    }

    // Calculate Fuel Consumption 
    // Duty Cycle (Percent, because that's how HFE gives us the calibration coefficients)
    float duty_cycle = (internal_state.cylinder_status[0].injection_time_ms * internal_state.engine_speed_rpm)/600.0f;
    uint32_t current_time = AP_HAL::millis();
    // Super Simplified integration method - Error Analysis TBD
    // This calculation gives erroneous results when the engine isn't running
    if (internal_state.engine_speed_rpm > RPM_THRESHOLD) {
        internal_state.fuel_consumption_rate_cm3pm = duty_cycle*get_coef1() - get_coef2();
        internal_state.estimated_consumed_fuel_volume_cm3 += internal_state.fuel_consumption_rate_cm3pm * (current_time - internal_state.last_updated_ms)/60000.0f;
    } else {
        internal_state.fuel_consumption_rate_cm3pm = 0;
    }
    internal_state.last_updated_ms = current_time;
    
    return true;
         
}

void AP_EFI_Serial_MS::send_request(uint8_t table, uint16_t first_offset, uint16_t last_offset)
{
    uint16_t length = last_offset - first_offset + 1;
    // Fixed message size (0x0007)
    // Command 'r' (0x72)
    // Null CANid (0x00)
    const uint8_t data[9] = {
        0x00,
        0x07,
        0x72,
        0x00,
        (uint8_t)table,
        (uint8_t)(first_offset >> 8),
        (uint8_t)(first_offset),
        (uint8_t)(length >> 8),
        (uint8_t)(length)   
    };
    
    uint32_t crc = 0;
    
    // Write the request and calc CRC
    for (uint8_t i = 0;  i != sizeof(data) ; i++) {
        // Message size is excluded from CRC
        if (i > 1) {
            crc = CRC32_compute_byte(crc, data[i]);
        }
        port->write(data[i]);
    }
    
    // Write the CRC32
    port->write((uint8_t)(crc >> 24));
    port->write((uint8_t)(crc >> 16));
    port->write((uint8_t)(crc >> 8));
    port->write((uint8_t)crc);

}

uint8_t AP_EFI_Serial_MS::read_byte_CRC32()
{   
    // Read a byte and update the CRC 
    uint8_t data = port->read();
    checksum = CRC32_compute_byte(checksum, data);
    return data;
}

// CRC32 matching MegaSquirt
uint32_t AP_EFI_Serial_MS::CRC32_compute_byte(uint32_t crc, uint8_t data)
{
    crc ^= ~0U;
    crc = crc_crc32(crc, &data, 1);
    crc ^= ~0U;
    return crc;
}

#endif // HAL_EFI_ENABLED