/**********************************************************************************************
    Copyright (C) 2007 Oliver Eichler oliver.eichler@gmx.de
                       bzrudi (bzrudi@users.sourceforge.net)

    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 "../Platform.h"
#include "CDevice.h"
#include <Garmin.h>

#include <cstdio>
#include <errno.h>
#include <iostream>
#include <sstream>
#include <stdio.h>

using namespace EtrexLegendC;
using namespace Garmin;
using namespace std;

namespace EtrexLegendC
{

    static const char _clrtbl[1024]= {
        0,0,0,0,32,0,0,0,65,0,0,0,106,0,0,0,-117,0,0,0,
        -76,0,0,0,-43,0,0,0,-1,0,0,0,0,48,0,0,32,48,0,0,
        65,48,0,0,106,48,0,0,-117,48,0,0,-76,48,0,0,-43,48,0,0,
        -1,48,0,0,0,101,0,0,32,101,0,0,65,101,0,0,106,101,0,0,
        -117,101,0,0,-76,101,0,0,-43,101,0,0,-1,101,0,0,0,-107,0,0,
        32,-107,0,0,65,-107,0,0,106,-107,0,0,-117,-107,0,0,-76,-107,0,0,
        -43,-107,0,0,-1,-107,0,0,0,-54,0,0,32,-54,0,0,65,-54,0,0,
        106,-54,0,0,-117,-54,0,0,-76,-54,0,0,-43,-54,0,0,-1,-54,0,0,
        0,-1,0,0,32,-1,0,0,65,-1,0,0,106,-1,0,0,-117,-1,0,0,
        -76,-1,0,0,-43,-1,0,0,-1,-1,0,0,0,0,57,0,32,0,57,0,
        65,0,57,0,106,0,57,0,-117,0,57,0,-76,0,57,0,-43,0,57,0,
        -1,0,57,0,0,48,57,0,32,48,57,0,65,48,57,0,106,48,57,0,
        -117,48,57,0,-76,48,57,0,-43,48,57,0,-1,48,57,0,0,101,57,0,
        32,101,57,0,65,101,57,0,106,101,57,0,-117,101,57,0,-76,101,57,0,
        -43,101,57,0,-1,101,57,0,0,-107,57,0,32,-107,57,0,65,-107,57,0,
        106,-107,57,0,-117,-107,57,0,-76,-107,57,0,-43,-107,57,0,-1,-107,57,0,
        0,-54,57,0,32,-54,57,0,65,-54,57,0,106,-54,57,0,-117,-54,57,0,
        -76,-54,57,0,-43,-54,57,0,-1,-54,57,0,0,-1,57,0,32,-1,57,0,
        65,-1,57,0,106,-1,57,0,-117,-1,57,0,-76,-1,57,0,-43,-1,57,0,
        -1,-1,57,0,0,0,123,0,32,0,123,0,65,0,123,0,106,0,123,0,
        -117,0,123,0,-76,0,123,0,-43,0,123,0,-1,0,123,0,0,48,123,0,
        32,48,123,0,65,48,123,0,106,48,123,0,-117,48,123,0,-76,48,123,0,
        -43,48,123,0,-1,48,123,0,0,101,123,0,32,101,123,0,65,101,123,0,
        106,101,123,0,-117,101,123,0,-76,101,123,0,-43,101,123,0,-1,101,123,0,
        0,-107,123,0,32,-107,123,0,65,-107,123,0,106,-107,123,0,-117,-107,123,0,
        -76,-107,123,0,-43,-107,123,0,-1,-107,123,0,0,-54,123,0,32,-54,123,0,
        65,-54,123,0,106,-54,123,0,-117,-54,123,0,-76,-54,123,0,-43,-54,123,0,
        -1,-54,123,0,0,-1,123,0,32,-1,123,0,65,-1,123,0,106,-1,123,0,
        -117,-1,123,0,-76,-1,123,0,-43,-1,123,0,-1,-1,123,0,0,0,-67,0,
        32,0,-67,0,65,0,-67,0,106,0,-67,0,-117,0,-67,0,-76,0,-67,0,
        -43,0,-67,0,-1,0,-67,0,0,48,-67,0,32,48,-67,0,65,48,-67,0,
        106,48,-67,0,-117,48,-67,0,-76,48,-67,0,-43,48,-67,0,-1,48,-67,0,
        0,101,-67,0,32,101,-67,0,65,101,-67,0,106,101,-67,0,-117,101,-67,0,
        -76,101,-67,0,-43,101,-67,0,-1,101,-67,0,0,-107,-67,0,32,-107,-67,0,
        65,-107,-67,0,106,-107,-67,0,-117,-107,-67,0,-76,-107,-67,0,-43,-107,-67,0,
        -1,-107,-67,0,0,-54,-67,0,32,-54,-67,0,65,-54,-67,0,106,-54,-67,0,
        -117,-54,-67,0,-76,-54,-67,0,-43,-54,-67,0,-1,-54,-67,0,0,-1,-67,0,
        32,-1,-67,0,65,-1,-67,0,106,-1,-67,0,-117,-1,-67,0,-76,-1,-67,0,
        -43,-1,-67,0,-1,-1,-67,0,0,0,-1,0,32,0,-1,0,65,0,-1,0,
        106,0,-1,0,-117,0,-1,0,-76,0,-1,0,-43,0,-1,0,-1,0,-1,0,
        0,48,-1,0,32,48,-1,0,65,48,-1,0,106,48,-1,0,-117,48,-1,0,
        -76,48,-1,0,-43,48,-1,0,-1,48,-1,0,0,101,-1,0,32,101,-1,0,
        65,101,-1,0,106,101,-1,0,-117,101,-1,0,-76,101,-1,0,-43,101,-1,0,
        -1,101,-1,0,0,-107,-1,0,32,-107,-1,0,65,-107,-1,0,106,-107,-1,0,
        -117,-107,-1,0,-76,-107,-1,0,-43,-107,-1,0,-1,-107,-1,0,0,-54,-1,0,
        32,-54,-1,0,65,-54,-1,0,106,-54,-1,0,-117,-54,-1,0,-76,-54,-1,0,
        -43,-54,-1,0,-1,-54,-1,0,0,-1,-1,0,32,-1,-1,0,65,-1,-1,0,
        106,-1,-1,0,-117,-1,-1,0,-76,-1,-1,0,-43,-1,-1,0,-1,-1,-1,0,
        -1,-1,-1,0,-26,-26,-26,0,-43,-43,-43,0,-59,-59,-59,0,-76,-76,-76,0,
        -92,-92,-92,0,-108,-108,-108,0,-125,-125,-125,0,115,115,115,0,98,98,98,0,
        82,82,82,0,65,65,65,0,49,49,49,0,32,32,32,0,16,16,16,0,
        0,0,0,0
    };

    class CMutexLocker
    {
        public:
            CMutexLocker(pthread_mutex_t& mutex)
            : mutex(mutex) {
                pthread_mutex_lock(&mutex);
            }

            ~CMutexLocker() {
                pthread_mutex_unlock(&mutex);
            }
        private:
            pthread_mutex_t& mutex;

    };

    void * rtThread(void *ptr) {
        cout << "start thread" << endl;
        Packet_t command;
        Packet_t response;

        CDevice * dev = (CDevice*)ptr;
        CMutexLocker lock(dev->mutex);
        try
        {
            pthread_mutex_lock(&dev->dataMutex);
            dev->_acquire();

            command.type = GUSB_APPLICATION_LAYER;
            command.id   = Pid_Command_Data;
            command.size = 2;
            *(uint16_t*)command.payload = Cmnd_Start_Pvt_Data;
            dev->usb->write(command);

            while(dev->doRealtimeThread) {
                pthread_mutex_unlock(&dev->dataMutex);

                if(dev->usb->read(response)) {
                    if(response.id == Pid_Pvt_Data) {
                        D800_Pvt_Data_t * srcPvt = (D800_Pvt_Data_t*)response.payload;
                        pthread_mutex_lock(&dev->dataMutex);
                        dev->PositionVelocityTime << *srcPvt;
                        pthread_mutex_unlock(&dev->dataMutex);
                    }
                }

                pthread_mutex_lock(&dev->dataMutex);
            }

            command.type = GUSB_APPLICATION_LAYER;
            command.id   = Pid_Command_Data;
            command.size = 2;
            *(uint16_t*)command.payload = Cmnd_Stop_Pvt_Data;
            dev->usb->write(command);

            dev->_release();
            pthread_mutex_unlock(&dev->dataMutex);
        }
        catch(exce_t& e) {
            pthread_mutex_trylock(&dev->dataMutex);
            dev->lasterror = "Realtime thread failed. " + e.msg;
            dev->doRealtimeThread = false;
            pthread_mutex_unlock(&dev->dataMutex);
        }
        cout << "stop thread" << endl;
        return 0;
    }

}


CDevice::CDevice()
: devid(0)
, usb(0)
, doRealtimeThread(false)
{
    pthread_mutex_init(&dataMutex, NULL);
}


CDevice::~CDevice()
{
}


const string& CDevice::getCopyright()
{
    copyright = "<h1>QLandkarte Device Driver for Garmin " + devname + "</h1>"
        "<h2>Driver I/F Ver. " INTERFACE_VERSION "</h2>"
        "<p>&#169; 2007 by bzrudi (bzrudi@users.sourceforge.net)</p>"
        "<p>This driver 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. </p>";
    return copyright;
}


void CDevice::_acquire()
{
#if defined(HAVE_BIGENDIAN) || !defined(CAN_UNALIGNED)
    throw exce_t(errSync, "This device has not yet been ported to your platform.");
#endif
    usb = new CUSB();
    usb->open();

    /**
    Workaround for faulty Etrex Legend C session start. Needs to be called
    3 times. Please refer to GPSBabel gusb_reset_toggles() function
    in gpslibusb.c.
    **/
    Packet_t command;
    command.type = GUSB_PROTOCOL_LAYER;
    command.id   = GUSB_SESSION_START;
    command.size = 0;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);
    usb->write(command);
    /**
    Now we call syncup() which takes care of the third try in session start
    **/
    usb->syncup();

    if(strncmp(usb->getProductString().c_str(), devname.c_str(), devname.size()) != 0) {
        string msg = "No " + devname + " unit detected. Please retry to select other device driver.";
        throw exce_t(errSync,msg);
    }
}


