#include "stdio.h"

#include <mbed.h>
#include <rtos.h>
#include <string>

#include "hp34comm.h"
#include "display.h"

#include "QEI.h"
#include "Keypad.h"

// Pins and device declarations
#if defined TARGET_NUCLEO_F446RE
#include "def_f446re.h"
#elif defined TARGET_HP34970_FP_F303RD
#include "def_hp34970_fp.h"
#endif

#include "platform/CircularBuffer.h"

// cannot make it work...
// #include <USBSerial.h>
// USBSerial usbcdc(false);


Display *dsp;
volatile bool splashscreen;
HPSerial *hp;
Thread tdsp(osPriorityNormal, OS_STACK_SIZE, nullptr, "DSP");
Ticker dsp_refresh;
Timeout rst_delay;
Timeout splashscreen_timer;
Timeout byescreen_timer;
InterruptIn rst(HP_RST);

QEI qenc(KP_ENC1, KP_ENC2, NC, 16);
volatile uint8_t knob;
bool shift;  // true when kp is shifted, cleared by command 0x01 from Unit
bool must_reset;
bool must_shutdown;

typedef enum {
    KEY_NONE=0,
    KEY_PRESSED,
    KEY_RELEASED
} key_event_t;

typedef struct keycode {
  uint8_t row;
  uint8_t col;
  key_event_t keyevent;
} keycode_t;


#define KEY_BUF_SIZE 10
CircularBuffer<keycode_t, KEY_BUF_SIZE> key_buf;

#define KP_NROWS 4
#define KP_NCOLS 5
DigitalIn kp_rows[KP_NROWS] = {
    KP_R0,
    KP_R1,
    KP_R2,
    KP_R3
};

DigitalOut kp_columns[KP_NCOLS] = {
    KP_C0, KP_C1,
    KP_C2, KP_C3,
    KP_C4
};

/* key mapping
   RxC   code   name
   0x2   0x00   View
   0x1   0x01   Mon
   3x3   0x02   Sto/Rcl
   0x0   0x03   Scan
   1x2   0x04   Alarm
   1x1   0x05   Mx+B
   1x0   0x06   Measure
   2x0   0x07   Interval
   3x2   0x08   Card Reset
   3x1   0x09   Close
   3x0   0x0A   Open
   0x3   0x0B   Read
   2x3   0x0C   Shift
   1x3   0x0D   Write
   0x4   0x0E   Left
   1x4   0x0F   Right
   2x2   0x10   Advanced
   2x1   0x11   Step
*/

uint8_t kp_mapping[KP_NROWS][KP_NCOLS] = {
    {0x03, 0x01, 0x00, 0x0B, 0x0E},
    {0x06, 0x05, 0x04, 0x0D, 0x0F},
    {0x07, 0x11, 0x10, 0x0C, 0xFF},
    {0x0A, 0x09, 0x08, 0x02, 0xFF}
};

#define KC_SHIFT 0x0C

void kp_cb(uint8_t row, uint8_t col);
void kr_cb(uint8_t row, uint8_t col);

Keypad *kpad;
keycode_t last_key = {0, 0, KEY_NONE};

uint8_t curchar;
//uint8_t curcmd;
uint8_t nchars;
char buffer[MAX_BUFF+1];

void refresh_display(void);

void timeout_h() {
#if defined(HAS_LED)
  led = !led;
#endif
}

#ifdef DEBUG

DigitalOut dbgpin(DBGPIN);
inline void pulse(uint8_t count=1, bool stayup=false)
{
  dbgpin = 0;
  wait_us(2);
  while (count--)
  {
    dbgpin = 1;
    wait_us(2);
    dbgpin = 0;
    wait_us(2);
  }
  if (stayup)
    dbgpin = 1;
}
#else
inline void pulse(uint8_t count=1, bool stayup=false)
{}
#endif

// callbacks & thread functions
void reset(void);
void reset_irq(void);
void qei_cb(int dir);
void end_splashscreen(void);


#if defined(HAVE_PC)
FileHandle *mbed::mbed_override_console(int fd)
{
	return static_cast<FileHandle*> (&pc);
}
#endif


