#ifndef _N2KTONMEA0183FUNCTIONS_H
#define _N2KTONMEA0183FUNCTIONS_H
/**
 * The class N2kToNMEA0183Functions is intended to be used for implementing all converter
 * functions
 * For easy extendability the implementation can be done completely within
 * this header file (java like) without the need to change multiple files
 */
#include <N2kMessages.h>
#include <NMEA0183Messages.h>
#include <math.h>
#include "N2kDataToNMEA0183.h"
#include <map>
class N2kToNMEA0183Functions : public N2kDataToNMEA0183
{
    typedef void (N2kToNMEA0183Functions::*N2KConverter)(const tN2kMsg &N2kMsg);
private:
    static const unsigned long RMCPeriod = 500;
    static void setMax(GwBoatItem<double> *maxItem, GwBoatItem<double> *item)
    {
        unsigned long now = millis();
        if (!item->isValid(now))
            return;
        if (item->getData() > maxItem->getData() || !maxItem->isValid(now))
        {
            maxItem->update(item->getData());
        }
    }
    static void updateDouble(GwBoatItem<double> *item, double value)
    {
        if (value == N2kDoubleNA)
            return;
        if (!item)
            return;
        item->update(value);
    }
    static double formatCourse(double cv)
    {
        double rt = cv * 180.0 / M_PI;
        if (rt > 360)
            rt -= 360;
        if (rt < 0)
            rt += 360;
        return rt;
    }
    static double formatWind(double cv)
    {
        double rt = formatCourse(cv);
        if (rt > 180)
            rt = 180 - rt;
        return rt;
    }
    static double formatKnots(double cv)
    {
        return cv * 3600.0 / 1852.0;
    }

    static uint32_t mtr2nm(uint32_t m)
    {
        return m / 1852;
    }
    GwBoatItem<double> *latitude;
    GwBoatItem<double> *longitude;
    GwBoatItem<double> *altitude;
    GwBoatItem<double> *variation;
    GwBoatItem<double> *heading;
    GwBoatItem<double> *cog;
    GwBoatItem<double> *sog;
    GwBoatItem<double> *stw;

    GwBoatItem<double> *tws;
    GwBoatItem<double> *twd;

    GwBoatItem<double> *aws;
    GwBoatItem<double> *awa;
    GwBoatItem<double> *awd;

    GwBoatItem<double> *maxAws;
    GwBoatItem<double> *maxTws;

    GwBoatItem<double> *rudderPosition;
    GwBoatItem<double> *waterTemperature;
    GwBoatItem<double> *waterDepth;

    GwBoatItem<uint32_t> *tripLog;
    GwBoatItem<uint32_t> *log;
    GwBoatItem<uint32_t> *daysSince1970;
    GwBoatItem<double> *secondsSinceMidnight;

    unsigned long LastPosSend;
    unsigned long NextRMCSend;
    unsigned long lastLoopTime;

    std::map<long,N2KConverter> converters;

    /**
     *  register a n2k message converter
     *  each of the converter functions must be registered in the constructor 
     **/
    void registerConverter(long pgn,N2KConverter converter){
        converters[pgn]=converter;
    }
    virtual const unsigned long * handledPgns(){
        logger->logString("CONV: # %d handled PGNS",(int)converters.size());
        unsigned long *rt=new unsigned long[converters.size()+1];
        int idx=0;
        for (std::map<long,N2KConverter>::iterator it=converters.begin();
        it != converters.end();it++){
            rt[idx]=it->first;
            idx++;
        }
        rt[idx]=0;
        return rt;
    }
    void SetNextRMCSend() { NextRMCSend = millis() + RMCPeriod; }