void CDevice::_uploadMap(const uint8_t * mapdata, uint32_t size, const char * key)
{
    if(usb == 0) return;
    Packet_t command;
    Packet_t response;
    int cancel = 0;

    // ???
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x1C;
    command.size = 2;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);

    // read SD Ram capacity
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Command_Data;
    command.size = 2;
    *(uint16_t*)command.payload = Cmnd_Transfer_Mem;
    usb->write(command);

    while(usb->read(response)) {
        if(response.id == Pid_Capacity_Data) {
            cout << "free memory: " << dec << (((uint32_t*)response.payload)[1] / (1024*1024)) << " MB" << endl;
            uint32_t memory = ((uint32_t*)response.payload)[1];
            if(memory < size) {
                stringstream msg;
                msg << "Failed to send map: Unit has not enought memory (available/needed): " << memory << "/" << size << " bytes";
                throw exce_t(errRuntime,msg.str());
            }
        }
    }

    // send unlock key if present
    if(key) {
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Tx_Unlock_Key;
        command.size = strlen(key) + 1;
        memcpy(command.payload,key,command.size);

        usb->write(command);

        while(usb->read(response)) {
            if(response.id == Pid_Ack_Unlock_key) {
                //TODO read data
            }
        }

    }

    // switch to map transfer mode erase old map(?)
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 75;
    command.size = 2;
    *(uint16_t*)command.payload = 0x000A;
    usb->write(command);

    while(usb->read(response)) {
        if(response.id == 74) {
            //TODO read data
        }
    }

    callback(0,0,&cancel,"Upload maps ...",0);

    uint32_t total  = size;
    uint32_t offset = 0, chunkSize;
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 36;
    // transfer file by chunks of 0x1000 - 0x0000C - sizeof(offset) = 0x0FF0 bytes
    while(size && !cancel) {
        chunkSize       = (size < (GUSB_PAYLOAD_SIZE - sizeof(offset))) ? size : (GUSB_PAYLOAD_SIZE - sizeof(offset));
        command.size    = chunkSize + sizeof(offset);

        *(uint32_t*)command.payload = offset;
        memcpy(command.payload + sizeof(offset),mapdata,chunkSize);
        size    -= chunkSize;
        mapdata += chunkSize;
        offset  += chunkSize;

        usb->write(command);

        double progress = ((total - size) * 100.0) / total;
        callback(progress,0,&cancel,0,"Transfering map data.");
    }

    callback(100,0,&cancel,0,"done");

    // terminate map transfer mode (?)
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 45;
    command.size = 2;
    *(uint16_t*)command.payload = 0x000A;
    usb->write(command);
}


