#include "progbob-armswd.h"
#include "serialport/serialport.hpp"

#include <iostream>
#include <string.h>
#include <boost/crc.hpp>      // for boost::crc_basic, boost::crc_optimal
//#include <boost/cstdint.hpp>  // for boost::uint16_t

#define PROGBOB_PAGESIZE 2048
#define PROGBOB_CHUNKSIZE 64

// external progress...
void doAvrdudeProgress(int percent, double etime) ;

// helper (avrlibc function _crc_ccitt_update())
// Polynomial: x^16 + x^12 + x^5 + 1 (0x8408)
// Initial value: 0xffff
static uint16_t crc_ccitt_update (uint16_t crc, uint8_t data) {
    data ^= (crc & 0xff);
    data ^= data << 4;
    return ((((uint16_t)data << 8) | ((crc>>8)&0xff)) ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3));
}



class ProgBob_ARMSWD_priv {
private:
    static const size_t buffer_size = 4096;
    char buffer[buffer_size];
    size_t buffer_pos;
    static const size_t txbuffer_size = 512;
    char txbuffer[txbuffer_size];
protected:
    SerialPortController & spc;
    SerialPort * serialport;
    
    ProgBob_ARMSWD_priv();
    std::string readline(unsigned int timeout_ms);
    void drain_debug();
    void signalError();
    void checkError(const std::string & line, const std::string & text);

    std::string getPageCRC(unsigned int pageAddress);
    
    friend class ProgBob_ARMSWD;
};


//////////////////////// private OBJECT


ProgBob_ARMSWD_priv::ProgBob_ARMSWD_priv() : spc(SerialPortController::singleton()) {
    
}

std::string  ProgBob_ARMSWD_priv::readline(unsigned int timeout_ms) {
    std::string result;
    size_t search_pos=0;
    while (timeout_ms--) {
        int len = serialport->read(buffer+buffer_pos, buffer_size-buffer_pos, 1);
        if (len<0) throw std::runtime_error("Read Error");
        if (len>0) {
            buffer_pos+=len;
        }
        for (size_t pos=search_pos; pos<buffer_pos; pos++) {
            if (buffer[pos]=='\n') {
                // endline detected!!!
                std::string result(buffer, int(pos));
                memmove(buffer, buffer+pos+1, buffer_pos-pos-1);
                buffer_pos = buffer_pos-pos-1;
                return result;
            }
        }
        search_pos=buffer_pos;
    }
    std::cout << "Timeout" << std::endl;
    throw std::runtime_error("Timeout");
}

void ProgBob_ARMSWD_priv::drain_debug() {
    try {
        std::string line;
        do {
            line = readline(100);
            std::cout << "RCV: '" << line << "'" << std::endl;
        } while (line!="OK");
    } catch (...) {
    }
}

void  ProgBob_ARMSWD_priv::signalError() {
    try {
        if (serialport) {
            serialport->write("+ERROR\n", 7);
            drain_debug();
        }
    } catch (...) {
    }
}


std::string ProgBob_ARMSWD_priv::getPageCRC(unsigned int pageAddress) {
    sprintf(txbuffer, "+SWD-CRCPAGE %08X\n", (pageAddress));
    //std::cout << "READY TO SEND: " << txbuffer << std::flush;
    serialport->write(txbuffer, strlen(txbuffer));
    std::string crc = "";
    std::string line;
    do {
        line = readline(1000);
        checkError(line, "Error during upload (CRCPAGE)!");
        //std::cout << "RCV: '" << line << "'" << std::endl;
        if (line.substr(0,6)=="CRC=0x") {
            crc = line.substr(6,8);
        }
    } while (line!="OK");
    return crc;
}


void  ProgBob_ARMSWD_priv::checkError(const std::string & line, const std::string & text) {
    if (line.substr(0,5)=="ERROR") {
        std::cout << line << std::endl;
        drain_debug();
        throw std::runtime_error(text);
    }
}

//////////////////////// public OBJECT


ProgBob_ARMSWD::ProgBob_ARMSWD() : ProgrammerBase(), priv(new ProgBob_ARMSWD_priv()) { 
    priv->serialport = 0;
    priv->buffer_pos = 0;
    std::cout << "serport count = " << priv->spc.getNoPorts() << std::endl;
}


