/* -*-mode:c++; c-style:k&r; c-basic-offset:4; -*- */
/**********************************************************************************************
    Copyright (C) 2007 Oliver Eichler oliver.eichler@gmx.de

    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 2 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, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA

  Garmin and MapSource are registered trademarks or trademarks of Garmin Ltd.
  or one of its subsidiaries.

**********************************************************************************************/
#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#include "Garmin.h"
#include "IDevice.h"
#include <iostream>
#include <sstream>

#define INT32_TO_DEG(x) ((double)((int32_t)(x)) * 360.0 / 4294967296.0)
#define DEG_TO_INT32(x) ((int32_t)((x) * 4294967296.0/360.0 + 0.5))

#define FLOAT64_TO_DEG(x) ((x) * 180.0/M_PI)

namespace Garmin
{
    // note: all D1xx_Wpt_t are properly aligned, so we can use the simple macros here
    void operator<<(Wpt_t& tar, const D108_Wpt_t& src) {
        tar.wpt_class    = src.wpt_class;
        tar.dspl_color   = src.color;
        tar.dspl_attr    = src.dspl;
        tar.smbl         = gar_endian(uint16_t, src.smbl);
        tar.lat          = INT32_TO_DEG(gar_endian(int32_t, src.lat));
        tar.lon          = INT32_TO_DEG(gar_endian(int32_t, src.lon));
        tar.alt          = gar_endian(float, src.alt);
        tar.dpth         = gar_endian(float, src.dpth);
        tar.dist         = gar_endian(float, src.dist);
        tar.state[0]     = src.state[0];
        tar.state[1]     = src.state[1];
        tar.state[2]     = 0;
        tar.cc[0]        = src.cc[0];
        tar.cc[1]        = src.cc[1];
        tar.cc[2]        = 0;
        tar.ete          = 0;    // not available

        const char* p = src.str;
        tar.ident = p;
        p += strlen(p) + 1;
        tar.comment = p;
        p += strlen(p) + 1;
        tar.facility = p;
        p += strlen(p) + 1;
        tar.city = p;
        p += strlen(p) + 1;
        tar.addr = p;
        p += strlen(p) + 1;
        tar.crossroad = p;
    }

    int operator>>(const Wpt_t& src, D108_Wpt_t& tar) {
        tar.wpt_class    = src.wpt_class;
        tar.color        = src.dspl_color;
        tar.dspl         = src.dspl_attr;
        tar.attr         = 0x60; // according to iop_spec.pdf
        tar.smbl         = gar_endian(uint16_t, src.smbl);
        tar.subclass[0]  = 0x00;
        tar.subclass[1]  = 0x00;
        tar.subclass[2]  = 0x00;
        tar.subclass[3]  = 0x00;
        tar.subclass[4]  = 0x00;
        tar.subclass[5]  = 0x00;
        tar.subclass[6]  = 0xFF;
        tar.subclass[7]  = 0xFF;
        tar.subclass[8]  = 0xFF;
        tar.subclass[9]  = 0xFF;
        tar.subclass[10] = 0xFF;
        tar.subclass[11] = 0xFF;
        tar.subclass[12] = 0xFF;
        tar.subclass[13] = 0xFF;
        tar.subclass[14] = 0xFF;
        tar.subclass[15] = 0xFF;
        tar.subclass[16] = 0xFF;
        tar.subclass[17] = 0xFF;
        tar.lat          = gar_endian(int32_t, DEG_TO_INT32(src.lat));
        tar.lon          = gar_endian(int32_t, DEG_TO_INT32(src.lon));
        tar.alt          = gar_endian(float, src.alt);
        tar.dpth         = gar_endian(float, src.dpth);
        tar.dist         = gar_endian(float, src.dist);
        tar.state[0]     = src.state[0];
        tar.state[1]     = src.state[1];
        tar.state[2]     = 0;
        tar.cc[0]        = src.cc[0];
        tar.cc[1]        = src.cc[1];
        tar.cc[2]        = 0;

        char * pStr     = tar.str;

        strcpy(pStr,src.ident.c_str());
        pStr += src.ident.length() + 1;
        strcpy(pStr,src.comment.c_str());
        pStr += src.comment.length() + 1;
        strcpy(pStr,src.facility.c_str());
        pStr += src.facility.length() + 1;
        strcpy(pStr,src.city.c_str());
        pStr += src.city.length() + 1;
        strcpy(pStr,src.addr.c_str());
        pStr += src.addr.length() + 1;
        strcpy(pStr,src.crossroad.c_str());
        pStr += src.crossroad.length() + 1;

                                 // size of packet
        return pStr - (char*)&tar.wpt_class;
    }