    //*************** the converters ***********************
    void HandleHeading(const tN2kMsg &N2kMsg)
    {
        unsigned char SID;
        tN2kHeadingReference ref;
        double _Deviation = 0;
        double Variation;
        tNMEA0183Msg NMEA0183Msg;
        double Heading;
        if (ParseN2kHeading(N2kMsg, SID, Heading, _Deviation, Variation, ref))
        {
            if (ref == N2khr_magnetic)
            {
                updateDouble(variation, Variation); // Update Variation
                if (!N2kIsNA(Heading) && variation->isValid(millis()))
                    Heading -= variation->getData();
            }
            updateDouble(heading, Heading);
            if (NMEA0183SetHDG(NMEA0183Msg, heading->getDataWithDefault(NMEA0183DoubleNA), _Deviation, variation->getDataWithDefault(NMEA0183DoubleNA)))
            {
                SendMessage(NMEA0183Msg);
            }
        }
    }

    //*****************************************************************************
    void HandleVariation(const tN2kMsg &N2kMsg)
    {
        unsigned char SID;
        tN2kMagneticVariation Source;
        uint16_t DaysSince1970;
        double Variation;
        ParseN2kMagneticVariation(N2kMsg, SID, Source, DaysSince1970, Variation);
        updateDouble(variation, Variation);
        if (DaysSince1970 != N2kUInt16NA && DaysSince1970 != 0)
            daysSince1970->update(DaysSince1970);
    }

    //*****************************************************************************
    void HandleBoatSpeed(const tN2kMsg &N2kMsg)
    {
        unsigned char SID;
        double WaterReferenced;
        double GroundReferenced;
        tN2kSpeedWaterReferenceType SWRT;

        if (ParseN2kBoatSpeed(N2kMsg, SID, WaterReferenced, GroundReferenced, SWRT))
        {
            tNMEA0183Msg NMEA0183Msg;
            updateDouble(stw, WaterReferenced);
            unsigned long now = millis();
            double MagneticHeading = (heading->isValid(now) && variation->isValid(now)) ? heading->getData() + variation->getData() : NMEA0183DoubleNA;
            if (NMEA0183SetVHW(NMEA0183Msg, heading->getData(), MagneticHeading, WaterReferenced))
            {
                SendMessage(NMEA0183Msg);
            }
        }
    }

    //*****************************************************************************
    void HandleDepth(const tN2kMsg &N2kMsg)
    {
        unsigned char SID;
        double DepthBelowTransducer;
        double Offset;
        double Range;
        double WaterDepth;
        if (ParseN2kWaterDepth(N2kMsg, SID, DepthBelowTransducer, Offset, Range))
        {

            WaterDepth = DepthBelowTransducer + Offset;
            updateDouble(waterDepth, WaterDepth);
            tNMEA0183Msg NMEA0183Msg;
            if (NMEA0183SetDPT(NMEA0183Msg, DepthBelowTransducer, Offset))
            {
                SendMessage(NMEA0183Msg);
            }
            if (NMEA0183SetDBx(NMEA0183Msg, DepthBelowTransducer, Offset))
            {
                SendMessage(NMEA0183Msg);
            }
        }
    }

    //*****************************************************************************
    void HandlePosition(const tN2kMsg &N2kMsg)
    {
        double Latitude;
        double Longitude;
        if (ParseN2kPGN129025(N2kMsg, Latitude, Longitude))
        {
            updateDouble(latitude, Latitude);
            updateDouble(longitude, Longitude);
        }
    }

    //*****************************************************************************
    void HandleCOGSOG(const tN2kMsg &N2kMsg)
    {
        unsigned char SID;
        tN2kHeadingReference HeadingReference;
        tNMEA0183Msg NMEA0183Msg;
        double COG;
        double SOG;
        if (ParseN2kCOGSOGRapid(N2kMsg, SID, HeadingReference, COG, SOG))
        {
            updateDouble(cog, COG);
            updateDouble(sog, SOG);
            double MCOG = (!N2kIsNA(COG) && variation->isValid()) ? COG - variation->getData() : NMEA0183DoubleNA;
            if (HeadingReference == N2khr_magnetic)
            {
                MCOG = COG;
                if (variation->isValid())
                {
                    COG -= variation->getData();
                    updateDouble(cog, COG);
                }
            }
            if (NMEA0183SetVTG(NMEA0183Msg, COG, MCOG, SOG))
            {
                SendMessage(NMEA0183Msg);
            }
        }
    }

