#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"
#include "wakeup.hpp"

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 << "(8)  set sensor pwm duty " << endl;
    cout << "(9)  set sensor enable" << endl;
    cout << "(10)  set ultrasonic param" << endl;
    cout << "(11)  get ultrasonic version" << endl;

    cout << "(0) Quit. " << endl
         << endl;
    cout << ": ";
    cin >> choice;
    return choice;
}

int main()
{
    FileTools ft("/home/linaro/regs.log");
    ft.clear();
    WakeUp wake;
    MotorBoard mb(nullptr);
    wake.setinitcallback([&mb]{
		std::this_thread::sleep_for(std::chrono::milliseconds(3000));
		mb.init_statecontrol();
	},[&mb]{
		mb.release_statecontrol();
	});
    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({0});
            break;
        }
        case 2:
            mb.stop();
            break;
        case 3:
            mb.clear_error();
            break;
        case 4:{
            {
                int size = 20;
                std::cin >> size;
                for(int x = 0;x< size;x++){
                    mb.pick_paper();
                    if(x > 20  && ((x%21) == 0) )
                    {
                        wake.power_off();
                        this_thread::sleep_for(chrono::seconds(5));
                        wake.power_on();
                        this_thread::sleep_for(chrono::seconds(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;
                            }
                        }
                    }
                    std::this_thread::sleep_for(std::chrono::milliseconds(500));
                    std::cout << "send == "<< x  << std::endl;
                }
            } 
            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");
            };

            auto mb_set_sleeptime_call = [&](unsigned int speedmode)
            {
                printf("\n mb_set_sleeptime_call val= %d ", speedmode);
            };
            MotorBoardGlue mb_glue = {mb_error_call, mb_scandone_call, mb_osmode_call, mb_set_sleeptime_call,nullptr,nullptr,nullptr};
            mb.set_callbacks(mb_glue);
            done_scan = 0;
            mb.SetKeyState(true);
            unsigned int reg0;
            mb.read(0x00, reg0);
            SMBCONFIG *smb_config = (SMBCONFIG *)&reg0;
            smb_config->dpi_mode = 0;
            mb.write(0x00, reg0);
            mb.set_speed_mode(2);
            mb.clear_error();
            //mb.set_cuospeed(41);
            mb.start({0});
            mb.pick_paper();
            std::this_thread::sleep_for(std::chrono::milliseconds(2000));
            StopWatch sw;
            int num = 0;
            for (;;)
            {
                if (mb.wait_paper_in(1000))
                {
                    // std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    std::cout << __func__ << " call:  paper pick num = "<< num++ << std::endl;
                    //std::this_thread::sleep_for(std::chrono::milliseconds(1000));//simulate capture
                    if(mb.wait_paper_out(1000))
                    {
                        std::cout << __func__ << " call:  paper out stop snap " <<sw.elapsed_ms() << std::endl;
                    }
                }
                else if (done_scan)
                {
                    break;
                }
                //else
                // {
                //     if (sw.elapsed_s() > 3)
                //         done_scan = -2;
                // }
                mb.pick_paper();
            }
            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;
        case 8:
        {
            int sensorid,duty;
            cout << "请输入传感器编号"<<std::endl;
            cin >> sensorid;
            cout << "请输入占空比 "<<std::endl;
            cin >> duty;
            mb.set_sensor_pwm_duty(sensorid,duty);
            break;
        }
        case 9:
        {
            int sensorid,en;
            cout << "请输入传感器编号"<<std::endl;
            cin >> sensorid;
            cout << "请使能模式 0开启 1关闭 "<<std::endl;
            cin >> en;
            mb.enable_sensor_pwm(sensorid,en);
            break;
        }
        case 10:
        {
            int sensorid,en;
            cout << "请输入配置类型: 1 双张cyc 2 无纸cyc 3 双张阈值 4 单张阈值 "<<std::endl;
            cin >> sensorid;
            cout << "请输入值 "<<std::endl;
            cin >> en;
            mb.set_ultrasonic_param(sensorid,en);
            break;
        }
        case 11:
        {
            std::cout << mb.get_ultrasonic_version() <<std::endl;
            break;
        }
        default:
            cout << "Please select again! " << endl;
            break;
        }
        /* code */
        if (exit)
            break;
    }

    cout << "exit munu" << endl;

    return 0;
}