#include <driver/i2c_master.h>
#include <esp_err.h>
#include <esp_log.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <portmacro.h>
#include <ssd1306.h>
#include <stdio.h>
#include <ultrasonic.h>

// defines for sonar sensor
#define TRIGGER 19
#define ECHO 18

// defines for i2c display
#define I2C_HOST 0
#define SDA 22
#define SCL 21
#define RESET_PIN -1
#define I2C_HW_ADDR 0x3C
#define LCD_H 128
#define LCD_W 64

void displayData(ssd1306_config_t *cfg, ssd1306_handle_t *handle, float data) {

  ESP_ERROR_CHECK(ssd1306_clear(*handle));
  ssd1306_draw_rect(*handle, 0, 0, 128, 4, true);

  char buf[64];
  sprintf(buf, "Distance: %.2f", data);

  ESP_ERROR_CHECK(ssd1306_draw_text(*handle, 0, 20, buf, true));
  ESP_ERROR_CHECK(ssd1306_draw_rect(*handle, 0, 40, 128, 4, true));
  ESP_ERROR_CHECK(ssd1306_display(*handle));
}

void app_main(void) {

  i2c_master_bus_config_t bus_cfg = {
      .i2c_port = I2C_HOST,
      .sda_io_num = SDA,
      .scl_io_num = SCL,
      .clk_source = I2C_CLK_SRC_DEFAULT,
      .glitch_ignore_cnt = 0,
      .flags.enable_internal_pullup = true,
  };
  i2c_master_bus_handle_t bus = NULL;
  ESP_ERROR_CHECK(i2c_new_master_bus(&bus_cfg, &bus));

  ssd1306_config_t cfg = {
      .width = LCD_H,
      .height = LCD_W,
      .fb = NULL, // let driver allocate internally
      .fb_len = 0,
      .iface.i2c =
          {
              .port = I2C_HOST,
              .addr = I2C_HW_ADDR,   // typical SSD1306 I2C address
              .rst_gpio = RESET_PIN, // no reset pin
          },
  };

  ssd1306_handle_t handle = NULL;
  ESP_ERROR_CHECK(ssd1306_new_i2c(&cfg, &handle));

  const ultrasonic_sensor_t sensor = {.trigger_pin = TRIGGER, .echo_pin = ECHO};
  const TickType_t duration = 500 / portTICK_PERIOD_MS;

  ultrasonic_init(&sensor);
  while (true) {
    float distance = 0;
    esp_err_t res = ultrasonic_measure(&sensor, 500, &distance);
    if (res != ESP_OK) {
      printf("Error %d: ", res);
      switch (res) {
      case ESP_ERR_ULTRASONIC_PING:
        printf("Cannot ping (device is in invalid state)\n");
        break;
      case ESP_ERR_ULTRASONIC_PING_TIMEOUT:
        printf("Ping timeout (no device found)\n");
        break;
      case ESP_ERR_ULTRASONIC_ECHO_TIMEOUT:
        printf("Echo timeout (i.e. distance too big)\n");
        break;
      default:
        printf("%s\n", esp_err_to_name(res));
      }
    } else {
      displayData(&cfg, &handle, (distance * 100));
      printf("Distance: %.2f cm\n", distance * 100);
    }
    vTaskDelay(duration);
  }
}