    //*****************************************************************************
    void HandleGNSS(const tN2kMsg &N2kMsg)
    {
        unsigned char SID;
        tN2kGNSStype GNSStype;
        tN2kGNSSmethod GNSSmethod;
        unsigned char nSatellites;
        double HDOP;
        double PDOP;
        double GeoidalSeparation;
        unsigned char nReferenceStations;
        tN2kGNSStype ReferenceStationType;
        uint16_t ReferenceSationID;
        double AgeOfCorrection;
        double Latitude;
        double Longitude;
        double Altitude;
        uint16_t DaysSince1970;
        double SecondsSinceMidnight;
        if (ParseN2kGNSS(N2kMsg, SID, DaysSince1970, SecondsSinceMidnight, Latitude, Longitude, Altitude, GNSStype, GNSSmethod,
                         nSatellites, HDOP, PDOP, GeoidalSeparation,
                         nReferenceStations, ReferenceStationType, ReferenceSationID, AgeOfCorrection))
        {
            updateDouble(latitude, Latitude);
            updateDouble(longitude, Longitude);
            updateDouble(altitude, Altitude);
            updateDouble(secondsSinceMidnight, SecondsSinceMidnight);
            if (DaysSince1970 != N2kUInt16NA && DaysSince1970 != 0)
                daysSince1970->update(DaysSince1970);
        }
    }

    //*****************************************************************************
    void HandleWind(const tN2kMsg &N2kMsg)
    {
        unsigned char SID;
        tN2kWindReference WindReference;
        tNMEA0183WindReference NMEA0183Reference = NMEA0183Wind_True;

        double x, y;
        double WindAngle, WindSpeed;

        if (ParseN2kWindSpeed(N2kMsg, SID, WindSpeed, WindAngle, WindReference))
        {
            tNMEA0183Msg NMEA0183Msg;

            if (WindReference == N2kWind_Apparent)
            {
                NMEA0183Reference = NMEA0183Wind_Apparent;
                updateDouble(awa, WindAngle);
                updateDouble(aws, WindSpeed);
                setMax(maxAws, aws);
            }

            if (NMEA0183SetMWV(NMEA0183Msg, formatCourse(WindAngle), NMEA0183Reference, WindSpeed))
                SendMessage(NMEA0183Msg);

            if (WindReference == N2kWind_Apparent && sog->isValid())
            { // Lets calculate and send TWS/TWA if SOG is available

                x = WindSpeed * cos(WindAngle);
                y = WindSpeed * sin(WindAngle);

                updateDouble(twd, atan2(y, -sog->getData() + x));
                updateDouble(tws, sqrt((y * y) + ((-sog->getData() + x) * (-sog->getData() + x))));

                setMax(maxTws, tws);

                NMEA0183Reference = NMEA0183Wind_True;
                if (NMEA0183SetMWV(NMEA0183Msg,
                                   formatCourse(twd->getData()),
                                   NMEA0183Reference,
                                   tws->getDataWithDefault(NMEA0183DoubleNA)))
                    SendMessage(NMEA0183Msg);
                double magnetic = twd->getData();
                if (variation->isValid())
                    magnetic -= variation->getData();
                if (!NMEA0183Msg.Init("MWD", "GP"))
                    return;
                if (!NMEA0183Msg.AddDoubleField(formatCourse(twd->getData())))
                    return;
                if (!NMEA0183Msg.AddStrField("T"))
                    return;
                if (!NMEA0183Msg.AddDoubleField(formatCourse(magnetic)))
                    return;
                if (!NMEA0183Msg.AddStrField("M"))
                    return;
                if (!NMEA0183Msg.AddDoubleField(tws->getData() / 0.514444))
                    return;
                if (!NMEA0183Msg.AddStrField("N"))
                    return;
                if (!NMEA0183Msg.AddDoubleField(tws->getData()))
                    return;
                if (!NMEA0183Msg.AddStrField("M"))
                    return;

                SendMessage(NMEA0183Msg);
            }
        }
    }
    //*****************************************************************************
    void SendRMC()
    {
        long now = millis();
        if (NextRMCSend <= millis() && latitude->isValid(now))
        {
            tNMEA0183Msg NMEA0183Msg;
            if (NMEA0183SetRMC(NMEA0183Msg,

                               secondsSinceMidnight->getDataWithDefault(NMEA0183DoubleNA),
                               latitude->getDataWithDefault(NMEA0183DoubleNA),
                               longitude->getDataWithDefault(NMEA0183DoubleNA),
                               cog->getDataWithDefault(NMEA0183DoubleNA),
                               sog->getDataWithDefault(NMEA0183DoubleNA),
                               daysSince1970->getDataWithDefault(NMEA0183UInt32NA),
                               variation->getDataWithDefault(NMEA0183DoubleNA)))
            {
                SendMessage(NMEA0183Msg);
            }
            SetNextRMCSend();
        }
    }

