#include "digilent_jtag.hpp"
#include "argparse.hpp"

#include <algorithm>
#include <cstdio>
#include <string>

static constexpr uint8_t OP_NOP         = 0x00;
static constexpr uint8_t OP_RESET_ON    = 0x10;
static constexpr uint8_t OP_RESET_OFF   = 0x11;
static constexpr uint8_t OP_WRITE8      = 0x20;
static constexpr uint8_t OP_READ8       = 0x21;
static constexpr uint8_t OP_WRITE32     = 0x22;
static constexpr uint8_t OP_READ32      = 0x23;
static constexpr uint8_t OP_PING        = 0x30;
static constexpr uint8_t OP_CLEAR_FLAGS = 0x40;

static void shift72(DigilentJtag &jtag, const uint8_t tx[9], uint8_t rx[9]) {
  jtag.shiftData(tx, rx, 72);
}

static void make_cmd(uint8_t out[9], uint8_t opcode, uint32_t addr, uint32_t data) {
  out[0] = (uint8_t)data;
  out[1] = (uint8_t)(data >> 8);
  out[2] = (uint8_t)(data >> 16);
  out[3] = (uint8_t)(data >> 24);
  out[4] = (uint8_t)addr;
  out[5] = (uint8_t)(addr >> 8);
  out[6] = (uint8_t)(addr >> 16);
  out[7] = (uint8_t)(addr >> 24);
  out[8] = opcode;
}

static uint32_t get_data32(const uint8_t rx[9]) {
    return ((uint32_t)rx[2]) |
           ((uint32_t)rx[3] << 8) |
           ((uint32_t)rx[4] << 16) |
           ((uint32_t)rx[5] << 24);
}

static uint32_t do_cmd32(DigilentJtag& jtag, uint8_t opcode, uint32_t addr, uint32_t data){
    uint8_t tx[9], rx[9];
    make_cmd(tx, opcode, addr, data);
    shift72(jtag, tx, rx);
    for(int i=0; i<32; i++){
        make_cmd(tx, OP_NOP, 0, 0);
        shift72(jtag, tx, rx);
        if(rx[0] == opcode){
            return get_data32(rx);
        }
    }
    printf("Could not do command\r\n");
    return 0;
}

static inline void write8(DigilentJtag& jtag, uint32_t addr, uint8_t value) {
    (void)do_cmd32(jtag, OP_WRITE8, addr, value);
}

static inline uint8_t read8(DigilentJtag& jtag, uint32_t addr) {
    return (uint8_t)do_cmd32(jtag, OP_READ8, addr, 0);
}

static inline void write32(DigilentJtag& jtag, uint32_t addr, uint32_t value) {
    (void)do_cmd32(jtag, OP_WRITE32, addr, value);
}

static inline uint32_t read32(DigilentJtag& jtag, uint32_t addr) {
    return do_cmd32(jtag, OP_READ32, addr, 0);
}

int main(int argc, char** argv){
    ArgParser parser(argc > 0 ? argv[0] : "test");
    parser.addString("file", "", "File to write", true, "f");
    parser.addFlag("verify", "Verify", "v");

    std::string parse_error;
    if (!parser.parse(argc, argv, &parse_error)) {
        if (parse_error == "help") {
            std::printf("%s", parser.helpText().c_str());
            return 0;
        }
        std::printf("Argument error: %s\n\n", parse_error.c_str());
        std::printf("%s", parser.helpText().c_str());
        return -1;
    }

    DigilentJtag jtag;
    if(!jtag.open()){
        printf("Could not open programmer\r\n");
        return -1;
    }
    jtag.setChain(1);


    do_cmd32(jtag, OP_CLEAR_FLAGS, 0, 0);
    // Check for ping
    if((do_cmd32(jtag, OP_PING, 0, 0) & 0xffu) != 0xa5u){
        printf("PING response was not right\r\n");
        jtag.close();
        return -1;
    }

    const std::string file = parser.getString("file");
    FILE* f = fopen(file.c_str(), "rb");
    if(!f){
        printf("Could not open file\r\n");
        jtag.close();
        return -1;
    }

    do_cmd32(jtag, OP_RESET_ON, 0, 0);

    int nr = 0;
    int addr = 0;
    do{
        uint32_t buf[32];
        nr = fread(buf, sizeof(uint32_t), 32, f);
        for(int i=0; i<nr; i++){
            write32(jtag, addr+(i*4), buf[i]);
            printf(".");
        }
        printf("\r\n");

        if(parser.getFlag("verify")){
            for(int i=0; i<nr; i++){
                uint32_t r = read32(jtag, addr+(i*4));
                if(r!=buf[i]){
                    printf(" -- Verify failed at %04x : %08x != %08x\r\n", addr+(i*4), r, buf[i]);
                }
            }
        }

        addr += nr*4;
    }while(nr > 0);
    
    do_cmd32(jtag, OP_RESET_OFF, 0, 0);

    fclose(f);
    jtag.close();
    return 0;
}