    void operator<<(Wpt_t& tar, const D109_Wpt_t& src) {
        tar.wpt_class    = src.wpt_class;
        tar.dspl_color   = src.dspl_color & 0x1F;
        tar.dspl_attr    = (src.dspl_color & 0x70) >> 5;
        tar.smbl         = gar_endian(uint16_t, src.smbl);
        tar.lat          = INT32_TO_DEG(gar_endian(int32_t, src.lat));
        tar.lon          = INT32_TO_DEG(gar_endian(int32_t, src.lon));
        tar.alt          = gar_endian(float, src.alt);
        tar.dpth         = gar_endian(float, src.dpth);
        tar.dist         = gar_endian(float, src.dist);
        tar.state[0]     = src.state[0];
        tar.state[1]     = src.state[1];
        tar.state[2]     = 0;
        tar.cc[0]        = src.cc[0];
        tar.cc[1]        = src.cc[1];
        tar.cc[2]        = 0;
        tar.ete          = gar_endian(uint32_t, src.ete);

        const char* p = src.str;
        tar.ident = p;
        p += strlen(p) + 1;
        tar.comment = p;
        p += strlen(p) + 1;
        tar.facility = p;
        p += strlen(p) + 1;
        tar.city = p;
        p += strlen(p) + 1;
        tar.addr = p;
        p += strlen(p) + 1;
        tar.crossroad = p;
    }

    int operator>>(const Wpt_t& src, D109_Wpt_t& tar) {
        tar.dtyp         = 0x01;
        tar.wpt_class    = src.wpt_class;
        tar.dspl_color   = (src.dspl_color | (src.dspl_attr << 5)) & 0x7F;
        tar.attr         = 0x70;
        tar.smbl         = gar_endian(uint16_t, src.smbl);
        tar.subclass[0]  = 0x00;
        tar.subclass[1]  = 0x00;
        tar.subclass[2]  = 0x00;
        tar.subclass[3]  = 0x00;
        tar.subclass[4]  = 0x00;
        tar.subclass[5]  = 0x00;
        tar.subclass[6]  = 0xFF;
        tar.subclass[7]  = 0xFF;
        tar.subclass[8]  = 0xFF;
        tar.subclass[9]  = 0xFF;
        tar.subclass[10] = 0xFF;
        tar.subclass[11] = 0xFF;
        tar.subclass[12] = 0xFF;
        tar.subclass[13] = 0xFF;
        tar.subclass[14] = 0xFF;
        tar.subclass[15] = 0xFF;
        tar.subclass[16] = 0xFF;
        tar.subclass[17] = 0xFF;
        tar.lat          = gar_endian(int32_t, DEG_TO_INT32(src.lat));
        tar.lon          = gar_endian(int32_t, DEG_TO_INT32(src.lon));
        tar.alt          = gar_endian(float, src.alt);
        tar.dpth         = gar_endian(float, src.dpth);
        tar.dist         = gar_endian(float, src.dist);
        tar.state[0]     = src.state[0];
        tar.state[1]     = src.state[1];
        tar.state[2]     = 0;
        tar.cc[0]        = src.cc[0];
        tar.cc[1]        = src.cc[1];
        tar.cc[2]        = 0;
        tar.ete          = gar_endian(uint32_t, src.ete);

        char * pStr     = tar.str;

        strcpy(pStr,src.ident.c_str());
        pStr += src.ident.length() + 1;
        strcpy(pStr,src.comment.c_str());
        pStr += src.comment.length() + 1;
        strcpy(pStr,src.facility.c_str());
        pStr += src.facility.length() + 1;
        strcpy(pStr,src.city.c_str());
        pStr += src.city.length() + 1;
        strcpy(pStr,src.addr.c_str());
        pStr += src.addr.length() + 1;
        strcpy(pStr,src.crossroad.c_str());
        pStr += src.crossroad.length() + 1;

        return pStr - (char*)&tar.dtyp;
    }

