/* TinyNav Simple GPS Navigator v2

   David Johnson-Davies - www.technoblogy.com - 7th February 2016
   ATtiny85 @ 8 MHz (external crystal; BOD disabled)
   
   CC BY 4.0
   Licensed under a Creative Commons Attribution 4.0 International license: 
   http://creativecommons.org/licenses/by/4.0/
*/

#include <EEPROM.h>

// Constants
const int DataIn = 0;
const int LedsNS = 2;
const int LedsEW = 1;
const int EEPROMLat = 0, EEPROMLong = 4;

// GPS Parser **********************************************

// Example: $GPRMC,194509.000,A,4042.6142,N,07400.4168,W,2.03,221.11,160412,,,A*77
char fmt[]="$GPRMC,dddtdd.ddm,A,eeae.eeee,l,eeeae.eeee,o,djdk,ddd.dc,dddy??,,,?*??";

int state = 0;
unsigned int temp;
long ltmp;

// GPS variables
volatile unsigned int Time, Msecs, Knots, Course, Date;
volatile long Lat, Long;
volatile boolean Fix;

void ParseGPS (char c) {
  if (c == '$') { state = 0; temp = 0; }
  char mode = fmt[state++];
  // If received character matches format string, or format is '?' - return
  if ((mode == c) || (mode == '?')) return;
  // d=decimal digit
  char d = c - '0';
  if (mode == 'd') temp = temp*10 + d;
  // e=long decimal digit
  else if (mode == 'e') ltmp = ltmp*10 + d;
  // a=angular measure
  else if (mode == 'a') ltmp = ltmp*6 + d;
  // t=Time - hhmm
  else if (mode == 't') { Time = temp*10 + d; temp = 0; }
  // m=Millisecs
  else if (mode == 'm') { Msecs = temp*10 + d; ltmp = 0; }
  // l=Latitude - in minutes*1000
  else if (mode == 'l') { if (c == 'N') Lat = ltmp; else Lat = -ltmp; ltmp = 0; }
  // o=Longitude - in minutes*1000
  else if (mode == 'o') { if (c == 'E') Long = ltmp; else Long = -ltmp; temp = 0; }
   // j/k=Speed - in knots*100
  else if (mode == 'j') { if (c != '.') { temp = temp*10 + d; state--; } }
  else if (mode == 'k') { Knots = temp*10 + d; temp = 0; }
  // c=Course (Track) - in degrees*100
  else if (mode == 'c') { Course = temp*10 + d; temp = 0; }
  // y=Date - ddmm
  else if (mode == 'y') { Date = temp*10 + d ; Fix = 1; }
  else state = 0;
}

// USI UART **********************************************

unsigned char ReverseByte (unsigned char x) {
  x = ((x >> 1) & 0x55) | ((x << 1) & 0xaa);
  x = ((x >> 2) & 0x33) | ((x << 2) & 0xcc);
  x = ((x >> 4) & 0x0f) | ((x << 4) & 0xf0);
  return x;    
}

// Initialise USI for UART reception.
void InitialiseUSI (void) {  
  pinMode(DataIn, INPUT);         // Define DI as input
  USICR = 0;                      // Disable USI
  GIFR = 1<<PCIF;                 // Clear pin change interrupt flag
  GIMSK |= 1<<PCIE;               // Enable pin change interrupts
  PCMSK |= 1<<PCINT0;             // Enable pin change on pin 0
}

// Pin change interrupt detects start of UART reseption.
ISR (PCINT0_vect) {
  if (!(PINB & 1<<PINB0)) {       // Ignore if DI is high
    GIMSK &= ~(1<<PCIE);          // Disable pin change interrupts
    TCCR0A = 2<<WGM00;            // CTC mode
    TCCR0B = 0<<WGM02 | 2<<CS00;  // Set prescaler to /8
    TCNT0 = 0;                    // Count up from 0
    OCR0A = 51;                   // Delay (51+1)*8 cycles
    TIFR |= 1<<OCF0A;             // Clear output compare flag
    TIMSK |= 1<<OCIE0A;           // Enable output compare interrupt
  }
}

// COMPA interrupt indicates middle of start bit
ISR (TIMER0_COMPA_vect) {
  TIMSK &= ~(1<<OCIE0A);          // Disable COMPA interrupt
  TCNT0 = 0;                      // Count up from 0
  OCR0A = 103;                    // Shift every (103+1)*8 cycles
  // Enable USI OVF interrupt, and select Timer0 compare match as USI Clock source:
  USICR = 1<<USIOIE | 0<<USIWM0 | 1<<USICS0;
  USISR = 1<<USIOIF | 8;          // Clear USI OVF flag, and set counter
}

// USI overflow interrupt indicates we've received a byte
ISR (USI_OVF_vect) {
  USICR = 0;                     // Disable USI         
  char c = USIDR;
  ParseGPS(ReverseByte(c));
  GIFR = 1<<PCIF;                // Clear pin change interrupt flag.
  GIMSK |= 1<<PCIE;              // Enable pin change interrupts again
}

// Angle measurements **********************************************
const long DEGREE = 600000; // Number of 1e-4 minutes in a degree

