#include "Arduino.h"
#include "states/state_impl/state_impl.h"
#include "drawer/drawer.h"
#include "EspNowSerial/EspNowSerial.h"
#include "nbus/nbus_comm.h"



NbusComm NBC(Dev.EnSerial);

StateLock   state_lock;
StateUnlock state_unlock;
StateTarget state_target;
StateStart  state_start;
StateRun    state_run;
StateStop   state_stop;
StateCancel state_cancel;

IState *states[_STATES_COUNT] = {
  &state_lock,
  &state_unlock,
  &state_target,
  &state_start,
  &state_run,
  &state_stop,
  &state_cancel
}; 

StateTx curr_state(STATE_LOCK);
StateTx next_state(STATE_LOCK);
int64_t time0;

void setup()
{

  Dev.hardware_init();

  Dev.Drawer.set_text_color(TFT_WHITE, TFT_BLACK);
  Dev.Drawer.draw_battery(Dev.M5Board.Power.getBatteryLevel());

  time0 = Dev.M5Board.millis();
  
  states[curr_state.id]->setup();
  Dev.Drawer.refresh();

  Dev.M5Board.delay(1000);
  uint8_t aa[] = {6, 1, 3};
  NBC.cmd_set_data(aa, 3); 



}

void loop()
{
  // 1. Spracovanie stavov
  next_state = states[curr_state.id]->loop(Dev.M5Board.millis() - time0);

  // pause transition if needed
  if (next_state.wait >= 0)
  {
    Dev.Drawer.refresh();
    
    if (next_state.wait > 0)
      Dev.M5Board.delay(next_state.wait);
    
    curr_state.wait = -1;
  }
  
  if (next_state.id != curr_state.id)
  {
    time0 = millis();
    curr_state = next_state;
    states[curr_state.id]->setup();
    Dev.Drawer.refresh();
  }

  if (Dev.get_battery_refresh())
  {
    Dev.set_battery_refresh(false);
    Dev.Drawer.draw_battery(Dev.M5Board.Power.getBatteryLevel());
  }

  Dev.M5Board.update();
}