#include "mgos.h"
#include "WEMOS_Motor.h"

#include "mgos_rpc.h"
#include "common/cs_dbg.h"
#include "common/json_utils.h"
#include "common/platform.h"
#include "common/mg_str.h"
#include "frozen/frozen.h"
#include "mgos_app.h"
#include "mgos_gpio.h"
#include "mgos_net.h"
#include "mgos_sys_config.h"
#include "mgos_timers.h"
#include "mgos_mqtt.h"
#include "common/str_util.h"
#include "mgos_crontab.h"

static double pwm = 30.0;
static double flashLightSpeed = 50.0; /* 0.0 .. 100.0 */
static double flashLightTargetSpeed = 50.0; // used to rampdown to a specific speed
static uint16_t motorRampUpTime_msec = 2000;
static uint16_t motorRampDownTime_msec = 1000;
static uint16_t motorUpdateTime_msec = 100;
static double flashLightRampUp_deltaSpeed = 0;
static double flashLightRampDown_deltaSpeed = 0;
static uint8_t motorDirection = _CW;
static const char * clientId = "";
static char commandTopic[256] = {'\0'};

static char pubStatusTopic[256] = {'\0'};
static bool mqttConnected = false;
enum MotorStatus { MotorStatus_Off = 0, MotorStatus_RampUp, MotorStatus_RampDown, MotorStatus_On, MotorStatus_DemoMode };
static uint8_t motorStatus = MotorStatus_DemoMode;

// Motor numbers
//Motor shiled I2C Address: 0x30
//PWM frequency: 1000Hz(1kHz)
#define M1 0
#define M1_addr 0x30
#define M1_freq 3000
static uint8_t motorAddress = M1_addr;
static uint32_t motorFrequency = M1_freq;

static void recalcTimings() {
  flashLightRampUp_deltaSpeed = (flashLightSpeed * motorUpdateTime_msec) / motorRampUpTime_msec;
  flashLightRampDown_deltaSpeed = (flashLightSpeed * motorUpdateTime_msec) / motorRampDownTime_msec;
}

static void pubStatus(const char *statusString, double percentage) {
  struct mbuf fb;
  struct json_out out = JSON_OUT_MBUF(&fb);

  if (!mqttConnected) return;

  mbuf_init(&fb, 30);
  if (mgos_mqtt_global_connect()) {
    json_printf(&out, "{statusString: %Q, speed: %f}", statusString, percentage);
    mgos_mqtt_pub(pubStatusTopic, fb.buf, fb.len, 0, true);
  }

}

static void motor_timer_cb(void *arg) {
  static bool stopped = false;

  switch (motorStatus) {
    case MotorStatus_Off:
        wemos_motor_stop(M1);
        break;
    case MotorStatus_On:
        wemos_motor_setmotor(M1, motorDirection, pwm);
        break;
    case MotorStatus_RampUp:
        pwm += flashLightRampUp_deltaSpeed;
        if (pwm >= flashLightSpeed) {
          LOG(LL_INFO, ("MotorStatus_RampUp: Speed target reached"));
          pwm = flashLightSpeed;
          motorStatus = MotorStatus_On;
        }
        wemos_motor_setmotor(M1, motorDirection, pwm);
        // LOG(LL_INFO, ("M1, dir=%d, pwm=%f", motorDirection, pwm));
        break;
    case MotorStatus_RampDown:
        pwm -= flashLightRampDown_deltaSpeed;
        if (pwm <= flashLightTargetSpeed) {
          if (pwm <= 0.0) {
            motorStatus = MotorStatus_Off;
            pwm = 0.0;
            wemos_motor_stop(M1);
            // change direction for next time, when motor turns on again
            if (motorDirection == _CW) {
              motorDirection = _CCW;
            } else {
              motorDirection = _CW;
            }
          } else {
            pwm = flashLightTargetSpeed;
            LOG(LL_INFO, ("MotorStatus_RampDown: Speed target reached"));
            motorStatus = MotorStatus_On;
            wemos_motor_setmotor(M1, motorDirection, pwm);
          }
        } else {
          wemos_motor_setmotor(M1, motorDirection, pwm);
          // LOG(LL_INFO, ("M1, dir=%d, pwm=%f", motorDirection, pwm));
        }
        break;
    case MotorStatus_DemoMode:
    default:
        if (pwm > flashLightSpeed + 5.0) {
          pwm = 0.0; // start again
          stopped = false;
        } else {
          pwm += 0.1;
          if (!stopped) {
            wemos_motor_setmotor(M1, motorDirection, pwm);
            LOG(LL_INFO, ("M1, dir=%d, pwm=%f", motorDirection, pwm));
            pubStatus("on", pwm);

            if (pwm > flashLightSpeed) {
              // wemos_motor_setmotor(M1, _STOP, 0.0);
              wemos_motor_setmotor(M1, _STANDBY, pwm);
              stopped = true;
              LOG(LL_INFO, ("Stopped/Standby"));
              pubStatus("off", 0);
            }
          }
        }
        break;
  }
  (void) arg;
}

