#include "../../include/main.h"
#include "Nmea2kTwai.h"
#include "driver/gpio.h"
#include "driver/twai.h"

#define LOGID(id) ((id >> 8) & 0x1ffff)

// Logging
static const char* TAG = "TWAI";

static const int TIMEOUT_OFFLINE = 256; // number of timeouts to consider offline

Nmea2kTwai::Nmea2kTwai(gpio_num_t _TxPin,  gpio_num_t _RxPin, unsigned long recP, unsigned long logP):
     tNMEA2000(), RxPin(_RxPin), TxPin(_TxPin)
{
    if (RxPin < 0 || TxPin < 0) {
        disabled = true;    
    } else {
        // timers.addAction(logP,[this](){ logStatus(); });
        // timers.addAction(recP,[this](){ checkRecovery(); });
    }
}

bool Nmea2kTwai::CANSendFrame(unsigned long id, unsigned char len, const unsigned char *buf, bool wait_sent)
{
    if (disabled) {
        return true;
    }
    twai_message_t message;
    memset(&message, 0, sizeof(message));
    message.identifier = id;
    message.extd = 1;
    message.data_length_code = len;
    memcpy(message.data, buf, len);
    esp_err_t rt = twai_transmit(&message, 0);
    if (rt != ESP_OK) {
        if (rt == ESP_ERR_TIMEOUT) {
            if (txTimeouts < TIMEOUT_OFFLINE) {
                txTimeouts++;
            }
        }
        LOGW(TAG, "twai transmit for %ld failed: %x", LOGID(id), (int)rt);
        return false;
    }
    txTimeouts = 0;
    LOGV(TAG, "twai transmit id %ld, len %d", LOGID(id), (int)len);
    return true;
}

bool Nmea2kTwai::CANOpen()
{
    if (disabled) {
        LOGD(TAG, "CAN disabled");
        return true;
    }
    esp_err_t rt = twai_start();
    if (rt != ESP_OK) {
        LOGE(TAG, "CANOpen failed: %x", rt);
        return false;
    } else {
        LOGI(TAG, "CANOpen ok");
    }
    return true;
}

bool Nmea2kTwai::CANGetFrame(unsigned long &id, unsigned char &len, unsigned char *buf)
{
    if (disabled) {
        return false;
    }
    twai_message_t message;
    esp_err_t rt = twai_receive(&message, 0);
    if (rt != ESP_OK){
        return false;
    }
    if (! message.extd) {
        return false;
    }
    id = message.identifier;
    len = message.data_length_code;
    if (len > 8) {
        LOGD(TAG, "twai: received invalid message %lld, len %d", LOGID(id), len);
        len = 8;
    }
    LOGV(TAG, "twai rcv id=%ld,len=%d, ext=%d", 
         LOGID(message.identifier), message.data_length_code, message.extd);
    if (! message.rtr) {
        memcpy(buf, message.data, message.data_length_code);
    }
    return true;
}

void Nmea2kTwai::initDriver()
{
    if (disabled) {
        return;
    }
    twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(TxPin,RxPin, TWAI_MODE_NORMAL);
    g_config.tx_queue_len = 20;
    twai_timing_config_t t_config = TWAI_TIMING_CONFIG_250KBITS();
    twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
    esp_err_t rt = twai_driver_install(&g_config, &t_config, &f_config);
    if (rt == ESP_OK) {
        LOGI(TAG, "twai driver initialzed, rx=%d,tx=%d", RxPin, TxPin);
    } else {
        LOGE(TAG,"twai driver init failed: %x", rt);
    }
}

/* This will be called on Open() before any other initialization.
 * Inherit this, if buffers can be set for the driver and you want to
 * change size of library send frame buffer size.
 * See e.g. NMEA2000_teensy.cpp.
 */
