#include "displayapp/screens/Motion.h"
#include <lvgl/lvgl.h>
#include "displayapp/DisplayApp.h"

// #if (portTICK_PERIOD_MS == 0)
#define APX_TICK_PERIOD_MS 1
// #else
// #define APX_TICK_PERIOD_MS portTICK_PERIOD_MS
// #endif

#define FRAME_HZ     100
#define FRAME_MS     (1000 / FRAME_HZ)
#define FRAME_TICKS  (FRAME_MS / APX_TICK_PERIOD_MS)
#define REDRAW_FRAME_HZ     40
#define REDRAW_FRAME_MS     (1000 / REDRAW_FRAME_HZ)
#define REDRAW_FRAME_TICKS  (REDRAW_FRAME_MS / APX_TICK_PERIOD_MS)
#define G_SCALE 0.001

using namespace Pinetime::Applications::Screens;

Motion::Motion(Pinetime::Applications::DisplayApp* app, System::SystemTask& systemTask, Controllers::MotionController& motionController, Controllers::MotorController& motorController)
  : Screen(app), motionController {motionController}, motorController {motorController}, systemTask {systemTask} {

  systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping);
  chart = lv_chart_create(lv_scr_act(), NULL);
  lv_obj_set_size(chart, 100, 100);
  lv_obj_align(chart, NULL, LV_ALIGN_IN_BOTTOM_MID, 0, 0);
  lv_chart_set_type(chart, LV_CHART_TYPE_LINE); /*Show lines and points too*/
  // lv_chart_set_series_opa(chart, LV_OPA_70);                            /*Opacity of the data series*/
  // lv_chart_set_series_width(chart, 4);                                  /*Line width and point radious*/

  lv_chart_set_range(chart, 0, 1000);
  lv_chart_set_update_mode(chart, LV_CHART_UPDATE_MODE_SHIFT);
  lv_chart_set_point_count(chart, 6);

  /*Add 3 data series*/
  ser1 = lv_chart_add_series(chart, LV_COLOR_RED);
  // ser2 = lv_chart_add_series(chart, LV_COLOR_GREEN);
  // ser3 = lv_chart_add_series(chart, LV_COLOR_YELLOW);

  lv_chart_init_points(chart, ser1, 0);
  // lv_chart_init_points(chart, ser2, 0);
  // lv_chart_init_points(chart, ser3, 0);
  lv_chart_refresh(chart); /*Required after direct set*/

  label = lv_label_create(lv_scr_act(), NULL);
  lv_label_set_text_static(label, labelText);
  // lv_label_set_text_fmt(label, "X #FF0000 %d# Y #008000 %d# Z #FFFF00 %d#", 0, 0, 0);
  // lv_label_set_align(label, LV_LABEL_ALIGN_CENTER);
  lv_obj_align(label, NULL, LV_ALIGN_IN_TOP_MID, 0, 0);
  // lv_label_set_recolor(label, true);

  lastLabel = lv_label_create(lv_scr_act(), NULL);
  lv_obj_align(lastLabel, label, LV_ALIGN_OUT_BOTTOM_MID, 0, 0);

  recordLabel = lv_label_create(lv_scr_act(), NULL);
  lv_obj_align(recordLabel, lastLabel, LV_ALIGN_OUT_BOTTOM_MID, 0, 0);

  taskRefresh = lv_task_create(RefreshTaskCallback, FRAME_MS, LV_TASK_PRIO_MID, this);
}

Motion::~Motion() {
  lv_task_del(taskRefresh);
  lv_obj_clean(lv_scr_act());
  systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping);
}

void Motion::Refresh() {
  bool new_record = false;
  bool jump_started_or_ended = false;
  double X; double Y; double_t Z;
  double G = motionController.GXYZ(X,Y,Z);
  // lv_chart_set_next(chart, ser2, motionController.Y());
  // lv_chart_set_next(chart, ser3, motionController.Z());
  TickType_t current_time = xTaskGetTickCount();
  if (started) {
    TickType_t current_frame_ms = current_time - last_frame_time;
    double G_scaled = G * G_SCALE;
    if (G_scaled < 1) {
      if (!jumping) {
        jump_started_or_ended = true;
        current_jump_speed = 0;
        current_jump_length = 0;
        jumping = true;
      }
      double current_jump_accel = (1.0 - G_scaled) * 9.8;
      double current_frame_s = current_frame_ms * 0.001;
      current_jump_speed += current_jump_accel * current_frame_s;
      current_jump_length += current_jump_speed * current_frame_s;
    } else {
      if (jumping) {
        double best_jump_length = records[0].jump_length;  
        if (current_jump_length >= best_jump_length) {
          records[4] = records[3];
          records[3] = records[2];
          records[2] = records[1];
          records[1] = records[0];
          records[0].jump_length = current_jump_length;
          new_record = 1;
        }
        jumping = false;
        jump_started_or_ended = true;
      }
    }
  }
  last_frame_time = current_time;
  if (started) {
    if (((TickType_t)(current_time - last_redraw_frame_time)) < REDRAW_FRAME_TICKS) { return; }
  } else {
    started = true;
  }
  last_redraw_frame_time = current_time;
  {
    uint16_t G_uint16 = G < 0xffff ? G : 0xffff;
    lv_chart_set_next(chart, ser1, 1000 - ((int16_t)G_uint16));
    labelText[4] = '0'+(G_uint16%10); G_uint16 /= 10;
    labelText[3] = '0'+(G_uint16%10); G_uint16 /= 10;
    labelText[2] = '0'+(G_uint16%10); G_uint16 /= 10;
    labelText[1] = '0'+(G_uint16%10); G_uint16 /= 10;
    labelText[0] = '0'+G_uint16;
  }
  lv_label_set_text_static(label, labelText);
  if (jump_started_or_ended) {
    if (new_record) {
      uint32_t a = (current_jump_length*100000.0);
      lv_label_set_text_fmt(recordLabel, "%d.%03d", a/1000, a%1000);
      motorController.RunForDuration(35);
    }
    lv_color_t color; 
    if (jumping) {
      color = LV_COLOR_CYAN;
    } else {
      double best_jump_length = records[0].jump_length;  
      if (current_jump_length * 100 > best_jump_length) {
        uint32_t a = (current_jump_length*100000.0);
        lv_label_set_text_fmt(lastLabel, "%d.%03d", a/1000, a%1000);
        if (!new_record && current_jump_length > 0.05) {
          motorController.RunForDuration(11);
        }
      }
      color = LV_COLOR_ORANGE;
    }
    lv_obj_set_style_local_text_color(lastLabel, LV_LABEL_PART_MAIN, LV_STATE_DEFAULT, color);
  }
#if 0
  lv_label_set_text_fmt(label,
                        "X #FF0000 %d# Y #008000 %d# Z #FFFF00 %d#",
                        motionController.X() / 0x10,
                        motionController.Y() / 0x10,
                        motionController.Z() / 0x10);
  lv_obj_align(label, NULL, LV_ALIGN_IN_TOP_MID, 0, 10);
#endif
}