#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_PING        = 0x30;
static constexpr uint8_t OP_CLEAR_FLAGS = 0x40;

static void shift48(DigilentJtag &jtag, const uint8_t tx[6], uint8_t rx[6]) {
  jtag.shiftData(tx, rx, 48);
}

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

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

struct Resp48 {
  uint8_t last_op;
  uint8_t flags;
  uint8_t data;
  uint8_t cmd_seq;
  uint8_t status;
  uint8_t resp_seq;
};

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_cmd(jtag, OP_CLEAR_FLAGS, 0, 0);
    // Check for ping
    if(do_cmd(jtag, OP_PING, 0, 0) != 0xa5){
        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_cmd(jtag, OP_RESET_ON, 0, 0);

    int nr = 0;
    int addr = 0;
    do{
        uint8_t buf[64];
        nr = fread(buf, 1, 64, f);
        for(int i=0; i<nr; i++){
            do_cmd(jtag, OP_WRITE8, addr+i, buf[i]);
            printf(".");
        }
        printf("\r\n");

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

        addr += nr;
    }while(nr > 0);
    
    do_cmd(jtag, OP_RESET_OFF, 0, 0);

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