#include "comm.h"
#include "comm_task.h"
#include <math.h>
#include <space_light.h>
#include <stdint.h>

#include "BkDriverPwm.h"

#define WARM BK_PWM_0
#define COLD BK_PWM_2
#define RED BK_PWM_3
#define GREEN BK_PWM_4
#define BLUE BK_PWM_5

static const float max_freq = 1000.0f;

// FIXME: consider creating a lookup table
static inline uint16_t get_led_frequency(const uint8_t value,
                                         const float gamma) {
  return (uint16_t)pow((float)value / 255.0f, gamma) * max_freq + 0.5;
}

// Complete hacky command which will be removed shortly
// this is just to shorten the developemnt cycle
void setPWM(uint8_t *values, size_t len) {
  // at least try something ^^

  switch (values[0]) {
  case 0x01: // initialzie
  {
    if (len < 5) {
      return;
    }
    uint8_t pwm = values[1];
    uint16_t frequency = values[2] << 8 | values[3];
    uint8_t duty = values[4];
    bk_pwm_initialize(pwm, frequency, duty);
    break;
  }
  case 0x02: // start
  {
    if (len < 2) {
      return;
    }
    uint8_t pwm = values[1];
    bk_pwm_start(pwm);
    break;
  }
  case 0x03: // stop
  {
    if (len < 2) {
      return;
    }
    uint8_t pwm = values[1];
    bk_pwm_stop(pwm);
    break;
  }
  case 0x04: // update param
  {
    if (len < 5) {
      return;
    }
    uint8_t pwm = values[1];
    uint16_t frequency = values[2] << 8 | values[3];
    uint8_t duty = values[4];
    bk_pwm_update_param(pwm, frequency, duty);
  }
  default:
    return;
  }
}

void setRGBCW(struct RGBCW *rgbcw) {
  if (rgbcw->r) {
    bk_pwm_initialize(RED, get_led_frequency(rgbcw->r, 2.8), 10);
    bk_pwm_start(RED);
  } else {
    // FIXME: same as in the init ^^ should be stop
    bk_pwm_initialize(RED, 100, 10);
    bk_pwm_start(RED);
  }

  if (rgbcw->g) {
    bk_pwm_initialize(GREEN, get_led_frequency(rgbcw->g, 2.8), 10);
    bk_pwm_start(GREEN);
  } else {
    // FIXME: same as in the init ^^ should be stop
    bk_pwm_initialize(GREEN, 100, 10);
    bk_pwm_start(GREEN);
  }

  if (rgbcw->b) {
    bk_pwm_initialize(BLUE, get_led_frequency(rgbcw->b, 2.8), 10);
    bk_pwm_start(BLUE);
  } else {
    // FIXME: same as in the init ^^ should be stop
    bk_pwm_initialize(BLUE, 100, 10);
    bk_pwm_start(BLUE);
  }

  if (rgbcw->c) {
    uint16_t value = rgbcw->c;
    if (value > 100) {
      value = 100;
    }
    value = value * 10;
    bk_pwm_initialize(COLD, value, 50);
    bk_pwm_start(COLD);
  } else {
    bk_pwm_initialize(COLD, 0, 50);
    bk_pwm_stop(COLD);
  }

  if (rgbcw->w) {
    uint16_t value = rgbcw->w;
    if (value > 100) {
      value = 100;
    }
    value = value * 10;
    value = 1000 - value; // the motor is inverted 1000 is the slowest value
    bk_pwm_initialize(WARM, value, 10);
    bk_pwm_start(WARM);
  } else {
    bk_pwm_initialize(WARM, 0, 10);
    bk_pwm_stop(WARM);
  }
}

// FIXME: this is where we want to shut down everything exept ble
void power_save_mode() {
  // FIXME: this is as it is stupid and bad but it works this way..
  // TODO: figure out how to just disable pwm without setting defaults first ^^
  // TODO: update pwm also is not working like it should
  bk_pwm_stop(RED);
  bk_pwm_stop(GREEN);
  bk_pwm_stop(BLUE);
  bk_pwm_stop(COLD);
  bk_pwm_stop(WARM);

  bk_pwm_initialize(RED, 100, 50);
  bk_pwm_start(RED);
  bk_pwm_initialize(GREEN, 100, 50);
  bk_pwm_start(GREEN);
  bk_pwm_initialize(BLUE, 100, 50);
  bk_pwm_start(BLUE);
  // FIXME: laser is still a puzzle to me
  bk_pwm_initialize(COLD, 100, 50);
  bk_pwm_start(COLD);
  bk_pwm_initialize(WARM, 1000, 50);
  bk_pwm_start(WARM);

  // FIXME: doing this will start all leds to full color ^^
  // TODO: figgure out how to reverse the level in the configuration of each PWM
  // bk_pwm_stop(RED);
  // bk_pwm_stop(GREEN);
  // bk_pwm_stop(BLUE);
  // bk_pwm_stop(COLD);
  // bk_pwm_stop(WARM);
}

void init_led_thread(void *arg) {
  // FIXME: this is as it is stupid and bad but it works this way..
  // TODO: figure out how to just disable pwm without setting defaults first ^^
  // TODO: update pwm also is not working like it should
  bk_pwm_stop(RED);
  bk_pwm_stop(GREEN);
  bk_pwm_stop(BLUE);
  bk_pwm_stop(COLD);
  bk_pwm_stop(WARM);

  bk_pwm_initialize(RED, 100, 50);
  bk_pwm_start(RED);
  bk_pwm_initialize(GREEN, 100, 50);
  bk_pwm_start(GREEN);
  bk_pwm_initialize(BLUE, 100, 50);
  bk_pwm_start(BLUE);
  // FIXME: laser is still a puzzle to me
  bk_pwm_initialize(COLD, 100, 50);
  bk_pwm_start(COLD);
  bk_pwm_initialize(WARM, 1000, 50);
  bk_pwm_start(WARM);

  // FIXME: doing this will start all leds to full color ^^
  // TODO: figgure out how to reverse the level in the configuration of each PWM
  // bk_pwm_stop(RED);
  // bk_pwm_stop(GREEN);
  // bk_pwm_stop(BLUE);
  // bk_pwm_stop(COLD);
  // bk_pwm_stop(WARM);

  rtos_delete_thread(NULL);
}