void CDevice::_uploadMap(const char * filename, uint32_t size, const char * key)
{
    if(usb == 0) return;
    Packet_t command;
    Packet_t response;
    int cancel = 0;

    // ???
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x1C;
    command.size = 2;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);

    // read SD Ram capacity
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Command_Data;
    command.size = 2;
    *(uint16_t*)command.payload = Cmnd_Transfer_Mem;
    usb->write(command);

    while(usb->read(response)) {
        if(response.id == Pid_Capacity_Data) {
            cout << "free memory: " << dec << (((uint32_t*)response.payload)[1] / (1024*1024)) << " MB" << endl;
            uint32_t memory = ((uint32_t*)response.payload)[1];
            if(memory < size) {
                stringstream msg;
                msg << "Failed to send map: Unit has not enought memory (available/needed): " << memory << "/" << size << " bytes";
                throw exce_t(errRuntime,msg.str());
            }
        }
    }

    // send unlock key if present
    if(key) {
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Tx_Unlock_Key;
        command.size = strlen(key) + 1;
        memcpy(command.payload,key,command.size);

        usb->write(command);

        while(usb->read(response)) {
            if(response.id == Pid_Ack_Unlock_key) {
                //TODO read data
            }
        }

    }

    // switch to map transfer mode erase old map(?)
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 75;
    command.size = 2;
    *(uint16_t*)command.payload = 0x000A;
    usb->write(command);

    while(usb->read(response)) {
        if(response.id == 74) {
            //TODO read data
        }
    }

    callback(0,0,&cancel,"Upload maps ...",0);

    FILE *fid = fopen(filename,"r");
    if(fid == NULL) {
        stringstream msg;
        msg << "Failed to send map: Can't open  " << filename;
        throw exce_t(errRuntime,msg.str());
    }

    uint32_t total  = size;
    uint32_t offset = 0, chunkSize;
    uint8_t  buffer[GUSB_PAYLOAD_SIZE - sizeof(offset)];

    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 36;
    // transfer file by chunks of 0x1000 - 0x0000C - sizeof(offset) = 0x0FF0 bytes
    while(size && !cancel) {
        chunkSize       = (size < (GUSB_PAYLOAD_SIZE - sizeof(offset))) ? size : (GUSB_PAYLOAD_SIZE - sizeof(offset));
        command.size    = chunkSize + sizeof(offset);

        fread(buffer, chunkSize, 1, fid);

        *(uint32_t*)command.payload = offset;
        memcpy(command.payload + sizeof(offset),buffer,chunkSize);
        size    -= chunkSize;
        offset  += chunkSize;

        usb->write(command);

        double progress = ((total - size) * 100.0) / total;
        callback(progress,0,&cancel,0,"Transfering map data.");
    }

    callback(100,0,&cancel,0,"done");

    // terminate map transfer mode (?)
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 45;
    command.size = 2;
    *(uint16_t*)command.payload = 0x000A;
    usb->write(command);
}