static void flashLightOn() {
  LOG(LL_INFO, ("FlashLight ON\n"));
  flashLightTargetSpeed = flashLightSpeed;
  motorStatus = MotorStatus_RampUp; // this starts the motor on next timer callback
  pubStatus("on", flashLightSpeed);
}

static void rpc_flashLightOn(struct mg_rpc_request_info *ri, void *cb_arg,
                        struct mg_rpc_frame_info *fi, struct mg_str args) {
  struct mbuf fb;
  struct json_out out = JSON_OUT_MBUF(&fb);

  flashLightOn();
  mbuf_init(&fb, 100);
  json_printf(&out, "{result: 0, resultString: %Q}", "OK");
  mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
  ri = NULL;
  mbuf_free(&fb);
  
  (void) cb_arg;
  (void) fi;
  (void) args;
}

static void cron_flashLightOn(struct mg_str action, struct mg_str payload, void *userdata) {
  LOG(LL_INFO, ("Crontab flashLightOn fired"));
  flashLightOn();
  (void) action;
  (void) payload;
  (void) userdata;
}

static void flashLightOff() {
  LOG(LL_INFO, ("FlashLight OFF\n"));
  flashLightTargetSpeed = 0.0;
  motorStatus = MotorStatus_RampDown; // this stops the motor on next timer callback 
  pubStatus("off", 0);
}

static void rpc_flashLightOff(struct mg_rpc_request_info *ri, void *cb_arg,
                        struct mg_rpc_frame_info *fi, struct mg_str args) {
  struct mbuf fb;
  struct json_out out = JSON_OUT_MBUF(&fb);

  flashLightOff();
  mbuf_init(&fb, 100);
  json_printf(&out, "{result: 0, resultString: %Q}", "OK");
  mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
  ri = NULL;
  mbuf_free(&fb);
  
  (void) cb_arg;
  (void) fi;
  (void) args;
}

static void cron_flashLightOff(struct mg_str action, struct mg_str payload, void *userdata) {
  LOG(LL_INFO, ("Crontab flashLightOff fired"));
  flashLightOff();
  (void) action;
  (void) payload;
  (void) userdata;
}

static void cron_init() {
  mgos_crontab_register_handler(mg_mk_str("FlashLightOn"), cron_flashLightOn, NULL);
  mgos_crontab_register_handler(mg_mk_str("FlashLightOff"), cron_flashLightOff, NULL);
}

static void flashLightSetSpeed(uint16_t newSpeed) {
  flashLightTargetSpeed = (double) newSpeed;
  flashLightSpeed = flashLightTargetSpeed;
  if (pwm < flashLightTargetSpeed) {
    motorStatus = MotorStatus_RampUp;
  } else if (pwm > flashLightTargetSpeed) {
    motorStatus = MotorStatus_RampDown;
  }
}

static void rpc_flashLightSetSpeed(struct mg_rpc_request_info *ri, void *cb_arg,
                        struct mg_rpc_frame_info *fi, struct mg_str args) {
  struct mbuf fb;
  struct json_out out = JSON_OUT_MBUF(&fb);

  mbuf_init(&fb, 100);

  int speed = 0;
  
  if (json_scanf(args.p, args.len, ri->args_fmt, &speed) == 1) {
    printf("FlashLight set speed to %d\n", speed);
    json_printf(&out, "{result: 0, resultString: %Q, speed: %d}", "OK", speed);
    flashLightSetSpeed(speed);
  } else {
    json_printf(&out, "{error: %Q}", "speed is required");
  }

  mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
  ri = NULL;
  mbuf_free(&fb);
  recalcTimings();
  
  (void) cb_arg;
  (void) fi;
  (void) args;
}

