//
// wirelessly connected cloud (based on a LPWAN, called ESP-NOW)
//

//
// puredata gathering @ ururu.cloud, Seoul
//

//
// 2024 02 17
//
//   this module will be an esp-now node in a group.
//       like, a bird in a group of birds.
//
//   esp-now @ esp8266 w/ broadcast address (FF:FF:FF:FF:FF:FF)
//       always broadcasting. everyone is 'talkative'.
//

//============<identities>============
#define MY_GROUP_ID       (6000)
#define MY_ID             (MY_GROUP_ID + 5)
#define MY_SIGN           ("VOLUME")
//============</identities>============

//============<parameters>============
#define WIFI_CHANNEL 1
//============</parameters>===========

//arduino
#include <Arduino.h>

//message types
#include "message.h"

//espnow
#include <ESP8266WiFi.h>
#include <espnow.h>

//task
#include <TaskScheduler.h>
Scheduler runner;

//-*-*-*-*-*-*-*-*-*-*-*-*-
#include <Wire.h>
#include <HMC5883L.h>
#include <MPU6050.h>
HMC5883L compass;
MPU6050 mpu;
float heading1;
float heading2;
extern Task compass_read_task;
// No tilt compensation
float noTiltCompensate(Vector mag)
{
  float heading = atan2(mag.YAxis, mag.XAxis);
  return heading;
}

// Tilt compensation
float tiltCompensate(Vector mag, Vector normAccel)
{
  // Pitch & Roll

  float roll;
  float pitch;

  roll = asin(normAccel.YAxis);
  pitch = asin(-normAccel.XAxis);

  if (roll > 0.78 || roll < -0.78 || pitch > 0.78 || pitch < -0.78)
  {
    return -1000;
  }

  // Some of these are used twice, so rather than computing them twice in the algorithem we precompute them before hand.
  float cosRoll = cos(roll);
  float sinRoll = sin(roll);
  float cosPitch = cos(pitch);
  float sinPitch = sin(pitch);

  // Tilt compensation
  float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch;
  float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch;

  float heading = atan2(Yh, Xh);

  return heading;
}

// Correct angle
float correctAngle(float heading)
{
  if (heading < 0) { heading += 2 * PI; }
  if (heading > 2 * PI) { heading -= 2 * PI; }

  return heading;
}
void compass_read() {
  //
  if (compass_read_task.isFirstIteration()) {
    //wire
    Wire.begin();

    while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
    {
      Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
      compass_read_task.disable();
    }

    mpu.setI2CMasterModeEnabled(false);
    mpu.setI2CBypassEnabled(true);
    mpu.setSleepEnabled(false);

    // Initialize Initialize HMC5883L
    Serial.println("Initialize HMC5883L");
    while (!compass.begin())
    {
      Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
      compass_read_task.disable();
    }

    // Set measurement range
    compass.setRange(HMC5883L_RANGE_1_3GA);

    // Set measurement mode
    compass.setMeasurementMode(HMC5883L_CONTINOUS);

    // Set data rate
    compass.setDataRate(HMC5883L_DATARATE_30HZ);

    // Set number of samples averaged
    compass.setSamples(HMC5883L_SAMPLES_8);

    // Set calibration offset. See HMC5883L_calibration.ino
    compass.setOffset(0, 0, 0);
  }

  // Read vectors
  Vector mag = compass.readNormalize();
  Vector acc = mpu.readScaledAccel();

  // Calculate headings
  heading1 = noTiltCompensate(mag);
  heading2 = tiltCompensate(mag, acc);

  if (heading2 == -1000)
  {
    heading2 = heading1;
  }

  // Set declination angle on your location and fix heading
  // You can find your declination on: http://magnetic-declination.com/
  // (+) Positive or (-) for negative
  // For Bytom / Poland declination angle is 4'26E (positive)
  // Formula: (deg + (min / 60.0)) / (180 / PI);
  float declinationAngle = (-9.0 + (5.0 / 60.0)) / (180 / PI);
  heading1 += declinationAngle;
  heading2 += declinationAngle;

  // Correct for heading < 0deg and heading > 360deg
  heading1 = correctAngle(heading1);
  heading2 = correctAngle(heading2);

  // Convert to degrees
  heading1 = heading1 * 180/PI;
  heading2 = heading2 * 180/PI;

  //
  float val = heading2;
  //
  Serial.println(val);
  //
  byte mac[6];
  WiFi.macAddress(mac);
  uint32_t mac32 = (((((mac[2] << 8) + mac[3]) << 8) + mac[4]) << 8) + mac[5];
  //
  Hello hello(String(MY_SIGN), MY_ID, mac32);   // the most basic 'hello'
  // and you can append some floats
  static int count = 0;
  count++;
  hello.h1 = (count % 1000);
  hello.h2 = val;
  // hello.h3 = 0;
  // hello.h4 = 0;
  //
  uint8_t frm_size = sizeof(Hello) + 2;
  uint8_t frm[frm_size];
  frm[0] = '{';
  memcpy(frm + 1, (uint8_t *) &hello, sizeof(Hello));
  frm[frm_size - 1] = '}';
  //
  esp_now_send(NULL, frm, frm_size);   // to all peers in the list.
  //
}
Task compass_read_task(25, TASK_FOREVER, &compass_read, &runner, true); // every 1 ms
//*-*-*-*-*-*-*-*-*-*-*-*-*