void CDevice::_queryMap(std::list<Map_t>& maps)
{
    maps.clear();
    if(usb == 0) return;

    Packet_t command;
    Packet_t response;

    // ???
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x1C;
    command.size = 2;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);

    // Request map overview table
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x59;
    command.size = 19;
    Map_Request_t * req = (Map_Request_t*)command.payload;
    req->dummy1 = 0;
    req->dummy2 = 10;
    strcpy(req->section,"MAPSOURC.MPS");
    usb->write(command);

    uint32_t size   = 1024;
    uint32_t fill   = 0;
    char * pData    = (char*)calloc(1,size);

    while(usb->read(response)) {
        // acknowledge request (???)
        if(response.id == 0x5B) {
            //TODO: read data
        }

        // chunk of MAPSOURC.MPS section
        // Each chunk is prepended by a chunk counter of type uint8_t.
        // This has to be skipped. That's why the peculiar math.
        if(response.id == 0x5A) {
            // realloc memory if chunk does not fit
            if((fill +  response.size - 1) > size) {
                size += size;
                pData = (char*)realloc(pData,size);
            }

            memcpy(&pData[fill], response.payload + 1, response.size - 1);

            fill += response.size - 1;
        }
    }

    Map_Info_t * pInfo = (Map_Info_t*)pData;
    while(pInfo->tok == 0x4C) {
        Map_t m;
        char * pStr = pInfo->name1;
        m.mapName = pStr;
        pStr += strlen(pStr) + 1;
        m.tileName = pStr;

        maps.push_back(m);

        pInfo =  (Map_Info_t*)(((char*)pInfo) + pInfo->size + sizeof(pInfo->tok) + sizeof(pInfo->size));
    }

    free(pData);

}