void Nmea2kTwai::InitCANFrameBuffers()
{
    if (disabled) {
        LOGI(TAG, "twai init - disabled");
    } else{
        initDriver();
    }
    tNMEA2000::InitCANFrameBuffers();
}

Nmea2kTwai::Status Nmea2kTwai::getStatus()
{
    twai_status_info_t state;
    Status rt;
    if (disabled) {
        rt.state = ST_DISABLED;
        return rt;
    }
    if (twai_get_status_info(&state) != ESP_OK) {
        return rt;
    }
    switch (state.state) {
        case TWAI_STATE_STOPPED:
            rt.state = ST_STOPPED;
            break;
        case TWAI_STATE_RUNNING:
            rt.state = ST_RUNNING;
            break;
        case TWAI_STATE_BUS_OFF:
            rt.state = ST_BUS_OFF;
            break;
        case TWAI_STATE_RECOVERING:
            rt.state = ST_RECOVERING;
            break;
    }
    rt.rx_errors = state.rx_error_counter;
    rt.tx_errors = state.tx_error_counter;
    rt.tx_failed = state.tx_failed_count;
    rt.rx_missed = state.rx_missed_count;
    rt.rx_overrun = state.rx_overrun_count;
    rt.tx_timeouts = txTimeouts;
    if (rt.tx_timeouts >= TIMEOUT_OFFLINE && rt.state == ST_RUNNING) {
        rt.state = ST_OFFLINE;
    }
    return rt;
}

bool Nmea2kTwai::checkRecovery()
{
    if (disabled) {
        return false;
    }
    Status canState = getStatus();
    bool strt = false;
    if (canState.state != Nmea2kTwai::ST_RUNNING) {
        if (canState.state == Nmea2kTwai::ST_BUS_OFF) {
            strt = true;
            bool rt = startRecovery();
            LOGI(TAG, "twai BUS_OFF: start can recovery - result %d", rt);
        }
        if (canState.state == Nmea2kTwai::ST_STOPPED) {
            bool rt = CANOpen();
            LOGI(TAG, "twai STOPPED: restart can driver - result %d", rt);
        }
    }
    return strt;
}

void Nmea2kTwai::loop()
{
    if (disabled) {
        return;
    }
    LOGW(TAG, "twai loop() not implemented");
    // TODO?
    // timers.loop();
}

Nmea2kTwai::Status Nmea2kTwai::logStatus()
{
    Status canState = getStatus();
    LOGD(TAG, "twai state %s, rxerr %d, txerr %d, txfail %d, txtimeout %d, rxmiss %d, rxoverrun %d",
         stateStr(canState.state),
         canState.rx_errors,
         canState.tx_errors,
         canState.tx_failed,
         canState.tx_timeouts,
         canState.rx_missed,
         canState.rx_overrun);
    return canState;
}

bool Nmea2kTwai::startRecovery()
{
    if (disabled) {
        return false;
    }
    lastRecoveryStart = millis();
    esp_err_t rt = twai_driver_uninstall();
    if (rt != ESP_OK) {
        LOGE(TAG, "twai: deinit for recovery failed with %x", rt);
    }
    initDriver();
    bool frt = CANOpen();
    return frt;
}