//task #0 : blink led
#define LED_PERIOD (11111)
#define LED_ONTIME (1)
#define LED_GAPTIME (222)
#define LED_PIN 2
extern Task blink_task;
void blink() {
  //
  static int count = 0;
  count++;
  //
  switch (count % 4) {
  case 0:
    digitalWrite(LED_PIN, LOW);     // first ON
    blink_task.delay(LED_ONTIME);
    break;
  case 1:
    digitalWrite(LED_PIN, HIGH);     // first OFF
    blink_task.delay(LED_GAPTIME);
    break;
  case 2:
    digitalWrite(LED_PIN, LOW);     // second ON
    blink_task.delay(LED_ONTIME);
    break;
  case 3:
    digitalWrite(LED_PIN, HIGH);     // second OFF
    blink_task.delay(LED_PERIOD - 2* LED_ONTIME - LED_GAPTIME);
    break;
  }
}
Task blink_task(0, TASK_FOREVER, &blink, &runner, true); // -> ENABLED, at start-up.

// on 'Note'
void onNoteHandler(Note & n) {
  //is it for me?
  if (n.id == MY_GROUP_ID || n.id == MY_ID) {
    //
    if (n.pitch < 0) n.pitch = 0;
    // volume_pin = n.pitch;     //useless:  for esp8266, A0 is only one adc.
    //
    if (n.velocity < 0) n.velocity = 0;
    //
    if (n.velocity == 0) {
      //schedule 1 read
      compass_read_task.disable();
      compass_read_task.setIterations(1);
      compass_read_task.restart();
    } else {
      // limiting max. speed.
      if (n.velocity < 20) n.velocity = 20;
      compass_read_task.setIterations(TASK_FOREVER);
      compass_read_task.setInterval(n.velocity);
      compass_read_task.restart();
    }
    //pull-up on/off
    if (n.onoff == 1) {
      // volume_pullup = true;       //useless:  for esp8266, A0 no support for internal pull-up ?
    } else {
      // volume_pullup = false;
    }
  }
}

// on 'receive'
void onDataReceive(uint8_t * mac, uint8_t *incomingData, uint8_t len) {

  //
  Serial.write(incomingData, len);

  // open => identify => use.
  if (incomingData[0] == '{' && incomingData[len - 1] == '}' && len == (sizeof(Hello) + 2)) {
    Hello hello("");
    memcpy((uint8_t *) &hello, incomingData + 1, sizeof(Hello));
    //
    Serial.println(hello.to_string());
  }

  // open => identify => use.
  if (incomingData[0] == '[' && incomingData[len - 1] == ']' && len == (sizeof(Note) + 2)) {
    Note note;
    memcpy((uint8_t *) &note, incomingData + 1, sizeof(Note));
    onNoteHandler(note);
    //
    Serial.println(note.to_string());
  }
}

// on 'sent'
void onDataSent(uint8_t *mac, uint8_t sendStatus) {
  char buff[256] = "";
  sprintf(buff, "Delivery failed! -> %02X:%02X:%02X:%02X:%02X:%02X",  mac[0],  mac[1],  mac[2],  mac[3],  mac[4],  mac[5]);
  if (sendStatus != 0) Serial.println(buff);
}

//
void setup() {

  //led
  pinMode(LED_PIN, OUTPUT);

  //serial
  Serial.begin(115200);
  delay(100);

  //info
  Serial.println();
  Serial.println();
  Serial.println("\"hi, i m your postman.\"");
  Serial.println("-");
  Serial.println("- my id: " + String(MY_ID) + ", gid: " + String(MY_GROUP_ID) + ", call me ==> \"" + String(MY_SIGN) + "\"");
  Serial.println("- mac address: " + WiFi.macAddress() + ", channel: " + String(WIFI_CHANNEL));
  Serial.println("-");

  //wifi - disabled
  system_phy_set_max_tpw(0);
  WiFiMode_t node_type = WIFI_STA;
  WiFi.mode(node_type);

  //esp-now
  if (esp_now_init() != 0) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }
  esp_now_set_self_role(ESP_NOW_ROLE_COMBO);
  esp_now_register_send_cb(onDataSent);
  esp_now_register_recv_cb(onDataReceive);
  //
  Serial.println("- ! (esp_now_add_peer) ==> add a 'broadcast peer' (FF:FF:FF:FF:FF:FF).");
  uint8_t broadcastmac[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
  esp_now_add_peer(broadcastmac, ESP_NOW_ROLE_COMBO, 1, NULL, 0);

  Serial.println("-");
  Serial.println("\".-.-.-. :)\"");
  Serial.println();
}

void loop() {
  //
  runner.execute();
  //
}