void CDevice::_downloadWaypoints(list<Garmin::Wpt_t>& waypoints)
{
    waypoints.clear();
    if(usb == 0) return;

    Packet_t command;
    Packet_t response;

    // ???
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x1C;
    command.size = 2;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);

    // request waypoints
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Command_Data;
    command.size = 2;
    *(uint16_t*)command.payload = Cmnd_Transfer_Wpt;
    usb->write(command);

    while(1) {
        if(!usb->read(response)) continue;

        if(response.id == Pid_Records) {
#ifdef DBG_SHOW_WAYPOINT
            cout << "number of waypoints:" << *(int16_t*)response.payload << endl;
#endif
        }

        if(response.id == Pid_Wpt_Data) {
            D109_Wpt_t * srcWpt = (D109_Wpt_t*)response.payload;
            waypoints.push_back(Wpt_t());
            Wpt_t& tarWpt = waypoints.back();

            tarWpt << *srcWpt;
        }

        if(response.id == Pid_Xfer_Cmplt) {
            break;
        }

    }

    // request proximity waypoints
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Command_Data;
    command.size = 2;
    *(uint16_t*)command.payload = Cmnd_Transfer_Prx;
    usb->write(command);

    while(1) {

        if(!usb->read(response)) continue;

        if(response.id == Pid_Records) {
            //TODO read data
#ifdef DBG_SHOW_WAYPOINT
            cout << "number of proximity waypoints:" << *(int16_t*)response.payload << endl;
#endif
        }

        if(response.id == Pid_Prx_Wpt_Data) {
            D109_Wpt_t * srcWpt = (D109_Wpt_t*)response.payload;
            waypoints.push_back(Wpt_t());
            Wpt_t& tarWpt = waypoints.back();

            tarWpt << *srcWpt;
        }

        if(response.id == Pid_Xfer_Cmplt) {
            break;
        }

    }

#ifdef DBG_SHOW_WAYPOINT
    list<Wpt_t>::const_iterator wpt = waypoints.begin();
    while(wpt != waypoints.end()) {
        cout << "-------------------------" << endl;
        cout << "class      " << hex << (int)wpt->wpt_class << endl;
        cout << "dspl_color " << hex << (int)wpt->dspl_color << endl;
        cout << "dspl_attr  " << hex << (int)wpt->dspl_attr << endl;
        cout << "smbl       " << dec <<(int)wpt->smbl << endl;
        cout << "lat        " << wpt->lat << endl;
        cout << "lon        " << wpt->lon << endl;
        cout << "alt        " << wpt->alt << endl;
        cout << "dpth       " << wpt->dpth << endl;
        cout << "dist       " << wpt->dist << endl;
        cout << "state      " << wpt->state << endl;
        cout << "cc         " << wpt->cc << endl;
        cout << "ete        " << wpt->ete << endl;
        cout << "temp       " << wpt->temp << endl;
        cout << "time       " << wpt->time << endl;
        cout << "category   " << wpt->wpt_cat << endl;
        cout << "ident      " << wpt->ident << endl;
        cout << "comment    " << wpt->comment << endl;
        cout << "facility   " << wpt->facility << endl;
        cout << "city       " << wpt->city << endl;
        cout << "addr       " << wpt->addr << endl;
        cout << "crossroad  " << wpt->crossroad << endl;

        ++wpt;
    }
