#include <iostream>
#include <iomanip>
#include <memory>
#include <thread>
#include <chrono>
#include "filetools.h"
#include "stringex.hpp"
#include <functional>
#include "uartregsaccess.h"
#include "config.h"
#include "StopWatch.h"
#include "applog.h"
#include "usbservice.h"
#include <string.h>

using namespace std;

int menu() {
	int choice;

	cout << " **** Menu **** " << endl << endl;
    cout << "(1) cap regs list." << endl;
    cout << "(2) motor regs list." << endl;
    cout << "(3) test write read regs" << endl;
	cout << "(0) Quit. " << endl << endl;
	cout << ": ";
	cin >> choice;
	return choice;
}

void list_regs(std::shared_ptr<IRegsAccess>& regs)
{
    StopWatch sw;
    unsigned int val;
    for (int i = 0; i < 0x11; i++)
    {
        sw.reset();
        val = 0;
        if (regs->read(i, val))
            std::cout << string_format("reg[0x%d] =0x%08x, read elapsed: %f\n", i, val, sw.elapsed_ms());
        else
            std::cout << string_format("reg[0x%d] read error\n", i);
    }
}


void test_regs(std::shared_ptr<IRegsAccess>& regs)
{
    StopWatch sw;
    unsigned int val;
    for (int i = 0; i < 1000; i++)
    {
        sw.reset();
        val = 0;
        if(regs->write(0,i))
        {
            std::this_thread::sleep_for(chrono::milliseconds(10));
            sw.reset();
            std::cout << string_format("reg[0x%d] =0x%08x, write elapsed: %f\n", 0, i, sw.elapsed_ms());
            if (regs->read(0, val))
                std::cout << string_format("reg[0x%d] =0x%08x, read elapsed: %f\n", i, val, sw.elapsed_ms());
            else
                std::cout << string_format("reg[0x%d] read error\n", 0);
        }
        else
        {
            std::cout << string_format("reg[0x%d] write error\n", 0);
        }
    }
}


int main()
{
    auto capregs = std::shared_ptr<IRegsAccess>(new UartRegsAccess(FPGA_UART, 921600, 0x03, 0x83));
    auto motorregs = std::shared_ptr<IRegsAccess>(new UartRegsAccess(MOTOR_UART, 921600, 0x07, 0x87));
    UsbService us(capregs, motorregs);
    unsigned int val = 0;
    bool exit = false;
    int option = 0;
    for (;;)
    {
        option = menu();
        switch (option)
        {
        case 0:
            exit = true;
            break;
        case 1:
        {
            std::cout << "cap regs list:" << std::endl;
            list_regs(capregs);
        }
            break;

        case 2:
        {  
            std::cout << "motor regs list:" << std::endl;
            list_regs(motorregs);
        }
        break;
        case 3:
            test_regs(capregs);
        break;
        // case 100:
        // {
        //     unsigned int vals[2] = {0xaaaaaaaa, 0x55555555};
        //     unsigned int valw;
        //     unsigned int valr;
        //     const unsigned int addr = 7;
        //     for (int i = 0; i < 1000; i++)
        //     {
        //         valw = vals[i % 2];
        //         regs->write(addr, valw);
        //         regs->read(addr, valr);
        //         if (valw != valr)
        //             std::cout << string_format("%d error\n", i);
        //         else
        //             std::cout << string_format("0x%08X:0x%08X\n", valw, valr);
        //     }
        // }
        // break;
        default:
            std::cout << "Please select again! \n" << endl;
            break;
        }
        /* code */
        if (exit)
            break;
    }

    std::cout << "exit munu";

    return 0;
}