#include "mylib.hpp"
#include <ArduCAM.h>
#include <SPI.h>
#include <Servo.h>
#include <Wire.h>
#include <semphr.h>

#define BUZZER_PIN D3
#define MOTER_PIN D6
#define CAMERA_CS_PIN PB1

namespace mylib__ {

// --------------------------------------------------------
// Lock for Serial
// --------------------------------------------------------

static xSemaphoreHandle handle = NULL;
struct Lock
{
  Lock() {
    while(xSemaphoreTake(handle,  portMAX_DELAY) != pdTRUE);
  }
  ~Lock()
  {
      xSemaphoreGive(handle);
  }
};

static void _print(const char* msg)
{
  Serial.print(msg);
}
static void _println(const char* msg)
{
  Serial.print("MSG ");
  Serial.print(msg);
  Serial.print("\n");
  Serial.flush();
}

void println(const char *msg)
{
  Lock lock;
  _println(msg);
}

#ifndef DISABLE_BLOCK
void highlightBlock(const char *msg)
{
  Lock lock;
  Serial.print("BLK ");
  Serial.print(msg);
  Serial.print("\n");
  Serial.flush();
}
#endif


void createTask(const char *name, TaskFunction_t func)
{
  auto task = xTaskCreate(func, name, configMINIMAL_STACK_SIZE, NULL, 2, NULL);
  configASSERT(task);
}

void delay(TickType_t millisec) {
  vTaskDelay(millisec / portTICK_PERIOD_MS);
}

// --------------------------------------------------------
// I2C
// --------------------------------------------------------

struct RegWriteInfo
{
  //! レジスタアドレス
  uint8_t address;
  //! 書き込むデータ
  uint8_t data;
};

static void writeRegister(uint8_t deviceAddress, RegWriteInfo const *write_data,
                          size_t length)
{
  Wire.beginTransmission(deviceAddress);
  RegWriteInfo const *const end = write_data + length;
  while (write_data != end) {
    Wire.write(write_data->address);
    Wire.write(write_data->data);
    ++write_data;
  }
  Wire.endTransmission();
}

void writeRegister(uint8_t deviceAddress, uint8_t addr, uint8_t data)
{
  Wire.beginTransmission(deviceAddress);
  Wire.write(addr);
  Wire.write(data);
  Wire.endTransmission();
}

void readI2C(uint8_t deviceAddress, uint8_t addr, uint8_t length, uint8_t *dst)
{
  Wire.beginTransmission(deviceAddress);
  Wire.write(addr);
  Wire.endTransmission();
  Wire.requestFrom(deviceAddress, length);
  while (Wire.available()) {
    *dst = Wire.read();
    ++dst;
  }
}

uint8_t readI2C(uint8_t deviceAddress, uint8_t addr)
{
  Wire.beginTransmission(deviceAddress);
  Wire.write(addr);
  Wire.endTransmission();
  Wire.requestFrom(deviceAddress, (uint8_t)1);
  return Wire.read();
}
// --------------------------------------------------------
// BME280
// --------------------------------------------------------

enum Oversampling : uint8_t
{
  Skipped = 0,
  O_1,
  O_2,
  O_4,
  O_8,
  O_16,
};

enum Mode : uint8_t
{
  Sleep = 0,
  Forced = 1,
  Default = 3,
};

enum Standby : uint8_t
{
  Standby0_5ms = 0,
  Standby62_5ms,
  Standby125ms,
  Standby250ms,
  Standby500ms,
  Standby1000ms,
  Standby10ms,
  Standby20ms,
};

enum IIRSize : uint8_t
{
  FILTER_OFF = 0,
  IIR2,
  IIR4,
  IIR8,
  IIR16
};

class BME280
{
public:
  static constexpr uint8_t ADDR_GND = 0x76;
  static constexpr uint8_t ADDR_VCC = 0x77;

  BME280() {}
  ~BME280() {}