#endif

}


void CDevice::_uploadWaypoints(std::list<Garmin::Wpt_t>& waypoints)
{
    if(usb == 0) return;
    // count number of proximity waypoints
    uint16_t prx_wpt_cnt = 0;
    list<Wpt_t>::const_iterator wpt = waypoints.begin();
    while(wpt != waypoints.end()) {
        if(wpt->dist != 1e25f) ++prx_wpt_cnt;
        ++wpt;
    }

    Packet_t command;
    Packet_t response;

    // ???
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x1C;
    command.size = 2;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);

    // transmit proximity waypoints first
    if(prx_wpt_cnt) {
        //announce number of records
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Records;
        command.size = 2;
        *(uint16_t*)command.payload = prx_wpt_cnt;
        usb->write(command);

        wpt = waypoints.begin();
        while(wpt != waypoints.end()) {
            if(wpt->dist != 1e25f) {
                command.type = GUSB_APPLICATION_LAYER;
                command.id   = Pid_Prx_Wpt_Data;

                D109_Wpt_t * p = (D109_Wpt_t *)command.payload;
                command.size = *wpt >> *p;

                usb->write(command);

            }
            ++wpt;
        }

        //announce number of records
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Xfer_Cmplt;
        command.size = 2;
        *(uint16_t*)command.payload = Cmnd_Transfer_Prx;
        usb->write(command);

    }

    //transmit _all_ waypoints
    //announce number of records
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Records;
    command.size = 2;
    *(uint16_t*)command.payload = waypoints.size();
    usb->write(command);

    wpt = waypoints.begin();
    while(wpt != waypoints.end()) {

        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Wpt_Data;

        D109_Wpt_t * p = (D109_Wpt_t *)command.payload;
        command.size = *wpt >> *p;

        usb->write(command);

        ++wpt;
    }

    //announce number of records
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Xfer_Cmplt;
    command.size = 2;
    *(uint16_t*)command.payload = Cmnd_Transfer_Wpt;
    usb->write(command);

}


void CDevice::_downloadTracks(std::list<Garmin::Track_t>& tracks)
{
    tracks.clear();
    if(usb == 0) return;

    Packet_t command;
    Packet_t response;

    // ???
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x1C;
    command.size = 2;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);

    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Command_Data;
    command.size = 2;
    *(uint16_t*)command.payload = Cmnd_Transfer_Trk;
    usb->write(command);

    int         trackidx = 0;
    string      name;
    Track_t *   track = 0;
    while(1) {

        if(!usb->read(response)) continue;

        if(response.id == Pid_Trk_Hdr) {
            trackidx = 0;
            D312_Trk_Hdr_t * hdr = (D312_Trk_Hdr_t*)response.payload;
            tracks.push_back(Track_t());
            track = &tracks.back();

            *track << *hdr;
            name  = hdr->ident;

        }

        if(response.id == Pid_Trk_Data) {
            D301_Trk_t * data = (D301_Trk_t*)response.payload;
            TrkPt_t pt;
            if(data->new_trk) {
                if(trackidx) {
                    tracks.push_back(Track_t());
                    Track_t& t = tracks.back();
                    t.color = track->color;
                    t.dspl = track->dspl;
                    char str[256];
                    sprintf(str,"%s_%d",name.c_str(),trackidx++);
                    t.ident = str;
                    track = &t;
                }
                else {
                    ++trackidx;
                }
            }

            pt << *data;
            track->track.push_back(pt);
        }

        if(response.id == Pid_Xfer_Cmplt) {
            break;
        }
    }
}