    void operator<<(Wpt_t& tar, const D110_Wpt_t& src) {
        tar.wpt_class    = src.wpt_class;
        tar.dspl_color   = src.dspl_color & 0x1F;
        tar.dspl_attr    = (src.dspl_color & 0x60) >> 5;
        tar.smbl         = gar_endian(uint16_t, src.smbl);
        tar.lat          = INT32_TO_DEG(gar_endian(int32_t, src.lat));
        tar.lon          = INT32_TO_DEG(gar_endian(int32_t, src.lon));
        tar.alt          = gar_endian(float, src.alt);
        tar.dpth         = gar_endian(float, src.dpth);
        tar.dist         = gar_endian(float, src.dist);
        tar.state[0]     = src.state[0];
        tar.state[1]     = src.state[1];
        tar.state[2]     = 0;
        tar.cc[0]        = src.cc[0];
        tar.cc[1]        = src.cc[1];
        tar.cc[2]        = 0;
        tar.ete          = gar_endian(uint32_t, src.ete);
        tar.temp         = gar_endian(float, src.temp);
        tar.time         = gar_endian(uint32_t, src.time);
        tar.wpt_cat      = gar_endian(uint16_t, src.wpt_cat);

        const char* p = src.str;
        tar.ident = p;
        p += strlen(p) + 1;
        tar.comment = p;
        p += strlen(p) + 1;
        tar.facility = p;
        p += strlen(p) + 1;
        tar.city = p;
        p += strlen(p) + 1;
        tar.addr = p;
        p += strlen(p) + 1;
        tar.crossroad = p;
    }

    int operator>>(const Wpt_t& src, D110_Wpt_t& tar) {
        tar.dtyp         = 0x01;
        tar.wpt_class    = src.wpt_class;
        tar.dspl_color   = (src.dspl_color | (src.dspl_attr << 5)) & 0x7F;
        tar.attr         = 0x80;
        tar.smbl         = gar_endian(uint16_t, src.smbl);
        tar.subclass[0]  = 0x00;
        tar.subclass[1]  = 0x00;
        tar.subclass[2]  = 0x00;
        tar.subclass[3]  = 0x00;
        tar.subclass[4]  = 0x00;
        tar.subclass[5]  = 0x00;
        tar.subclass[6]  = 0xFF;
        tar.subclass[7]  = 0xFF;
        tar.subclass[8]  = 0xFF;
        tar.subclass[9]  = 0xFF;
        tar.subclass[10] = 0xFF;
        tar.subclass[11] = 0xFF;
        tar.subclass[12] = 0xFF;
        tar.subclass[13] = 0xFF;
        tar.subclass[14] = 0xFF;
        tar.subclass[15] = 0xFF;
        tar.subclass[16] = 0xFF;
        tar.subclass[17] = 0xFF;
        tar.lat          = gar_endian(int32_t, DEG_TO_INT32(src.lat));
        tar.lon          = gar_endian(int32_t, DEG_TO_INT32(src.lon));
        tar.alt          = gar_endian(float, src.alt);
        tar.dpth         = gar_endian(float, src.dpth);
        tar.dist         = gar_endian(float, src.dist);
        tar.state[0]     = src.state[0];
        tar.state[1]     = src.state[1];
        tar.state[2]     = 0;
        tar.cc[0]        = src.cc[0];
        tar.cc[1]        = src.cc[1];
        tar.cc[2]        = 0;
        tar.ete          = gar_endian(uint32_t, src.ete);
        tar.temp         = gar_endian(float, src.temp);
        tar.time         = gar_endian(uint32_t, src.time);
        tar.wpt_cat      = gar_endian(uint16_t, src.wpt_cat);

        char * pStr     = tar.str;

        strcpy(pStr,src.ident.c_str());
        pStr += src.ident.length() + 1;
        strcpy(pStr,src.comment.c_str());
        pStr += src.comment.length() + 1;
        strcpy(pStr,src.facility.c_str());
        pStr += src.facility.length() + 1;
        strcpy(pStr,src.city.c_str());
        pStr += src.city.length() + 1;
        strcpy(pStr,src.addr.c_str());
        pStr += src.addr.length() + 1;
        strcpy(pStr,src.crossroad.c_str());
        pStr += src.crossroad.length() + 1;

        return pStr - (char*)&tar.dtyp;
    }