// Difference between two angular measures, shortest way round Earth
long Diff (long deg1, long deg2) {
  long result = deg2 - deg1;
  if (result > 180 * DEGREE) return result - 360 * DEGREE;
  else if (result < -180 * DEGREE) return result + 360 * DEGREE;
  else return result;
}

// Approximation to cos for angle in 1e-4 minutes. Result scaled by 2^8.
unsigned int CosFix (long angle) {
  long u = labs(angle)>>16;
  u = (u * u * 6086)>>24;
  return 246 - u;
}

// Simple distance calculation; result in metres
unsigned int DistanceBetween (long lat1, long long1, long lat2, long long2) {
  long dx = (Diff(long2, long1) * CosFix((lat1 + lat2)/2)) / 256;
  long dy = Diff(lat2, lat1);
  unsigned long adx = labs(dx);
  unsigned long ady = labs(dy);
  unsigned long b = max(adx, ady);
  unsigned long a = min(adx, ady);
  if (b == 0) return 0;
  return 95 * (b + (110 * a / b * a + 128) / 256) / 512;
}

// Course from lat1,long1 to lat2,long2; result in degrees (0 to 359)
unsigned int CourseTo (long lat1, long long1, long lat2, long long2) {
  int c;
  long dx = (Diff(long2, long1) * CosFix((lat1 + lat2)/2)) / 256;
  long dy = Diff(lat2, lat1);
  long adx = labs(dx);
  long ady = labs(dy);
  if (adx == 0) c = 0;
  else if (adx < ady) c = (adx * (45 + (16 * (ady - adx))/ady))/ady;
  else c = 90 - (ady * (45 + (16 * (adx - ady))/adx))/adx;
  //
  if (dx <= 0 && dy < 0) return c;
  else if (dx < 0 && dy >= 0) return 180 - c;
  else if (dx >= 0 && dy >= 0) return 180 + c;
  else return 360 - c;
}

// Returns cardinal 0=N, 1=NE, 2=E, 3=SE, 4=S, 5=SW, 6=W, 7=NW from dir in degrees
int Cardinal (unsigned int dir) {
  return (((dir*2 + 45) / 90) & 7);
}

// Control LEDs **********************************************

// Display compass bearing; 0=N, 1=NE, 2=E ... to 7=NW; -1 = all off
void DisplayLEDs (int i) {
    pinMode(LedsEW, INPUT);  pinMode(LedsNS, INPUT);
    if (i < 0) return;
    if (i%4 != 0) {digitalWrite(LedsEW,(i/4 == 1)); pinMode(LedsEW,OUTPUT);}
    if ((i+2)%4 != 0) {digitalWrite(LedsNS,((i+2)/4 == 1)); pinMode(LedsNS,OUTPUT);}
}

// Light all 4 lights for delay msec
void FlashLEDs (int delay) {
  unsigned long start = millis();
  int flash = 1;
  do {
    DisplayLEDs(flash);
    flash = 6 - flash;
  } while (millis()-start < delay);
}
    
// Read and write long numbers to EEPROM **********************************************

void EEPROMWritelong(int address, long value) {
  EEPROM.write(address, value & 0xFF);
  EEPROM.write(address+1, value>>8 & 0xFF);
  EEPROM.write(address+2, value>>16 & 0xFF);
  EEPROM.write(address+3, value>>24 & 0xFF);
}

long EEPROMReadlong(long address) {
  return EEPROM.read(address) | (long)EEPROM.read(address+1)<<8 |
    (long)EEPROM.read(address+2)<<16 | (long)EEPROM.read(address+3)<<24;
}
      
// Setup **********************************************

long Lat1, Long1;

void setup (void) {
  InitialiseUSI();
  // Clear display
  DisplayLEDs(-1);
  // Read stored Lat and long
  Lat1 = EEPROMReadlong(EEPROMLat);
  Long1 = EEPROMReadlong(EEPROMLong);
  // Wait for fix
  do ; while(!Fix);
}

void loop (void) {
  long Lat2, Long2;                          // in minutes*10000
  unsigned int Course2, WayHome, Correction; // All in degrees 0 to 359
  unsigned int Distance;                     // in metres
  // Read data
  cli(); Lat2 = Lat; Long2 = Long; Course2 = Course; sei();
  Fix = 0;
  Course2 = Course2/100;
  WayHome = CourseTo(Lat2, Long2, Lat1, Long1);
  if (WayHome >= Course2) Correction = WayHome-Course2;
    else Correction = WayHome+360-Course2;
  Distance = min(DistanceBetween(Lat2, Long2, Lat1, Long1), 1000);
  // If button pressed, set Home to current Latitude and Longitude and store in EEPROM
  if (analogRead(A0) < 900) {
    Lat1 = Lat2; Long1 = Long2;
    EEPROMWritelong(EEPROMLat, Lat1);
    EEPROMWritelong(EEPROMLong, Long1);
    FlashLEDs(1000);                         // One second
  }
  // Display direction while waiting for fresh data
  unsigned long Start = millis();
  do {
    if (millis() - Start > Distance) DisplayLEDs(Cardinal(Correction));
  } while(!Fix);
  DisplayLEDs(-1); // All LEDs off
}