/*!
  * @file  DigitalCompass.ino
  * @brief Get the geomagnetic data at 3 axis (x, y, z), get the compass degree
  * @n "Compass Degree", the angle formed when the needle rotates counterclockwise from the current position to the true north
  * @n Experimental phenomenon: serial print the geomagnetic data of x-axis, y-axis and z-axis and the compass degree
  * @license     The MIT License (MIT)
  * @author      Dorian
  */
#include "DFRobot_BMM150.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

DFRobot_BMM150_I2C bmm150(&Wire, I2C_ADDRESS_4);

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET -1 
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int last_dx, last_dy, dx, dy; // to store ponctual values
const int centreX = 86;
const int centreY = 32;
const int radius  = 30; 


void setup() 
{
  Serial.begin(115200);
  while(!Serial);
  while(bmm150.begin()){
    Serial.println("bmm150 init failed, Please try again!");
    delay(1000);
  } Serial.println("bmm150 init success!");
  // Configure Magnometer
  bmm150.setOperationMode(BMM150_POWERMODE_NORMAL);
  bmm150.setPresetMode(BMM150_PRESETMODE_HIGHACCURACY);
  bmm150.setRate(BMM150_DATA_RATE_10HZ);
  bmm150.setMeasurementXYZ();


  
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  display.clearDisplay();
  last_dx = centreX;
  last_dy = centreY;
}

void loop()
{
  sBmm150MagData_t magData = bmm150.getGeomagneticData();
  Serial.print("mag x = "); Serial.print(magData.x); Serial.println(" uT");
  Serial.print("mag y = "); Serial.print(magData.y); Serial.println(" uT");
  Serial.print("mag z = "); Serial.print(magData.z); Serial.println(" uT");
  float compassDegree = bmm150.getCompassDegree();
  Serial.print("the angle between the pointing direction and north (counterclockwise) is:");
  Serial.println(compassDegree);
  Serial.println("--------------------------------");
  display.clearDisplay();
  Draw_Compass(); 
  dx = (0.7*radius * cos((compassDegree-90)*3.14/180)) + centreX;  // calculate X position for the screen coordinates - can be confusing!
  dy = (0.7*radius * sin((compassDegree-90)*3.14/180)) + centreY;  // calculate Y position for the screen coordinates - can be confusing!
  draw_arrow(last_dx,last_dy, centreX, centreY, 2,2,BLACK);      // Erase last arrow      
  draw_arrow(dx,dy, centreX, centreY, 2, 2,WHITE);               // Draw arrow in new position
  last_dx = dx; 
  last_dy = dy;
  display.setCursor(0,48);
  display.setTextSize(2);
  display.print(compassDegree);
  display.print(char(247)); // and the degree symbol
  display.display();
  delay(100);
}

void draw_arrow(int x2, int y2, int x1, int y1, int alength, int awidth, int colour) {
  float distance;
  int dx, dy, x2o,y2o,x3,y3,x4,y4,k;
  distance = sqrt(pow((x1 - x2),2) + pow((y1 - y2), 2));
  dx = x2 + (x1 - x2) * alength / distance;
  dy = y2 + (y1 - y2) * alength / distance;
  k = awidth / alength;
  x2o = x2 - dx;
  y2o = dy - y2;
  x3 = y2o * k + dx;
  y3 = x2o * k + dy;
  x4 = dx - y2o * k;
  y4 = dy - x2o * k;
  display.drawLine(x1, y1, x2, y2, colour);
  display.drawLine(x1, y1, dx, dy, colour);
  display.drawLine(x3, y3, x4, y4, colour);
  display.drawLine(x3, y3, x2, y2, colour);
  display.drawLine(x2, y2, x4, y4, colour);
} 

void display_direction(int x, int y, String dir) {
  display.setCursor(x, y);
  display.setTextColor(WHITE);
  display.setTextSize(1);
  display.print(dir);
}

void Draw_Compass() {
  int dxo, dyo, dxi, dyi;
  display.drawCircle(centreX,centreY,radius,WHITE);  // Draw compass circle
  for (float i = 0; i <360; i = i + 22.5) {
    dxo = radius * cos(i*3.14/180);
    dyo = radius * sin(i*3.14/180);
    dxi = dxo * 0.95;
    dyi = dyo * 0.95;
    display.drawLine(dxi+centreX,dyi+centreY,dxo+centreX,dyo+centreY,WHITE);   
  }
  display_direction((centreX-2),(centreY-24),"N");
  display_direction((centreX-2),(centreY+17),"S");
  display_direction((centreX+19),(centreY-3),"E");
  display_direction((centreX-23),(centreY-3),"W");
}