    // same as D312, but without color=16=transparent
    void operator<<(Track_t& tar, const D310_Trk_Hdr_t& src) {
        tar.dspl     = src.dspl;
        tar.color    = src.color;
        tar.ident    = src.ident;
    }

    void operator<<(Track_t& tar, const D311_Trk_Hdr_t& src) {
        std::stringstream strs;
        strs << int(src.ident);
        tar.ident    = strs.str();
        tar.ident= std::string(4-tar.ident.size(),'0')+tar.ident;
        //std::cout<<"track: "<< tar.ident<<"  --  "<< tar.ident<<std::endl;
    }

    void operator<<(Track_t& tar, const D312_Trk_Hdr_t& src) {
        tar.dspl     = src.dspl;
        tar.color    = src.color;
        tar.ident    = src.ident;
    }

    int operator>>(const Track_t& src, D312_Trk_Hdr_t& tar) {
        tar.dspl     = src.dspl;
        tar.color    = src.color;

        char * pIdent     = tar.ident;

        strcpy(pIdent,src.ident.c_str());
        pIdent += src.ident.length() + 1;

        return pIdent - (char*)&tar.dspl;
    }

    void operator<<(TrkPt_t& tar, const D301_Trk_t& src) {
        tar.lat      = INT32_TO_DEG(gar_endian(int32_t, src.lat));
        tar.lon      = INT32_TO_DEG(gar_endian(int32_t, src.lon));
        tar.time     = gar_endian(uint32_t, src.time);
        tar.alt      = gar_endian(float, src.alt);
        tar.dpth     = gar_endian(float, src.dpth);
    }

    void operator<<(TrkPt_t& tar, const D302_Trk_t& src) {
        tar.lat      = INT32_TO_DEG(gar_endian(int32_t, src.lat));
        tar.lon      = INT32_TO_DEG(gar_endian(int32_t, src.lon));
        tar.time     = gar_endian(uint32_t, src.time);
        tar.alt      = gar_endian(float, src.alt);
        tar.dpth     = gar_endian(float, src.dpth);
    }

    int operator>>(const TrkPt_t& src, D302_Trk_t& tar) {
        tar.lat      = gar_endian(int32_t, DEG_TO_INT32(src.lat));
        tar.lon      = gar_endian(int32_t, DEG_TO_INT32(src.lon));
        tar.time     = gar_endian(uint32_t, src.time);
        tar.alt      = gar_endian(float, src.alt);
        //        tar.dpth     = gar_endian(float, src.dpth);
        return (char*)&tar.alt - (char*)&tar.lat + 1;
    }

