/* I2C GPS Mapping Application - see http://www.technoblogy.com/show?2C86
 
   David Johnson-Davies - www.technoblogy.com - 15th November 2018
   Adafruit ItsyBitsy M4 (SAMD51)
   
   CC BY 4.0
   Licensed under a Creative Commons Attribution 4.0 International license: 
   http://creativecommons.org/licenses/by/4.0/
*/

#include <Wire.h>

// Pins
int const sda = 0;
int const scl = 2;

// Constants
int const address = 60;
int const commands = 0x00;
int const onecommand = 0x80;
int const data = 0x40;
int const onedata = 0xC0;

// OLED display **********************************************

// Write a single command
void Single (uint8_t x) {
  Wire.write(onecommand);
  Wire.write(x);
}

void InitDisplay () {
  Wire.beginTransmission(address);
  Wire.write(commands);
  Wire.write(0xA1);  // Flip horizontal
  Wire.write(0xAF);  // Display on
  Wire.endTransmission();
}

void ClearDisplay () {
  for (int p = 0 ; p < 8; p++) {
    Wire.beginTransmission(address);
    Single(0xB0 + p);
    Wire.endTransmission();
    for (int q = 0 ; q < 8; q++) {
      Wire.beginTransmission(address);
      Wire.write(data);
      for (int i = 0 ; i < 20; i++) Wire.write(0);
      Wire.endTransmission();
    }
  }
}

// Plot point x,y into buffer if in current slice
void PlotPoint (int x, int y) {
  Wire.beginTransmission(address);
  Single(0x00 + ((x + 2) & 0x0F));        // Column low nibble
  Single(0x10 + ((x + 2)>>4));            // Column high nibble
  Single(0xB0 + (y >> 3));                // Page
  Single(0xE0);                           // Read modify write
  Wire.write(onedata);
  Wire.endTransmission();
  Wire.requestFrom(address, 2);
  Wire.read();                            // Dummy read
  int j = Wire.read();
  Wire.beginTransmission(address);
  Wire.write(onedata);
  Wire.write((1<<(y & 0x07)) | j);
  Single(0xEE);                           // Cancel read modify write
  Wire.endTransmission();
}

// Draw crosswires
void Cross () {
  for (int x=0; x<64; x++) PlotPoint(2*x, 32);
  for (int y=0; y<32; y++) PlotPoint(64, 2*y);
}

// GPS Routines **********************************************

const float R = 6.371e6;

// Print a latitude/longitude
float Radians (float deg) {
  return deg * asin((float)1) / 90;
}

// Read latitude and longitude from I2C GPS board, and convert to x,y in metres
void ReadPosition (float *x, float *y) {
  Wire.beginTransmission(0x3A);
  Wire.write(4);                                 // Start with latitude
  Wire.endTransmission();
  Wire.requestFrom(0x3A, 8);                     // Ask for 8 bytes
  long Lat = 0, Long = 0;
  for (int i=0; i<4; i++) Lat = Lat | (long)Wire.read()<<(i*8);
  for (int i=0; i<4; i++) Long = Long | (long)Wire.read()<<(i*8);
  *x = R * Radians(Long/6e5) * cos(Radians(Lat/6e5));
  *y = R * Radians(Lat/6e5);
}

// Demo **********************************************

float xHome, yHome;

void setup() {
  Wire.begin();
  InitDisplay();
  ClearDisplay();
  while(digitalRead(2) == 1);                    // Wait for fix
  Cross();
  ReadPosition(&xHome, &yHome);
}

void loop() {
  float xHere, yHere;
  ReadPosition(&xHere, &yHere);
  int dx = round((xHere - xHome)/10);
  int dy = round((yHere - yHome)/10);
  PlotPoint(dx+64, dy+32);
  while(digitalRead(2) == 1);
}