  void init(bool ground, Standby standby, IIRSize iir, Oversampling temperatuer,
            Oversampling pressure, Oversampling humidity, Mode mode)
  {
    const uint8_t addr = ground ? ADDR_GND : ADDR_VCC;
    this->address = addr;
    uint8_t config = (standby & 7) << 5 | (iir & 7) << 2;
    uint8_t ctrl_meas =
      (temperatuer & 7) << 5 | (pressure & 7) << 2 | (mode & 3);
    uint8_t ctrl_hum = humidity & 7;
    const RegWriteInfo writeData[] = {
      {0xF4, ctrl_meas},
      {0xF2, ctrl_hum},
      {0xF5, config},
    };
    const uint8_t WRITE_DATA_LEN = sizeof(writeData) / sizeof(RegWriteInfo);
    writeRegister(addr, writeData, WRITE_DATA_LEN);
    uint8_t data[32];
    readI2C(addr, 0x88, 24, data);
    data[24] = readI2C(addr, 0xA1);
    readI2C(addr, 0xE1, 7, data + 25);

    dig_T1 = ((uint16_t)data[1] << 8) | data[0];
    dig_T2 = ((uint16_t)data[3] << 8) | data[2];
    dig_T3 = ((uint16_t)data[5] << 8) | data[4];
    dig_P1 = ((uint16_t)data[7] << 8) | data[6];
    dig_P2 = ((uint16_t)data[9] << 8) | data[8];
    dig_P3 = ((uint16_t)data[11] << 8) | data[10];
    dig_P4 = ((uint16_t)data[13] << 8) | data[12];
    dig_P5 = ((uint16_t)data[15] << 8) | data[14];
    dig_P6 = ((uint16_t)data[17] << 8) | data[16];
    dig_P7 = ((uint16_t)data[19] << 8) | data[18];
    dig_P8 = ((uint16_t)data[21] << 8) | data[20];
    dig_P9 = ((uint16_t)data[23] << 8) | data[22];
    dig_H1 = data[24];
    dig_H2 = ((uint16_t)data[26] << 8) | data[25];
    dig_H3 = data[27];
    dig_H4 = ((uint16_t)data[28] << 4) | (0x0F & data[29]);
    dig_H5 = ((uint16_t)data[30] << 4) | ((data[29] >> 4) & 0x0F);
    dig_H6 = data[31];
  }