static void flashLightSetRampupTime(struct mg_rpc_request_info *ri, void *cb_arg,
                        struct mg_rpc_frame_info *fi, struct mg_str args) {
  struct mbuf fb;
  struct json_out out = JSON_OUT_MBUF(&fb);

  mbuf_init(&fb, 100);

  uint16_t rampUpTime_msec = 0;
  
  if (json_scanf(args.p, args.len, ri->args_fmt, &rampUpTime_msec) == 1) {
    motorRampUpTime_msec = rampUpTime_msec;
    printf("FlashLight set motor ramp up time to %d\n", rampUpTime_msec);
    json_printf(&out, "{result: 0, resultString: %Q, rampUpTime_ms: %d}", "OK", rampUpTime_msec);
  } else {
    json_printf(&out, "{error: %Q}", "rampUpTime_msec is required");
  }

  mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
  ri = NULL;
  mbuf_free(&fb);
  recalcTimings();
  
  (void) cb_arg;
  (void) fi;
  (void) args;
}

static void flashLightSetRampdownTime(struct mg_rpc_request_info *ri, void *cb_arg,
                        struct mg_rpc_frame_info *fi, struct mg_str args) {
  struct mbuf fb;
  struct json_out out = JSON_OUT_MBUF(&fb);

  mbuf_init(&fb, 100);

  uint16_t rampDownTime_msec = 0;
  
  if (json_scanf(args.p, args.len, ri->args_fmt, &rampDownTime_msec) == 1) {
    motorRampDownTime_msec = rampDownTime_msec;
    printf("FlashLight set motor ramp up time to %d\n", rampDownTime_msec);
    json_printf(&out, "{result: 0, resultString: %Q, rampDownTime_ms: %d}", "OK", rampDownTime_msec);
  } else {
    json_printf(&out, "{error: %Q}", "rampDownTime_msec is required");
  }

  mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
  ri = NULL;
  mbuf_free(&fb);
  recalcTimings();
  
  (void) cb_arg;
  (void) fi;
  (void) args;
}

static void flashLightSetMotorUpdateTime(struct mg_rpc_request_info *ri, void *cb_arg,
                        struct mg_rpc_frame_info *fi, struct mg_str args) {
  struct mbuf fb;
  struct json_out out = JSON_OUT_MBUF(&fb);

  mbuf_init(&fb, 100);

  uint16_t updateTime = 0;
  
  if (json_scanf(args.p, args.len, ri->args_fmt, &updateTime) == 1) {
    motorUpdateTime_msec = updateTime;
    printf("FlashLight set updateTime_msec tp %d\n", updateTime);
    json_printf(&out, "{result: 0, resultString: %Q, udateTime_ms: %d}", "OK", updateTime);
  } else {
    json_printf(&out, "{error: %Q}", "motorUpdateTime_msec is required");
  }

  mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
  ri = NULL;
  mbuf_free(&fb);
  recalcTimings();
  
  (void) cb_arg;
  (void) fi;
  (void) args;
}

static void flashLightGetSettings(struct mg_rpc_request_info *ri, void *cb_arg,
                        struct mg_rpc_frame_info *fi, struct mg_str args) {
  struct mbuf fb;
  struct json_out out = JSON_OUT_MBUF(&fb);

  mbuf_init(&fb, 1024);

  json_printf(&out, "{pwm: %f, speed: %f, rampupTime_ms: %d, rampdownTime_ms: %d, updateTime_ms: %d, rampupDeltaSpeed: %f, rampdownDeltaSpeed: %f, motorDirection: %d, clientId: %Q, commandTopic: %Q, statusTopic: %Q, mqttConnected: %Q, motorStatus: %d}",
    pwm, flashLightSpeed, motorRampUpTime_msec, motorRampDownTime_msec, motorUpdateTime_msec, flashLightRampUp_deltaSpeed, flashLightRampDown_deltaSpeed,
    motorDirection, clientId, commandTopic, pubStatusTopic, mqttConnected ? "true" : "false", motorStatus);

  mg_rpc_send_responsef(ri, "%.*s", fb.len, fb.buf);
  ri = NULL;
  mbuf_free(&fb);
  
  (void) cb_arg;
  (void) fi;
  (void) args;
}

void net_changed(int ev, void *evd, void *arg) {
  if (ev != MGOS_NET_EV_IP_ACQUIRED) return;
  // call_peer();
  (void) evd;
  (void) arg;
}

