#include <iostream>
#include <iomanip>
#include <memory>
#include <thread>
#include <chrono>
#include "motorboard.h"
#include "filetools.h"
#include "stringex.hpp"
#include <functional>
#include "uartregsaccess.h"
#include "Gpio.h"
#include "StopWatch.h"

using namespace std;

volatile int done_scan = 0;

int menu()
{
    int choice;

    cout << " **** Menu **** " << endl
         << endl;
    cout << "(1)  start." << endl;
    cout << "(2)  stop. " << endl;
    cout << "(3)  clear error. " << endl;
    cout << "(4)  pick paper. " << endl;
    cout << "(5)  regs list. " << endl;
    cout << "(6)  scan num. " << endl;
    cout << "(7)  scan auto" << endl;
    cout << "(0) Quit. " << endl
         << endl;
    cout << ": ";
    cin >> choice;
    return choice;
}

int main()
{
    FileTools ft("/home/linaro/regs.log");
    ft.clear();
    MotorBoard mb;
    unsigned int val = 0;
    //GpioOut gpio(151);
    bool exit = false;
    int option = 0;
    for (;;)
    {
        option = menu();
        switch (option)
        {
        case 0:
            exit = true;
            break;
        case 1:{
            mb.start();
            break;
        }
        case 2:
            mb.stop();
            break;
        case 3:
            mb.clear_error();
            break;
        case 4:
            mb.pick_paper();
            break;
        case 5:
        {
            for (int i = 0; i < 8; i++)
            {
                StopWatch sw;
                // gpio.setValue(Gpio::Low);
                // gpio.setValue(Gpio::High);
                if (mb.read(i, val))
                {
                    // gpio.setValue(Gpio::Low);
                    std::cout << string_format("read : %f\n", sw.elapsed_ms());
                    std::cout << hex;
                    std::cout << "reg[0x" << i << "] =0x" << setw(8) << setfill('0') << val << std::endl;
                }
                else
                {
                    std::cout << "read regs error " << std::endl;
                }
            }
        }
        break;
        case 6:
            // cout << "scan num: " << mb.paper_counter() << endl;
            break;
        case 7:
        {

            auto mb_error_call = [&](unsigned int error_code)
            {
                std::cout << __func__ << " call:  error" << std::endl;
                done_scan = -1;
                mb.stop();
            };

            auto mb_scandone_call = [&]()
            {
                std::cout << __func__ << " call:  done" << std::endl;
                done_scan = 1;
            };

            auto mb_osmode_call = [&](unsigned int osmode)
            {
                printf("motorboard callback mb_osmode_call changed");
            };

            MotorBoardGlue mb_glue = {mb_error_call, mb_scandone_call, mb_osmode_call,nullptr,nullptr,nullptr};
            mb.set_callbacks(mb_glue);
            done_scan = 0;
            mb.start();
            mb.set_speed_mode(1);
            StopWatch sw;
            for (;;)
            {
                if (mb.wait_paper_out(3000))
                {
                    // std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    std::cout << __func__ << " call:  paper pick" << std::endl;
                    mb.pick_paper();
                    sw.reset();
                }
                else if (done_scan)
                {
                    break;
                }
                else
                {
                    if (sw.elapsed_s() > 3)
                        done_scan = -2;
                }
            }
            std::cout << __func__ << " call:  exit" << std::endl;
        }
        break;
        case 100:
        {
            unsigned int vals[2] = {0xaaaaaaaa, 0x55555555};
            unsigned int valw;
            unsigned int valr;
            const unsigned int addr = 7;
            std::cout << "start verify" << endl;
            // mb.read(addr, valr);
            // mb.read(addr, valr);
            // mb.read(addr, valr);
            // mb.read(addr, valr);
            for (int i = 0; i < 1000; i++)
            {
                valw = vals[i % 2];

                mb.write(addr, valw);
                // std::this_thread::sleep_for(std::chrono::milliseconds(2));
                mb.read(addr, valr);
                std::cout << "\r" << i;
                std::cout.flush();
                if (valw != valr)
                    ft.append_log(string_format("%d error", i));
                else
                    ft.append_log(string_format("0x%08X:0x%08X", valw, valr));
            }
        }
        break;
        default:
            cout << "Please select again! " << endl;
            break;
        }
        /* code */
        if (exit)
            break;
    }

    cout << "exit munu" << endl;

    return 0;
}