ProgBob_ARMSWD::~ProgBob_ARMSWD() {
    delete priv->serialport;
    delete priv;
}


void ProgBob_ARMSWD::connect(const std::string & port) {
    if (port=="") {
        if (priv->serialport) {
            // 1.) reset baudrate, 2.) close
            priv->serialport->setBaudrate(19200);
            delete priv->serialport;
            priv->serialport = 0;
        }
    } else {
        int index = priv->spc.findPort(port.c_str());
        std::cout << "serport index for '" << port << "' = " << index << std::endl;
        priv->serialport = priv->spc.open(port.c_str(), 300);
        if (!priv->serialport) throw std::runtime_error("Unable to open serial port!");
        priv->serialport->write("+VERSION\n", 9);
        try {
            while (1) {
                std::string line = priv->readline(500);
                std::cout << "RCV: '" << line << "'" << std::endl;
                if (line.substr(0,7)=="PROGBOB") {
                    // success
                    priv->drain_debug();
                    return;
                }
            }
        } catch (const std::exception & e) {
            std::cout << "EXCEPTION '" << e.what() << "'" << std::endl;
            if (e.what()==std::string("Timeout")) throw std::runtime_error("Progbob firmware to old, new protocol version not supported!");
            throw;
        }
    }
}

void ProgBob_ARMSWD::configure(const std::string & part) {
    
}

void ProgBob_ARMSWD::signalError() {
    std::cout << "ProgBob_ARMSWD::signalError()" << std::endl;
    priv->signalError();
}

std::string ProgBob_ARMSWD::getStatus() {
    std::cout << "ProgBob_ARMSWD::getStatus()" << std::endl;
    priv->serialport->write("+STATUS\n", 8);
    char buffer[100];
    int len = priv->serialport->read(buffer, sizeof(buffer), 100);
    if (len>4) {
        std::string value(buffer, len-4);
        std::string value_ok(buffer+len-4, 4);
        if (value_ok=="\nOK\n") {
            std::cout << "OK - value='" << value << "'" << std::endl;
            return value;
        }
    }
    if (len>0) {
        std::cout << "Error - RCV: '" <<  std::string(buffer, len) << "'" << std::endl;
    } else {
        std::cout << "Error - result=" << len << std::endl;
    }
    return "";
}


void ProgBob_ARMSWD::reset() {
    std::cout << "ProgBob_ARMSWD::reset()" << std::endl;
    priv->serialport->write("+RESET\n", 8);
    std::string line;
    do {
        line = priv->readline(1000);
        //std::cout << "RCV: '" << line << "'" << std::endl;
    } while (line!="OK");
}