void CDevice::_uploadRoutes(list<Garmin::Route_t>& routes)
{
    if(usb == 0) return;
    // count number of proximity waypoints

    Packet_t command;
    Packet_t response;

    // ???
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x1C;
    command.size = 2;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);

    list<Garmin::Route_t>::const_iterator route = routes.begin();
    while(route != routes.end()) {
        //announce number of records
        // D202_Rte_Hdr_t + (D110_Wpt_t + D210_Tre_Link_t) * number of route points
        uint16_t nrec = 1 + route->route.size() * 2;
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Records;
        command.size = 2;
        *(uint16_t*)command.payload = nrec;
        usb->write(command);

        // write route header
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Rte_Hdr;
        D202_Rte_Hdr_t * r = (D202_Rte_Hdr_t *)command.payload;
        command.size = *route >> *r;
        usb->write(command);

        vector<RtePt_t>::const_iterator rtept = route->route.begin();

        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Rte_Wpt_Data;
        D109_Wpt_t * p = (D109_Wpt_t *)command.payload;
        command.size = *rtept >> *p;
        usb->write(command);

        ++rtept;

        while(rtept != route->route.end()) {

            command.type = GUSB_APPLICATION_LAYER;
            command.id   = Pid_Rte_Link_Data;
            D210_Rte_Link_t * l = (D210_Rte_Link_t *)command.payload;
            command.size = *rtept >> *l;
            usb->write(command);

            command.type = GUSB_APPLICATION_LAYER;
            command.id   = Pid_Rte_Wpt_Data;
            D109_Wpt_t * p = (D109_Wpt_t *)command.payload;
            command.size = *rtept >> *p;
            usb->write(command);

            ++rtept;
        }

        // finish block
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Xfer_Cmplt;
        command.size = 2;
        *(uint16_t*)command.payload = Cmnd_Transfer_Rte;
        usb->write(command);
        ++route;
    }

}


void CDevice::_uploadCustomIcons(list<Garmin::Icon_t>& icons)
{
    if(usb == 0) return;

    Packet_t command;
    Packet_t response;

    // ???
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x1C;
    command.size = 2;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);

    list<Garmin::Icon_t>::const_iterator icon = icons.begin();
    while(icon != icons.end()) {
        uint32_t tan = 0;

        // get tan
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Req_Icon_Id;
        command.size = 2;
        *(uint16_t*)command.payload = icon->idx + 1;
        usb->write(command);

        while(usb->read(response)) {
            if(response.id == Pid_Ack_Icon_Id) {
                tan = *(uint32_t*)response.payload;
            }
        }

        // request color table
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Req_Clr_Tbl;
        command.size = 4;
        *(uint32_t*)command.payload = tan;
        usb->write(command);

        while(usb->read(response)) {
            if(response.id == Pid_Ack_Clr_Tbl) {
                // send back color table
                command = response;
            }
        }

        usb->write(command);
        while(usb->read(response)) {
            if(response.id == Pid_Req_Clr_Tbl) {
                // TODO: ignore?
            }
        }

        // send icon data
        command.type = GUSB_APPLICATION_LAYER;
        command.id   = Pid_Icon_Data;
        command.size = 0x104;
        *(uint32_t*)command.payload = tan;
        memcpy(command.payload + sizeof(tan),icon->data,sizeof(icon->data));
        usb->write(command);

        while(usb->read(response)) {
            if(response.id == Pid_Ack_Icon_Data) {
                // TODO: ignore?
            }
        }

        ++icon;
    }
}