    void operator<<(TrkPt_t& tar, const D304_Trk_t& src) {

        tar.lat      = INT32_TO_DEG(gar_endian(int32_t, src.lat));
        tar.lon      = INT32_TO_DEG(gar_endian(int32_t, src.lon));
        tar.time     = gar_endian(uint32_t, src.time);
        tar.alt      = gar_endian(float, src.alt);

        tar.distance    = gar_endian(float, src.distance);
        tar.heart_rate  = src.heart_rate;
        tar.cadence     = src.cadence;

        //std::cout<< "----------src.lat:"<<src.lat<< "   src.lon:"<< src.lon<<std::endl;
        // std::cout<< "lat:"<<tar.lat<< "   lon:"<< tar.lon<< "   time:"<<tar.time
        // << "   alt:"<<tar.alt;
        // #ifdef FR304XDATA
        // std::cout<<"   distance:"<<tar.distance<<"    heartrate:"<<int(tar.heart_rate)<<std::endl;
        // #endif
        // std::cout<<std::endl;
    }

    void operator<<(Pvt_t& tar, const D800_Pvt_Data_t& src) {
        // note: some fields are mis-aligned, so we have to use gar_load_* there ...
        tar.alt         = gar_endian(float, src.alt);
        tar.msl_hght    = gar_load(float, src.msl_hght);

        tar.epe         = gar_endian(float, src.epe);
        tar.eph         = gar_endian(float, src.eph);
        tar.epv         = gar_endian(float, src.epv);
        tar.fix         = gar_endian(uint16_t, src.fix);
        tar.lat         = FLOAT64_TO_DEG(gar_load(double, src.lat));
        tar.lon         = FLOAT64_TO_DEG(gar_load(double, src.lon));

        tar.tow         = gar_load(double, src.tow);
        tar.wn_days     = gar_endian(uint32_t, src.wn_days);
        tar.leap_scnds  = gar_endian(int16_t, src.leap_scnds);

        tar.north       = gar_load(float, src.north);
        tar.east        = gar_load(float, src.east);
        tar.up          = gar_load(float, src.up);

    }

    void operator<<(Route_t& tar, const D202_Rte_Hdr_t& src) {
        tar.ident = src.ident;
    }

    int  operator>>(const Route_t& src, D202_Rte_Hdr_t& tar) {
        char * pStr     = tar.ident;
        strcpy(pStr,src.ident.c_str());
        return src.ident.size() + 1;
    }

    void operator<<(RtePt_t& tar, const D210_Rte_Link_t& src) {
        tar.rte_class = gar_endian(uint16_t, src.rte_class);
        tar.subclass_1 = gar_endian(uint16_t, src.subclass_1);
        tar.subclass_2 = gar_endian(uint32_t, src.subclass_2);
        tar.subclass_3 = gar_endian(uint32_t, src.subclass_3);
        tar.subclass_4 = gar_endian(uint32_t, src.subclass_4);
        tar.subclass_5 = gar_endian(uint32_t, src.subclass_5);
        tar.ident = src.ident;
    }

    int  operator>>(const RtePt_t& src, D210_Rte_Link_t& tar) {
        tar.rte_class = gar_endian(uint16_t, src.rte_class);
        tar.subclass_1 = gar_endian(uint16_t, src.subclass_1);
        tar.subclass_2 = gar_endian(uint32_t, src.subclass_2);
        tar.subclass_3 = gar_endian(uint32_t, src.subclass_3);
        tar.subclass_4 = gar_endian(uint32_t, src.subclass_4);
        tar.subclass_5 = gar_endian(uint32_t, src.subclass_5);

        char * pStr     = tar.ident;
        *pStr++ = 0;

        return pStr - (char*)&tar.rte_class;
    }

    int  operator<<(Map_t& tar, const Map_Info_t& src) {
        const char * pStr = src.name1;
        tar.mapName = pStr;
        pStr += strlen(pStr) + 1;
        tar.tileName = pStr;

        // return the src record size
        uint16_t entry_size = gar_load(uint16_t, src.size);
        return sizeof(src.tok) + sizeof(src.size) + entry_size;
    }

}