    //*****************************************************************************
    void HandleLog(const tN2kMsg &N2kMsg)
    {
        uint16_t DaysSince1970;
        double SecondsSinceMidnight;
        uint32_t Log, TripLog;
        if (ParseN2kDistanceLog(N2kMsg, DaysSince1970, SecondsSinceMidnight, Log, TripLog))
        {
            if (Log != N2kUInt32NA)
                log->update(Log);
            if (TripLog != N2kUInt32NA)
                tripLog->update(TripLog);
            if (DaysSince1970 != N2kUInt16NA && DaysSince1970 != 0)
                daysSince1970->update(DaysSince1970);
            tNMEA0183Msg NMEA0183Msg;

            if (!NMEA0183Msg.Init("VLW", "GP"))
                return;
            if (!NMEA0183Msg.AddDoubleField(Log / 1852.0))
                return;
            if (!NMEA0183Msg.AddStrField("N"))
                return;
            if (!NMEA0183Msg.AddDoubleField(TripLog / 1852.0))
                return;
            if (!NMEA0183Msg.AddStrField("N"))
                return;

            SendMessage(NMEA0183Msg);
        }
    }

    //*****************************************************************************
    void HandleRudder(const tN2kMsg &N2kMsg)
    {

        unsigned char Instance;
        tN2kRudderDirectionOrder RudderDirectionOrder;
        double AngleOrder;
        double RudderPosition;

        if (ParseN2kRudder(N2kMsg, RudderPosition, Instance, RudderDirectionOrder, AngleOrder))
        {

            updateDouble(rudderPosition, RudderPosition);
            if (Instance != 0)
                return;

            tNMEA0183Msg NMEA0183Msg;

            if (!NMEA0183Msg.Init("RSA", "GP"))
                return;
            if (!NMEA0183Msg.AddDoubleField(formatCourse(RudderPosition)))
                return;
            if (!NMEA0183Msg.AddStrField("A"))
                return;
            if (!NMEA0183Msg.AddDoubleField(0.0))
                return;
            if (!NMEA0183Msg.AddStrField("A"))
                return;

            SendMessage(NMEA0183Msg);
        }
    }