const char * Nmea2kTwai::stateStr(const Nmea2kTwai::STATE &st)
{
    switch (st) {
        case ST_BUS_OFF: return "BUS_OFF";
        case ST_RECOVERING: return "RECOVERING";
        case ST_RUNNING: return "RUNNING";
        case ST_STOPPED: return "STOPPED";
        case ST_OFFLINE: return "OFFLINE";
        case ST_DISABLED: return "DISABLED";
    }
    return "ERROR";
}
const Nmea2kTwai::ManufacturerEntry Nmea2kTwai::ManufacturerTable[] = {
    {0, "Reserved"},
    {69, "ARKS Enterprises, Inc."},
    {78, "FW Murphy/Enovation Controls"},
    {80, "Twin Disc"},
    {85, "Kohler Power Systems"},
    {88, "Hemisphere GPS Inc"},
    {116, "BEP Marine"},
    {135, "Airmar"},
    {137, "Maretron"},
    {140, "Lowrance"},
    {144, "Mercury Marine"},
    {147, "Nautibus Electronic GmbH"},
    {148, "Blue Water Data"},
    {154, "Westerbeke"},
    {161, "Offshore Systems (UK) Ltd."},
    {163, "Evinrude/BRP"},
    {165, "CPAC Systems AB"},
    {168, "Xantrex Technology Inc."},
    {172, "Yanmar Marine"},
    {174, "Volvo Penta"},
    {175, "Honda Marine"},
    {176, "Carling Technologies Inc. (Moritz Aerospace)"},
    {185, "Beede Instruments"},
    {192, "Floscan Instrument Co. Inc."},
    {193, "Nobletec"},
    {198, "Mystic Valley Communications"},
    {199, "Actia"},
    {200, "Honda Marine"},
    {201, "Disenos Y Technologia"},
    {211, "Digital Switching Systems"},
    {215, "Xintex/Atena"},
    {224, "EMMI NETWORK S.L."},
    {225, "Honda Marine"},
    {228, "ZF"},
    {229, "Garmin"},
    {233, "Yacht Monitoring Solutions"},
    {235, "Sailormade Marine Telemetry/Tetra Technology LTD"},
    {243, "Eride"},
    {250, "Honda Marine"},
    {257, "Honda Motor Company LTD"},
    {272, "Groco"},
    {273, "Actisense"},
    {274, "Amphenol LTW Technology"},
    {275, "Navico"},
    {283, "Hamilton Jet"},
    {285, "Sea Recovery"},
    {286, "Coelmo SRL Italy"},
    {295, "BEP Marine"},
    {304, "Empir Bus"},
    {305, "NovAtel"},
    {306, "Sleipner Motor AS"},
    {307, "MBW Technologies"},
    {311, "Fischer Panda"},
    {315, "ICOM"},
    {328, "Qwerty"},
    {329, "Dief"},
    {341, "Böning Automationstechnologie GmbH & Co. KG"},
    {345, "Korean Maritime University"},
    {351, "Thrane and Thrane"},
    {355, "Mastervolt"},
    {356, "Fischer Panda Generators"},
    {358, "Victron Energy"},
    {370, "Rolls Royce Marine"},
    {373, "Electronic Design"},
    {374, "Northern Lights"},
    {378, "Glendinning"},
    {381, "B & G"},
    {384, "Rose Point Navigation Systems"},
    {385, "Johnson Outdoors Marine Electronics Inc Geonav"},
    {394, "Capi 2"},
    {396, "Beyond Measure"},
    {400, "Livorsi Marine"},
    {404, "ComNav"},
    {409, "Chetco"},
    {419, "Fusion Electronics"},
    {421, "Standard Horizon"},
    {422, "True Heading AB"},
    {426, "Egersund Marine Electronics AS"},
    {427, "em-trak Marine Electronics"},
    {431, "Tohatsu Co, JP"},
    {437, "Digital Yacht"},
    {438, "Comar Systems Limited"},
    {440, "Cummins"},
    {443, "VDO (aka Continental-Corporation)"},
    {451, "Parker Hannifin aka Village Marine Tech"},
    {459, "Alltek Marine Electronics Corp"},
    {460, "SAN GIORGIO S.E.I.N"},
    {466, "Veethree Electronics & Marine"},
    {467, "Humminbird Marine Electronics"},
    {470, "SI-TEX Marine Electronics"},
    {471, "Sea Cross Marine AB"},
    {475, "GME aka Standard Communications Pty LTD"},
    {476, "Humminbird Marine Electronics"},
    {478, "Ocean Sat BV"},
    {481, "Chetco Digitial Instruments"},
    {493, "Watcheye"},
    {499, "Lcj Capteurs"},
    {502, "Attwood Marine"},
    {503, "Naviop S.R.L."},
    {504, "Vesper Marine Ltd"},
    {510, "Marinesoft Co. LTD"},
    {517, "NoLand Engineering"},
    {518, "Transas USA"},
    {529, "National Instruments Korea"},
    {532, "Onwa Marine"},
    {571, "Marinecraft (South Korea)"},
    {573, "McMurdo Group aka Orolia LTD"},
    {578, "Advansea"},
    {579, "KVH"},
    {580, "San Jose Technology"},
    {583, "Yacht Control"},
    {586, "Suzuki Motor Corporation"},
    {591, "US Coast Guard"},
    {595, "Ship Module aka Customware"},
    {600, "Aquatic AV"},
    {605, "Aventics GmbH"},
    {606, "Intellian"},
    {612, "SamwonIT"},
    {614, "Arlt Tecnologies"},
    {637, "Bavaria Yacts"},
    {641, "Diverse Yacht Services"},
    {644, "Wema U.S.A dba KUS"},
    {645, "Garmin"},
    {658, "Shenzhen Jiuzhou Himunication"},
    {688, "Rockford Corp"},
    {704, "JL Audio"},
    {715, "Autonnic"},
    {717, "Yacht Devices"},
    {734, "REAP Systems"},
    {735, "Au Electronics Group"},
    {739, "LxNav"},
    {743, "DaeMyung"},
    {744, "Woosung"},
    {773, "Clarion US"},
    {776, "HMI Systems"},
    {777, "Ocean Signal"},
    {778, "Seekeeper"},
    {781, "Poly Planar"},
    {785, "Fischer Panda DE"},
    {795, "Broyda Industries"},
    {796, "Canadian Automotive"},
    {797, "Tides Marine"},
    {798, "Lumishore"},
    {799, "Still Water Designs and Audio"},
    {802, "BJ Technologies (Beneteau)"},
    {803, "Gill Sensors"},
    {811, "Blue Water Desalination"},
    {815, "FLIR"},
    {824, "Undheim Systems"},
    {838, "TeamSurv"},
    {844, "Fell Marine"},
    {847, "Oceanvolt"},
    {862, "Prospec"},
    {868, "Data Panel Corp"},
    {890, "L3 Technologies"},
    {894, "Rhodan Marine Systems"},
    {896, "Nexfour Solutions"},
    {905, "ASA Electronics"},
    {909, "Marines Co (South Korea)"},
    {911, "Nautic-on"},
    {930, "Ecotronix"},
    {962, "Timbolier Industries"},
    {963, "TJC Micro"},
    {968, "Cox Powertrain"},
    {969, "Blue Seas"},
    {1850, "Teleflex Marine (SeaStar Solutions)"},
    {1851, "Raymarine"},
    {1852, "Navionics"},
    {1853, "Japan Radio Co"},
    {1854, "Northstar Technologies"},
    {1855, "Furuno"},
    {1856, "Trimble"},
    {1857, "Simrad"},
    {1858, "Litton"},
    {1859, "Kvasar AB"},
    {1860, "MMP"},
    {1861, "Vector Cantech"},
    {1862, "Yamaha Marine"},
    {1863, "Faria Instruments"},
    {2046, "Open Boat Projects"}
};

const char* Nmea2kTwai::GetManufacturerName(uint16_t code) {
    constexpr size_t ManufacturerCount =
        sizeof(Nmea2kTwai::ManufacturerTable) /
        sizeof(Nmea2kTwai::ManufacturerTable[0]);
    int low = 0;
    int high = ManufacturerCount - 1;

    while (low <= high) {
        int mid = (low + high) / 2;
        uint16_t midCode = ManufacturerTable[mid].code;

        if (midCode == code) return ManufacturerTable[mid].name;
        if (midCode < code) low = mid + 1;
        else high = mid - 1;
    }
    return "Unknown";
}