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

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

void write(DigilentJtag &jtag, uint32_t addr, uint8_t data, bool reset){
    uint8_t d[6], din[6];
    d[0] = data;
    d[1] = (uint8_t)addr;
    d[2] = (uint8_t)(addr>>8);
    d[3] = (uint8_t)(addr>>16);
    d[4] = (uint8_t)(addr>>24);
    d[5] = ((reset) ? 1 : 0) | 0x2; // <we><rst>

    jtag.shiftData(d, din, 42);
}

uint8_t read(DigilentJtag &jtag, uint32_t addr, bool reset){
    uint8_t d[6], din[6];
    d[0] = 0xff;
    d[1] = (uint8_t)addr;
    d[2] = (uint8_t)(addr>>8);
    d[3] = (uint8_t)(addr>>16);
    d[4] = (uint8_t)(addr>>24);
    d[5] = ((reset) ? 1 : 0); // <we><rst>

    jtag.shiftData(d, din, 42);
    // Read back
    jtag.shiftData(d, din, 42);

    return din[0];
}

int main(int argc, char** argv){
    ArgParser parser(argc > 0 ? argv[0] : "test");
    parser.addString("write", "", "file to write");

    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;
    }

    const std::string arg_write = parser.getString("write");

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

    // Start reset
    read(jtag, 0, true);

    if(arg_write!=""){
        uint32_t addr = 0;
        uint8_t buf[32];
        int nr;
        FILE* f = fopen(arg_write.c_str(), "rb");
        if(!f){
            goto end;
        }

        do{
            nr = fread(buf, 1, 32, f);
            for(int i=0; i<32; i++){
                write(jtag, addr, buf[i], true);
                addr++;
            }
        }while(nr>0);

        fclose(f);
    }


end:
    // End reset
    read(jtag, 0, false);

    jtag.close();
    return 0;
}