void setup() {
  printf("\n\nSETUP\n");
  printf("  System Core Clock = %ld MHZ\r\n", SystemCoreClock/1000000);

  hp = NULL;
#if defined(TARGET_NUCLEO_F446RE)
  printf("Serial communication pins\r\n");
  printf("     USBRX=%d\r\n", USBRX);
  printf("     USBTX=%d\r\n", USBTX);
#endif
#if defined(TARGET_HP34970_FP_F303RD)
  printf("Serial communication via UART4\r\n");
#endif

  printf("Setup HP communication pins\r\n");
  //printf("     HP_RX=%d\r\n", HP_RX);
  DigitalIn(HP_RX).mode(PullDown);
  //printf("     HP_TX=%d\r\n", HP_TX);
  DigitalOut(HP_TX).write(1);
  //printf("     HP_RST=%d\r\n", HP_RST);
  DigitalIn(HP_RST).mode(PullDown);

  printf("  setup QEI pins\r\n");
  //printf("     ENC1=%d\r\n", KP_ENC1);
  DigitalIn(KP_ENC1).mode(PullDown);
  //printf("     ENC2=%d\r\n", KP_ENC2);
  DigitalIn(KP_ENC2).mode(PullDown);
  qenc.attach(&qei_cb);

  printf("  setup Keypad\r\n");

  shift = false;
  must_reset = false;
  must_shutdown = false;
  kpad = new Keypad(KP_NROWS, kp_rows, KP_NCOLS, kp_columns);
  printf("  attach Keypad callbacks\r\n");
  kpad->attach(&kp_cb, &kr_cb);
  printf("  start Keypad\r\n");
  kpad->start();

  printf("Setup OLED display\r\n");
  // init the LCD
  /*
  printf("     DSP_MOSI=%d\r\n", DSP_MOSI);
  printf("     DSP_MISO=%d\r\n", DSP_MISO);
  printf("     DSP_SCLK=%d\r\n", DSP_SCLK);
  printf("     DSP_CS=%d\r\n", DSP_CS);
  printf("     DSP_RST=%d\r\n", DSP_RST);
  printf("     DSP_DC=%d\r\n", DSP_DC);
  */
  dsp = new Display(20000000, DSP_MOSI, DSP_MISO, DSP_SCLK, DSP_CS,
            DSP_RST, DSP_DC, "SSD1322");

  curchar = 0;
  nchars = 0;

  //for (uint8_t i=0; i<sizeof(table)/sizeof(table[0]); ++i)
  //  memset(table[i].buffer, 0, MAX_BUFF+1);

  printf("  display splash screen\r\n");
  dsp->show_splashscreen();

  printf("Starting LCD thread\r\n");
  // does not compile...
  // dsp_refresh.attach(callback(dsp, &Display::copy_to_lcd), 50ms);
  tdsp.start(&refresh_display);

  printf("Attach timers\r\n");
  splashscreen = true;
  splashscreen_timer.attach(callback(&end_splashscreen), 2ms);
  rst.fall(&reset_irq);

  printf("SETUP DONE\r\n");
}

void end_splashscreen(void)
{
  // print is forbidden here because we are in an ISR context here
  //printf("End of splash screen CB\r\n");
  splashscreen = false;
}

void reset_irq_tmr(void)
{
	must_reset = true;
}

void reset_irq(void)
{
	rst_delay.attach(callback(&reset_irq_tmr), 50ms);
}

void reset(void)
{
  // this should be called as a result of the HP_RST pin going LO
  printf("Reset connection to the main unit\n");
  if (DigitalIn(HP_RST).read() == 0) {
	  byescreen_timer.detach(); // cancel a "poweroff", if any

    if (hp == NULL) {
      printf("setup HP communication handler\r\n");
      hp = new HPSerial(HP_TX, HP_RX);
    }
	else {
		printf("Connection already initialized\n");
	}

    printf("!! RST !! (gstate=%d, state=%d)\r\n",
           hp->gstate(), hp->state());
    //printf("Value is ... %X\n", hp->search());
	dsp->power_on();
	dsp->cls();
    printf("Initiate startup sequence\n");
	if (last_key.keyevent == KEY_PRESSED)
		hp->send_startup_seq(kp_mapping[last_key.row][last_key.col]);
	else
		hp->send_startup_seq();
  }
  else
  {
	  printf("HP_RST is not LOW, skipping\n");
  }
}

void refresh_display(void) {
	while(1) { //!must_shutdown) {
		dsp->copy_to_lcd();
		ThisThread::sleep_for(50ms);
	}
}

void shutdown(void)
{
  must_shutdown = true;
}