    //*****************************************************************************
    void HandleWaterTemp(const tN2kMsg &N2kMsg)
    {

        unsigned char SID;
        double OutsideAmbientAirTemperature;
        double AtmosphericPressure;
        double WaterTemperature;
        if (ParseN2kPGN130310(N2kMsg, SID, WaterTemperature, OutsideAmbientAirTemperature, AtmosphericPressure))
        {

            updateDouble(waterTemperature, WaterTemperature);
            tNMEA0183Msg NMEA0183Msg;

            if (!NMEA0183Msg.Init("MTW", "GP"))
                return;
            if (!NMEA0183Msg.AddDoubleField(KelvinToC(WaterTemperature)))
                return;
            if (!NMEA0183Msg.AddStrField("C"))
                return;

            SendMessage(NMEA0183Msg);
        }
    }

public:
    N2kToNMEA0183Functions(GwLog *logger, GwBoatData *boatData, tNMEA2000 *NMEA2000, tNMEA0183 *NMEA0183) : N2kDataToNMEA0183(logger, boatData, NMEA2000, NMEA0183)
    {
        LastPosSend = 0;
        lastLoopTime = 0;
        NextRMCSend = millis() + RMCPeriod;

        this->logger = logger;
        this->boatData = boatData;
        heading = boatData->getDoubleItem(F("Heading"), true, 4000, &formatCourse);
        latitude = boatData->getDoubleItem(F("Latitude"), true, 4000);
        longitude = boatData->getDoubleItem(F("Longitude"), true, 4000);
        altitude = boatData->getDoubleItem(F("Altitude"), true, 4000);
        cog = boatData->getDoubleItem(F("COG"), true, 4000, &formatCourse);
        sog = boatData->getDoubleItem(F("SOG"), true, 4000, &formatKnots);
        stw = boatData->getDoubleItem(F("STW"), true, 4000, &formatKnots);
        variation = boatData->getDoubleItem(F("Variation"), true, 4000, &formatCourse);
        tws = boatData->getDoubleItem(F("TWS"), true, 4000, &formatKnots);
        twd = boatData->getDoubleItem(F("TWD"), true, 4000, &formatCourse);
        aws = boatData->getDoubleItem(F("AWS"), true, 4000, &formatKnots);
        awa = boatData->getDoubleItem(F("AWA"), true, 4000, &formatWind);
        awd = boatData->getDoubleItem(F("AWD"), true, 4000, &formatCourse);
        maxAws = boatData->getDoubleItem(F("MaxAws"), true, 0, &formatKnots);
        maxTws = boatData->getDoubleItem(F("MaxTws"), true, 0, &formatKnots);
        rudderPosition = boatData->getDoubleItem(F("RudderPosition"), true, 4000, &formatCourse);
        waterTemperature = boatData->getDoubleItem(F("WaterTemperature"), true, 4000, &KelvinToC);
        waterDepth = boatData->getDoubleItem(F("WaterDepth"), true, 4000);
        log = boatData->getUint32Item(F("Log"), true, 0, mtr2nm);
        tripLog = boatData->getUint32Item(F("TripLog"), true, 0, mtr2nm);
        daysSince1970 = boatData->getUint32Item(F("DaysSince1970"), true, 4000);
        secondsSinceMidnight = boatData->getDoubleItem(F("SecondsSinceMidnight"), true, 4000);

        //register all converter functions
        //for each vonverter you should have a member with the N2KMsg as parameter
        //and register it here
        //with this approach we easily have a list of all handled
        //pgns
        registerConverter(127250UL,&N2kToNMEA0183Functions::HandleMsg);
        registerConverter(127258UL,&N2kToNMEA0183Functions::HandleVariation);
        registerConverter(128259UL,&N2kToNMEA0183Functions::HandleBoatSpeed);
        registerConverter(128267UL,&N2kToNMEA0183Functions::HandleDepth);
        registerConverter(129025UL,&N2kToNMEA0183Functions::HandlePosition);
        registerConverter(129026UL,&N2kToNMEA0183Functions::HandleCOGSOG);
        registerConverter(129029UL,&N2kToNMEA0183Functions::HandleGNSS);
        registerConverter(130306UL,&N2kToNMEA0183Functions::HandleWind);
        registerConverter(128275UL,&N2kToNMEA0183Functions::HandleLog);
        registerConverter(127245UL,&N2kToNMEA0183Functions::HandleRudder);
        registerConverter(130310UL,&N2kToNMEA0183Functions::HandleWaterTemp);
    }
    virtual void loop()
    {
        N2kDataToNMEA0183::loop();
        unsigned long now = millis();
        if (now < (lastLoopTime + 100))
            return;
        lastLoopTime = now;
        SendRMC();
    }
    virtual void HandleMsg(const tN2kMsg &N2kMsg)
    {
        std::map<long,N2KConverter>::iterator it;
        it = converters.find(N2kMsg.PGN);
        if (it != converters.end()){
            //logger->logString("CONV: handle PGN %ld",N2kMsg.PGN);
            //call to member function - see e.g. https://isocpp.org/wiki/faq/pointers-to-members
            ((*this).*(it->second))(N2kMsg);
            return;
        }
    }
};
#endif