static void mqttCommandHandler(struct mg_connection *c, const char *topic, int topic_len,
                    const char *msg, int msg_len, void *userdata) {
  LOG(LL_INFO, ("Got message on topic %.*s", topic_len, topic));

  (void) c;
  (void) topic;
  (void) topic_len;
  (void) msg;
  (void) msg_len;
  (void) userdata;
}

// void onMqttConnection(struct mg_connection *c, const char *client_id, struct mg_send_mqtt_handshake_opts *opts, void *fn_arg) {
void onMqttConnection(struct mg_connection *c, const char *client_id, struct mg_send_mqtt_handshake_opts *opts, void *fn_arg) {
  // add MQTT cmd subscription
  LOG(LL_INFO, ("onMqttConnection handler called with clientId=%s", client_id));
  #if 0
  #endif
  mgos_mqtt_sub(commandTopic, mqttCommandHandler, NULL);
  mqttConnected = true;

  (void) c;
  (void) client_id;
  (void) opts;
  (void) fn_arg;
}

enum mgos_app_init_result mgos_app_init(void) {
  struct mg_rpc *c = mgos_rpc_get_global();
  mg_rpc_add_handler(c, "FlashLight.On", NULL, rpc_flashLightOn, NULL);
  mg_rpc_add_handler(c, "FlashLight.Off", NULL, rpc_flashLightOff, NULL);
  mg_rpc_add_handler(c, "FlashLight.Speed", "{speed: %d}", rpc_flashLightSetSpeed, NULL);
  mg_rpc_add_handler(c, "FlashLight.RampUpTime_msec", "{rampUpTime_ms: %d}", flashLightSetRampupTime, NULL);
  mg_rpc_add_handler(c, "FlashLight.RampDownTime_msec", "{rampDownTime_ms: %d}", flashLightSetRampdownTime, NULL);
  mg_rpc_add_handler(c, "FlashLight.MotorUpdateTime_msec", "{uptdateTime_ms: %d}", flashLightSetMotorUpdateTime, NULL);
  mg_rpc_add_handler(c, "FlashLight.GetSettings", NULL, flashLightGetSettings, NULL);
  mgos_event_add_group_handler(MGOS_EVENT_GRP_NET, net_changed, NULL);

  // enable crontab
  cron_init();

  // add MQTT cmd subscription
  LOG(LL_INFO, ("Initializing MQTT"));
  clientId = mgos_sys_config_get_mqtt_client_id();
  clientId = clientId ? clientId : mgos_sys_config_get_device_id();
  LOG(LL_INFO, ("clientId=%s", clientId));
  LOG(LL_INFO, ("cmdTopic=%s", mgos_sys_config_get_flashLight_mqttCtrlTopic()));
  LOG(LL_INFO, ("pubStatusTopic=%s", mgos_sys_config_get_flashLight_mqttStatusTopic()));
  c_snprintf(commandTopic, sizeof(commandTopic), mgos_sys_config_get_flashLight_mqttCtrlTopic(), clientId);
  c_snprintf(pubStatusTopic, sizeof(pubStatusTopic), mgos_sys_config_get_flashLight_mqttStatusTopic(), clientId);
  LOG(LL_INFO, ("cmdTopic=%s", commandTopic));
  LOG(LL_INFO, ("pubStatusTopic=%s", pubStatusTopic));
  mgos_mqtt_set_connect_fn(onMqttConnection, NULL);

  motorFrequency = mgos_sys_config_get_flashLight_motorFrequency();
  LOG(LL_INFO, ("motorFrequency=%d Hz", motorFrequency));
  motorAddress = mgos_sys_config_get_flashLight_address();
  LOG(LL_INFO, ("motorAddress=%d", motorAddress));
  LOG(LL_INFO, ("Initializing motor controller"));
  recalcTimings();
  wemos_motor_init();
  LOG(LL_INFO, ("Initializing motor M1"));
  wemos_motor_initMotor(M1, motorAddress, motorFrequency);
  LOG(LL_INFO, ("Setting up timer"));
  motorUpdateTime_msec = mgos_sys_config_get_flashLight_motorUpdateTime();
  mgos_set_timer(motorUpdateTime_msec, MGOS_TIMER_REPEAT, motor_timer_cb, NULL);
  LOG(LL_INFO, ("Initialization done"));
  return MGOS_APP_INIT_SUCCESS;
}