  SensorData read()
  {
    SensorData out;
    const auto addr = this->address;

    uint8_t data[8];
    readI2C(addr, 0xF7, 8, data);

    int32_t t_fine;
    {
      const uint32_t adc_T = ((uint32_t)data[3] << 12) |
                             ((uint32_t)data[4] << 4) |
                             ((uint32_t)data[5] >> 4);
      const int32_t dig_T1 = this->dig_T1, dig_T2 = this->dig_T2,
                    dig_T3 = this->dig_T3;
      int32_t var1 = (((adc_T >> 3) - (dig_T1 << 1)) * dig_T2) >> 11;
      int32_t var2 =
        (((((adc_T >> 4) - dig_T1) * ((adc_T >> 4) - dig_T1)) >> 12) *
         dig_T3) >>
        14;

      t_fine = var1 + var2;
      out.temperatuer = (t_fine * 5 + 128) >> 8;
    }

    {
      const uint32_t adc_P = ((uint32_t)data[0] << 12) |
                             ((uint32_t)data[1] << 4) |
                             ((uint32_t)data[2] >> 4);
      const int32_t dig_P1 = this->dig_P1, dig_P2 = this->dig_P2,
                    dig_P3 = this->dig_P3, dig_P4 = this->dig_P4,
                    dig_P5 = this->dig_P5, dig_P6 = this->dig_P6;
      int32_t var1 = (t_fine >> 1) - 64000;
      int32_t var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6;
      var2 = var2 + (var1 * dig_P5 << 1);
      var2 = (var2 >> 2) + (dig_P4 << 16);
      var1 = (((dig_P3 * (((var1 >> 2) * (var1 >> 2)) >> 13)) >> 3) +
              (dig_P2 * var1 >> 1)) >>
             18;
      var1 = (32768 + var1) * dig_P1 >> 15;

      if (var1 == 0) {
        out.pressure = 0;
      } else {
        uint32_t p = (uint32_t)((1048576 - adc_P) - (var2 >> 12)) * 3125;
        if (p < 0x80000000) {
          p = (p << 1) / ((uint32_t)var1);
        } else {
          p = (p / (uint32_t)var1) * 2;
        }
        const int32_t dig_P7 = this->dig_P7, dig_P8 = this->dig_P8,
                      dig_P9 = this->dig_P9;
        var1 = dig_P9 * (int32_t)((p >> 3) * (p >> 3) >> 13) >> 12;
        var2 = (int32_t)(p >> 2) * dig_P8 >> 13;
        out.pressure = (uint32_t)((int32_t)p + ((var1 + var2 + dig_P7) >> 4));
      }
    }

    {
      const uint32_t adc_H = ((uint32_t)data[6] << 8) | data[7];
      int32_t v_x1 = (t_fine - ((int32_t)76800));
      const int32_t dig_H1 = this->dig_H1, dig_H2 = this->dig_H2,
                    dig_H3 = this->dig_H3, dig_H4 = this->dig_H4,
                    dig_H5 = this->dig_H5, dig_H6 = this->dig_H6;

      v_x1 = ((((adc_H << 14) - (dig_H4 << 20) - (dig_H5 * v_x1)) +
               ((int32_t)16384)) >>
              15) *
             ((((((v_x1 * dig_H6 >> 10) *
                  ((v_x1 * dig_H3 >> 11) + (int32_t)32768)) >>
                 10) +
                ((int32_t)2097152)) *
                 dig_H2 +
               8192) >>
              14);
      v_x1 = v_x1 - (((((v_x1 >> 15) * (v_x1 >> 15)) >> 7) * dig_H1) >> 4);
      v_x1 = v_x1 < 0 ? 0 : v_x1;
      v_x1 = v_x1 > 419430400 ? 419430400 : v_x1;
      out.humidity = (uint32_t)(v_x1 >> 12);
    }
    return out;
  }

private:
  uint8_t address;
  uint16_t dig_T1;
  int16_t dig_T2;
  int16_t dig_T3;
  uint16_t dig_P1;
  int16_t dig_P2;
  int16_t dig_P3;
  int16_t dig_P4;
  int16_t dig_P5;
  int16_t dig_P6;
  int16_t dig_P7;
  int16_t dig_P8;
  int16_t dig_P9;
  int8_t dig_H1;
  int16_t dig_H2;
  int8_t dig_H3;
  int16_t dig_H4;
  int16_t dig_H5;
  int8_t dig_H6;
};

static BME280 bme280;
static char buffer[256];
SensorData getSensorData() { return bme280.read(); }
SensorData printSensorData()
{
  SensorData sdata = bme280.read();
  auto t = sdata.temperatuer / 100;
  auto dt = sdata.temperatuer - t * 100;
  auto p = sdata.pressure / 100;
  auto dp = sdata.pressure - p * 100;
  auto h = sdata.humidity >> 10;
  auto dh = (sdata.humidity - h * 1024) * 100 / 1024;

  {
    Lock lock;
    sprintf(buffer, "MSG 温度: %d.%03d,", t, dt);
    _print(buffer);
    sprintf(buffer, "気圧:%d.%03d hPa, 湿度:%d.%03d %\n", t, dt, h, dh);
    _print(buffer);
    Serial.flush();
  }

  return sdata;
}

// --------------------------------------------------------
// Buzzer
// --------------------------------------------------------

void soundBuzzer(uint32_t hz)
{
  pwm_start(digitalPinToPinName(BUZZER_PIN), hz, 127, RESOLUTION_8B_COMPARE_FORMAT);
}
void stopBuzzer() { pwm_stop(digitalPinToPinName(BUZZER_PIN)); }

// --------------------------------------------------------
// Moter
// --------------------------------------------------------
static Servo servo;

void moveMoter(int dig)
{
  int v = dig > 180 ? 180 : dig < 0 ? 0 : dig;
  servo.write(v);
}

// --------------------------------------------------------
// Camera
// --------------------------------------------------------

static ArduCAM cam;
static void initArduCAM(int pin)
{
  cam = ArduCAM(OV2640, pin);
  pinMode(pin, OUTPUT);
  digitalWrite(pin, HIGH);
  SPI.begin();
  // Reset the CPLD
  cam.write_reg(0x07, 0x80);
  ::delay(100);
  cam.write_reg(0x07, 0x00);
  ::delay(100);

  while (1) {
    // Check if the ArduCAM SPI bus is OK
    cam.write_reg(ARDUCHIP_TEST1, 0x35);
    auto temp = cam.read_reg(ARDUCHIP_TEST1);
    if (temp != 0x35) {
      Serial.print("FATAL OV2640 SPI interface Error!\n");
      ::delay(1000);
      continue;
    } else
      break;
  }

  while (1) {
    // Check if the camera module type is OV2640
    uint8_t vid, pid;
    cam.wrSensorReg8_8(0xff, 0x01);
    cam.rdSensorReg8_8(OV2640_CHIPID_HIGH, &vid);
    cam.rdSensorReg8_8(OV2640_CHIPID_LOW, &pid);
    if ((vid != 0x26) && ((pid != 0x41) || (pid != 0x42))) {
      Serial.print("FATAL Can't find OV2640\n");
      ::delay(1000);
      continue;
    } else
      break;
  }
  cam.set_format(JPEG);
  cam.InitCAM();
  cam.OV2640_set_JPEG_size(OV2640_320x240);

  ::delay(1000);
  cam.clear_fifo_flag();
}

void printJPEG()
{
  cam.flush_fifo();
  cam.clear_fifo_flag();
  cam.start_capture();
  while (!cam.get_bit(ARDUCHIP_TRIG, CAP_DONE_MASK))
    ;
  delay(50);

  auto length = cam.read_fifo_length();
  if (length == 0)
    return;

  cam.CS_LOW();
  cam.set_fifo_burst();
  uint8_t before = SPI.transfer(0);
  --length;

  {
    Lock lock;
    bool output = false;
    Serial.print("IMG\n");
    Serial.flush();
    while (length--) {
      uint8_t current = SPI.transfer(0);

      // find JPEG start magic number
      if (!output && before == 0xFF && current == 0xD8) {
        output = true;
        Serial.write(before);
      }

      if (output)
        Serial.write(current);

      // JPEG end magic number
      if (output && before == 0xFF && current == 0xD9)
        break;
      before = current;
      delayMicroseconds(15);
    }
    Serial.print("GMI\n");
    Serial.flush();
  }
  cam.CS_HIGH();
  cam.clear_fifo_flag();
}

// --------------------------------------------------------
// init
// --------------------------------------------------------
void init()
{
  handle = xSemaphoreCreateMutex();
  xSemaphoreGive(handle);
  Serial.begin(921600);
  Wire.begin();


  bme280.init(true, Standby::Standby125ms, IIRSize::IIR4, Oversampling::O_1,
              Oversampling::O_1, Oversampling::O_1, Mode::Default);

  // moter
  pinMode(MOTER_PIN, OUTPUT);
  servo.attach(MOTER_PIN, 500, 2400);

  initArduCAM(CAMERA_CS_PIN);
}


} // namespace mylib__

extern "C" {

// Arduino
void setup()
{
  mylib__::init();
  user__::__createTasks__();
  vTaskStartScheduler();
}

void loop() {}
}