void CDevice::_screenshot(char *& clrtbl, char *& data, int& width, int& height)
{
    if(usb == 0) return;

    Packet_t command;
    Packet_t response;

    // ???
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x1C;
    command.size = 2;
    *(uint16_t*)command.payload = 0x0000;
    usb->write(command);

    uint32_t tan = 0;

    // get tan
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Req_Icon_Id;
    command.size = 2;
    *(uint16_t*)command.payload = 0;
    usb->write(command);

    while(usb->read(response)) {
        if(response.id == Pid_Ack_Icon_Id) {
            tan = *(uint32_t*)response.payload;
        }
    }

    // request color table
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Req_Clr_Tbl;
    command.size = 4;
    *(uint32_t*)command.payload = tan;
    usb->write(command);

    while(usb->read(response)) {
        if(response.id == Pid_Ack_Clr_Tbl) {
            // send back color table
            memcpy(aClrtbl,_clrtbl,sizeof(aClrtbl));
            command = response;
        }
    }

    usb->write(command);
    while(usb->read(response)) {
        if(response.id == Pid_Req_Clr_Tbl) {
            // TODO: ignore?
        }
    }

    char buffer[SCREEN_WIDTH * SCREEN_HEIGHT];
    char * pData = buffer;
    uint32_t byteCnt;
    uint32_t byteCntTotal = 0;

    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Ack_Icon_Data;
    command.size = 4;
    *(uint32_t*)command.payload = tan;
    usb->write(command);

    // loop will end after reception of Pid_Icon_Data with length of 4 (tan only)
    // or too much data
    while(1) {
        if(!usb->read(response)) {
            usb->write(command);
            continue;
        }

        if(response.id == Pid_Icon_Data) {
            if(response.size == sizeof(tan)) break;
            byteCnt = response.size - sizeof(tan);
            memcpy(pData,response.payload + sizeof(tan), byteCnt);
            pData += byteCnt;
            byteCntTotal += byteCnt;
            if(byteCntTotal > sizeof(buffer)) break;
        }
    }

    command.type = GUSB_APPLICATION_LAYER;
    command.id   = 0x373;
    command.size = 4;
    *(uint32_t*)command.payload = tan;
    usb->write(command);

    for(int r = 0; r < SCREEN_HEIGHT; ++r) {
        for(int c = 0; c < SCREEN_WIDTH; ++c) {
            aScreen[r * SCREEN_WIDTH + c] = buffer[(SCREEN_HEIGHT - 1 - r)*SCREEN_WIDTH + c];
        }
    }

    clrtbl  = aClrtbl;
    data    = aScreen;
    width   = SCREEN_WIDTH;
    height  = SCREEN_HEIGHT;
}


void CDevice::_setRealTimeMode(bool on)
{
    CMutexLocker lock(dataMutex);
    if(doRealtimeThread == on) return;
    doRealtimeThread = on;
    if(doRealtimeThread) {
        pthread_create(&thread,NULL,rtThread, this);
    }

}


void CDevice::_getRealTimePos(Garmin::Pvt_t& pvt)
{
    if(pthread_mutex_trylock(&mutex) != EBUSY) {
        pthread_mutex_unlock(&mutex);
        throw exce_t(errRuntime,lasterror);
    }

    CMutexLocker lock(dataMutex);
    pvt = PositionVelocityTime;
}


void CDevice::_release()
{
    if(usb == 0) return;

    usb->close();
    delete usb;
    usb = 0;
}


void CDevice::_getDevProperties(Garmin::DevProperties_t& dev_properties)
{
    if(usb == 0) return;
    Packet_t command;
    Packet_t response;

    // ask for SD Ram capacity
    command.type = GUSB_APPLICATION_LAYER;
    command.id   = Pid_Command_Data;
    command.size = 2;
    *(uint16_t*)command.payload = Cmnd_Transfer_Mem;
    usb->write(command);

    // try to read SD Ram capacity
    uint32_t memory = 0;
    uint16_t tile_limit = 0;
    while(usb->read(response)) {
        if(response.id == Pid_Capacity_Data) {
            tile_limit = ((uint16_t*)response.payload)[1];
            memory = ((uint32_t*)response.payload)[1];
        }
    }
    if(tile_limit == 0) {
        throw exce_t(errRuntime,"Failed to send map: Unable to find the tile limit of the GPS");
    }
    if(memory == 0) {
        throw exce_t(errRuntime,"Failed to send map: Unable to find the available memory of the GPS");
    }

    // add to the properties list
    properties.memory_limit = memory;
    properties.set.item.memory_limit = 1;
    properties.maps_limit = tile_limit;
    properties.set.item.maps_limit = 1;

    // return the properties
    dev_properties = properties;
}