void mainloop()
{ // run over and over
  keycode_t key = {0, 0, KEY_NONE};
  uint8_t keycode;  // actual sent value to the CPU
  HPSerial::CMD cmd;
  unsigned int nkeys = 0;
  unsigned int err[8];
  for (uint8_t i=0; i<8; i++)
    err[i] = 0;

  while(1) {
	if (must_reset) {
	  must_reset = false;
	  reset();
	}
	if (must_shutdown) {
		must_shutdown = false;
		dsp->power_off();
	}

    if (knob != 0)
    {
      if (hp != NULL)
      {
        printf("KEY[%d] %s%X\n", nkeys++, knob<0x0F ? "0" : "", knob);
        hp->sendkey(knob);
      }
      else
      {
        dsp->locate(70, 0);
        dsp->printf("Knob = %X  ", knob);
        dsp->copy_to_lcd();
      }
      knob = 0;
    }

    if (!key_buf.empty())
    {
      key_buf.pop(key);
	  keycode = kp_mapping[key.row][key.col];

	  if ((keycode == KC_SHIFT) && (key.keyevent == KEY_PRESSED))
	  {
		  // toggle the shift indicator
		  if (shift)
			  shift = false;
		  else
			  shift = false;
		  dsp->set_flag_status(0x0E, shift);
		  dsp->draw_flag(0x0E);
	  }

      if (hp != NULL) {
        if ((shift) && (keycode != KC_SHIFT))
		{
          keycode |= 0x20;  // bit 5: key shifted
		  shift = false;
		  dsp->set_flag_status(0x0E, false);
		}
        if (key.keyevent == KEY_RELEASED)
          keycode |= 0x40;  // bit 6: key relased
        printf("KEY[%d] %s%X\n", nkeys++, keycode<0x0F ? "0" : "", keycode);
        hp->sendkey(keycode);
      }
      else
      {
        dsp->locate(140, 0);
        dsp->printf("KC: %dx%d[0x%s%X] %s",
              key.row, key.col,
              kp_mapping[key.row][key.col] <= 0x0F ? "0" : "",
              kp_mapping[key.row][key.col],
              key.keyevent==KEY_PRESSED ? "PRE" : "REL");
        dsp->copy_to_lcd();
      }
    }

    if ((hp != NULL) && (hp->cmd_available()))
    {
      if (hp->pop(cmd))
      {
#if defined(HAS_LED)
        led = 1;
#endif
        for (uint8_t i=0; i<7; i++)
          if (hp->nerrors(i) > err[i]) {
            printf("ERR: %X/%X/%X/%X/%X/%X/%X\n",
                   hp->nerrors(0),
                   hp->nerrors(1),
                   hp->nerrors(2),
                   hp->nerrors(3),
                   hp->nerrors(4),
                   hp->nerrors(5),
                   hp->nerrors(6)
                   );
            break;
          }
        for (uint8_t i=0; i<7; i++)
          err[i] = hp->nerrors(i);

        printf("CMD[%d] %s%X", (int)cmd.id, cmd.cmd<0x10 ? "0" : "", cmd.cmd);

        // 0x00: main display
        // 0x0C: channel display
        if ((cmd.cmd == 0x00) || (cmd.cmd == 0x0C))
          printf(": '%s'\n", cmd.value);
        else {
          printf(": ");
          for (uint8_t i=0; i<cmd.size; i++)
			  printf("%s%X ", cmd.value[i] < 0x10 ? "0" : "", cmd.value[i]);
          printf("\n");
        }

        if (cmd.cmd == 0x01) {
          // clear a flag
          if (cmd.value[0] == 0x0E) {
            // clear the Shift flag
            shift = false;
          }
		  dsp->set_flag_status(cmd.value[0], false);
		  dsp->draw_flag(cmd.value[0]);
		} else if (cmd.cmd == 0x08) {
		  // set a flag dimmed
		  dsp->set_flag_status(cmd.value[0], true);
		  dsp->set_flag_dim(cmd.value[0], true);
		  dsp->draw_flag(cmd.value[0]);
		} else if (cmd.cmd == 0x09) {
		  // set a flag bright
		  dsp->set_flag_status(cmd.value[0], true);
		  dsp->set_flag_dim(cmd.value[0], false);
		  dsp->draw_flag(cmd.value[0]);
        } else if (cmd.cmd == 0x86) {
			// shutdown
			dsp->show_byescreen();
			byescreen_timer.attach(callback(&shutdown), 2s);
         } else {
          // display related commands
          dsp->show(cmd.cmd, cmd.value, cmd.size);
        }
#if defined(HAS_LED)
        led = 0;
#endif
      }
    }
    //else
    ThisThread::sleep_for(1ms);
  }
}


void qei_cb(int dir)
{
  if(dir == 1) // turn right
    knob = 0x80;
  else         // turn left
    knob = 0x81;  // 83?
}

void kp_cb(uint8_t row, uint8_t col)
{
  last_key.row = row;
  last_key.col = col;
  last_key.keyevent = KEY_PRESSED;
  if(!key_buf.full())
    key_buf.push(last_key);
}

void kr_cb(uint8_t row, uint8_t col)
{
  last_key.row = row;
  last_key.col = col;
  last_key.keyevent = KEY_RELEASED;
  if(!key_buf.full())
    key_buf.push(last_key);
}

int main()
{
  setup();
  printf("Main loop (noop)\r\n");
  mainloop();
}