void ProgBob_ARMSWD::program(const std::string & segment, unsigned int startAddress, const std::vector<uint8_t> & _memory) {
    std::vector<uint8_t> memory(_memory);
    char buffer[512];
    doAvrdudeProgress(0, 0);
    
    int pages = ((memory.size()-1)/0x800) + 1;
    memory.resize(pages*0x800, 0xff);
    
    unsigned int endAddress = startAddress+memory.size();
    std::cout << "ProgBob_ARMSWD::program() segment=" << segment << " startAddress=0x" << std::hex << startAddress << " endAddress=0x" << endAddress << std::dec << std::endl;
    if (segment=="flash") {
        unsigned int pageAddress = startAddress&0xfffff800;
        if (pageAddress!=startAddress) throw std::runtime_error("Flash programmming must be aligned to page boundaries!");
        unsigned int pageAddressLast = (endAddress-1)&0xfffff800;
        while (pageAddress <= pageAddressLast) {
            std::string pageCRC32 = priv->getPageCRC(pageAddress);
            // std::cout << "pageCRC32: " << pageCRC32 << std::endl;
            // crc for 0x800 * 0xff: cebd6be1
            
            
            //                         0x04C11DB7, 0x00000000, 0x00000000, ????, true
            boost::crc_basic<32> crc32(0x04C11DB7, 0x00000000, 0x00000000, true, true);
            for (unsigned int i=0; i<0x800; i++) {
                crc32.process_byte(memory[pageAddress+i]);
            }
            // std::cout << "calculated crc32: " << std::hex << crc32.checksum() << std::dec << std::endl;
            sprintf(buffer, "%08x", crc32.checksum());
            if (pageCRC32 == buffer) {
                std::cout << "skipping page address=0x" << std::hex << pageAddress << std::dec << std::endl;
            } else {
                std::cout << "programming page address=0x" << std::hex << pageAddress << std::dec << std::endl;
            
                unsigned int offset = 0;
                while ((offset<0x800)&&(pageAddress+offset < endAddress)) {
                    uint16_t crc = 0xffff;
                    sprintf(buffer, "+SWD-STOREBUF %04X ", offset);
                    crc = crc_ccitt_update(crc, (offset>>8)&0xff);
                    crc = crc_ccitt_update(crc, (offset>>0)&0xff);
                    unsigned int boffset = 0;
                    while ((boffset<PROGBOB_CHUNKSIZE)&&(pageAddress+offset+boffset < endAddress)) {
                        uint8_t value = memory[pageAddress+offset+boffset];
                        sprintf(buffer+19+2*boffset, "%02X", (int)value);
                        crc = crc_ccitt_update(crc, value);
                        boffset++;
                    }
                    sprintf(buffer+19+2*boffset, " %04X\n", crc);
                    //std::cout << "READY TO SEND: " << buffer << std::flush;
                    priv->serialport->write(buffer, strlen(buffer));
                    std::string line;
                    do {
                        line = priv->readline(1000);
                        priv->checkError(line, "Error during upload (STOREBUF)!");
                        //std::cout << "RCV: '" << line << "'" << std::endl;
                    } while (line!="OK");
                    offset += PROGBOB_CHUNKSIZE;
                }
                
                sprintf(buffer, "+SWD-FLASHPAGE %08X %04X\n", (pageAddress), 0x800);
                //std::cout << "READY TO SEND: " << buffer << std::flush;
                priv->serialport->write(buffer, strlen(buffer));
                std::string line;
                do {
                    line = priv->readline(1000);
                    //std::cout << "RCV: '" << line << "'" << std::endl;
                    priv->checkError(line, "Error during upload (FLASHPAGE)!");
                } while (line!="OK");
            }
                
            pageAddress += 0x800; // 2k
            doAvrdudeProgress(100*(pageAddress-startAddress)/(endAddress-startAddress), 0);
        }
    }
    std::cout << "ProgBob_ARMSWD::program() finished" << std::endl;
}


void ProgBob_ARMSWD::verify(const std::string & segment, unsigned int startAddress, const std::vector<uint8_t> & _memory) {

}


void ProgBob_ARMSWD::startProgramMode(const std::string & mode, bool erase) {
    std::string line;
    
    try {
        bool need_unlock = false;
        priv->serialport->write("+SWD-INIT\n", 10);
        do {
            line = priv->readline(100);
            std::cout << "RCV: '" << line << "'" << std::endl;
            if (line=="IDR: AAP locked") need_unlock = true;
        } while (line!="OK");
        
        if (!erase && need_unlock) {
            throw std::runtime_error("Device is locked!");
        }

        if (erase && need_unlock) {
            std::cout << "ProgBob_ARMSWD::startProgramMode - unlock device... " << std::endl;
            priv->serialport->write("+SWD-UNLOCK\n", 12);
            do {
                line = priv->readline(100);
                std::cout << "RCV: '" << line << "'" << std::endl;
            } while (line!="OK");

            priv->serialport->write("+SWD-INIT\n", 10);
            do {
                line = priv->readline(100);
                std::cout << "RCV: '" << line << "'" << std::endl;
                if (line=="IDR: AAP locked") {
                    throw std::runtime_error("Unable to unlock device!");
                }
            } while (line!="OK");
            
        }
        
        std::cout << "ProgBob_ARMSWD::startProgramMode - success " << std::endl;
    } catch (const std::exception & e) {
        std::cout << "exception: " << e.what() << std::endl;
    }
    // +SWD-UNLOCK
    
}

void ProgBob_ARMSWD::leaveProgramMode() {
    
}

std::string ProgBob_ARMSWD::readSignature() {
    return "";
}
