//2 tilt servo motors
//micromag 3axis module

int servoAngleMidPulseX= 1500;
int servoAngleMidPulseY= 1500;
int servoAngleRange=900;

int servoX,servoX2;
int servoY,servoY2;
long lastPulse = 0;    // the time in milliseconds of the last pulse
int refreshTime = 20; // the time needed in between pulses
int lastFuncPulse=0;

int distance=0;
long angle=0;
long cDist=0;
long pDist=0;

float servoAngle;

boolean printStuff=false;

boolean startingUp=true;
boolean gpsConnected=false;
boolean satelliteLock=false;


float xpos, ypos;
//GPS

int rxPin = 2;                    // RX PIN 
int txPin = 3;                    // TX TX
int byteGPS=-1;
char linea[300] = "";
char comandoGPR[7] = "$GPRMC";
int cont=0;
int bien=0; 
int conta=0;
int indices[13];


//itp                    40729489, -73993662
//washington square park 40730849, -73997520
//796 dekalb             40692163, -73944355
long wLatitude=40730849;
long wLongitude=-73997520;
long myLatitude=40729489; //north south
long myLongitude=-73993662; //east west
long initDist=0;


//vibe
int vibe=4;
int vibeOut=0;





//Arduino code for controlling a MicroMag3 magnetometer through SPI
//by Daniel Soltis
#define SCLK 7 //the clock to pulse (#1 on breakout)
#define MISO 6 //master in, slave out (#2 on breakout)
#define MOSI 5 //master out, slave in (#3 on breakout)
#define SSNOT 4 //when low, the device is enabled (#4 on breakout)
#define DRDY 3 //this is low after a reset, high when data is ready (#5 on breakout)
#define RESET 2 //this needs to be toggled low-high-low before each measurement (#6 on breakout)

int x = 0;
int y = 0;
int z = 0;
int heading = 0;
long microMagPulse=0;
int compassZ;
float headingValue=0;
float tiltValue=0;

//accelerometer
int AccelX, AccelY, AccelZ; 
boolean zero=false;
long lastAccelReading=0;
float tiltX,tiltY, tiltZ;

float vertAngleX;
float vertAngleY;

float tiltYmax=112;
float tiltXmax=112;
float tiltZmax=80.0;
float tiltXangle;
float tiltYangle;
float pitch, roll;


//xbee
char XBline[30];
int XBcounta=0;
int XBindices[5];
int XBcount=0;
int XBrxPin = 0;
int XBtxPin = 1; 

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void setup() {

  Serial.begin(9600);

  // Set servo pin as OUTPUT
  pinMode(10,OUTPUT);
  pinMode(11,OUTPUT);
  pinMode(12, OUTPUT); 
  pinMode(13, OUTPUT);
  servoX = servoAngleMidPulseX;//set position to middle           
  servoY = servoAngleMidPulseY;        

  //GPS
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  for (int i=0;i<300;i++){       // Initialize a buffer for received data
    linea[i]=' ';
  }

  //xbee
  pinMode(XBrxPin, INPUT);
  pinMode(XBtxPin, OUTPUT);
  for (int i=0;i<30;i++){       // Initialize a buffer for received data
    XBline[i]=' ';
  }

  //keep reset pin HIGH
  pinMode(12,OUTPUT);
  digitalWrite(12, LOW); 


  //micromag
  pinMode(SSNOT, OUTPUT);
  pinMode(RESET, OUTPUT);
  pinMode(MOSI, OUTPUT);
  pinMode(MISO, INPUT);
  pinMode(DRDY, INPUT);
  pinMode(SCLK, OUTPUT);



}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void printInfo(){

  Serial.print("-------#");
  //Serial.println(locationCount);
  //Serial.println(wLocations[locationCount]);

  //Serial.print("wLat: ");
  //Serial.println(wLatitude);
  //Serial.print("wLong: ");
  //Serial.println(wLongitude);

  Serial.print("myLat: ");
  Serial.println(myLatitude);
  Serial.print("myLong: ");
  Serial.println(myLongitude);

  cDist=dist(myLongitude,myLatitude,wLongitude,wLatitude); // current dist

  Serial.print("initial dist: ");
  Serial.println( initDist);
  Serial.print("current dist: ");
  Serial.println(cDist);

  Serial.print("angle: ");
  Serial.println(angle);

  if(gpsConnected==true){
    Serial.println("GPS connected");  
  }
}


///////////////////////////////////////////////////

float smoothCompass(float data, float filterVal, float smoothedVal){
  if(data-smoothedVal>2000) data-=3600;
  if(data-smoothedVal<-2000) data+=3600;
  if (filterVal > 1)   filterVal = .99;
  else if (filterVal <= 0)  filterVal = 0;
  smoothedVal = (data * (1 - filterVal)) + (smoothedVal  *  filterVal);
  return smoothedVal;
}


float smooth(float data, float filterVal, float smoothedVal){
  if (filterVal > 1) filterVal = .99;
  else if (filterVal <= 0) filterVal = 0;
  smoothedVal = (data * (1 - filterVal)) + (smoothedVal  *  filterVal);
  return smoothedVal;
}
/////////////////////////////////////////////////////

//Micromag 3-axis

void send_bit(int _high){
  //this sends the bit on the rising edge of the clock
  digitalWrite(MOSI, _high);
  digitalWrite(SCLK, HIGH);
  digitalWrite(SCLK, LOW);
}

int receive_bit(){
  //this receives the data on the falling edge of the clock
  digitalWrite(SCLK, HIGH);
  int bit = digitalRead(MISO); 
  digitalWrite(SCLK, LOW);
  return bit;
}

long readaxis(int _axis){
  //this function sends eight bits, waits until the data is ready
  //and receives 16 bits
  long axisValue=0;

  //pulse the reset
  digitalWrite(RESET, LOW);
  digitalWrite(RESET, HIGH);
  digitalWrite(RESET, LOW);

  //send the command byte
  //this sends data that we are not in debug mode
  //and sets the amount of time to read the magnetic sensors (the ASIC period)
  //as /2048 
  /*
  send_bit(LOW);
   send_bit(HIGH);
   send_bit(HIGH);
   send_bit(LOW);
   send_bit(LOW);
   send_bit(LOW);
   */
  //as 1024
  send_bit(LOW);
  send_bit(HIGH);
  send_bit(LOW);
  send_bit(HIGH);
  send_bit(LOW);
  send_bit(LOW);


  //the last two bits select the axis
  if (_axis == 0){ //x axis
    send_bit(LOW);
    send_bit(HIGH);
  }
  else if (_axis == 1){ //y axis
    send_bit(HIGH);
    send_bit(LOW);
  }
  else{ //z axis
    send_bit(HIGH);
    send_bit(HIGH);
  }

  //wait until the drdy line is high
  while (digitalRead(DRDY) == LOW){
    //wait
    //Serial.println("waiting"); 
  }
  //Serial.println("data!");
  long runningtotal = 0;
  //receive the results and tally them up as they come in 
  //the leftmost bit signs the number as positive or negative
  long sign = receive_bit();
  //the remaining bits need to be translated from individual bits into an integer
  for (int i = 14; i >= 0; i = i - 1){
    long thisbit = receive_bit();
    thisbit = thisbit << i;
    runningtotal = runningtotal | thisbit;
  }
  if (sign == 1)  runningtotal = runningtotal - 32768; 
  //this sends the data over the serial line, as a string separated by commas
  axisValue = runningtotal;
  //Serial.print(axisValue,DEC);
  //Serial.print(44, BYTE);
  return axisValue;
}


//////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////

float getSphericalHeading(float x, float y, float z){//, float tiltX, float tiltY, float tiltXZ){
  float heading=0;
  if (x==0 && y<0) heading=900.0;
  if(x==0 && y>0) heading=2700.0;
  if (x < 0) heading = (PI - atan(y/x))/TWO_PI*3600.0;
  if (x > 0 && y < 0) heading = (-1.0*atan(y/x))/TWO_PI*3600.0;
  if (x > 0 && y > 0) heading = (2.0*PI - atan(y/x))/TWO_PI*3600.0;

  // heading= (atan2(y,x)+PI)/TWO_PI*3600.0;  //heading = atan(y/x)/TWO_PI*3600.0;
  //Serial.print("heading: ");
  //Serial.println(int(heading));
  return heading;
}


float getSphericalTilt(float x, float y, float z){
  float tiltHeading=0;
  float r=sqrt(x*x+y*y+z*z);  
  tiltHeading=acos(z/r)/TWO_PI*3600.0;
  return tiltHeading;
}

//////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//convert NMEA gps syntax to decimal point degrees
long decimalMinutes( long l, char dir){ 
  //latitude ddmmmmmm
  //longitude dddmmmmmm
  long decimal;  //ddmmmmmm
  float ll=(float)l/1000000.0;//dd.mmmmmmm
  int dd=floor(ll);//dd.mmmmmm
  float mmmmmm=(ll-dd);// .mmmmmm
  float dddddd=mmmmmm/6.0*10.0; // .mmmmmm convert minutes to decimal degrees .dddddd
  decimal=(float)(dd+dddddd)*1000000.0;

  if(dir=='W'||dir=='S')decimal=decimal*-1.0;
  return decimal;
}

long dist(long x,long y, long xx, long yy){
  float scale=10.0;
  distance=min(sqrt((x-xx)/scale*(x-xx)/scale+(y-yy)/scale*(y-yy)/scale),1000);
  distance=constrain(distance,0,1000);
  return distance;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void readGPS() {
  byteGPS=Serial.read();         // Read a byte of the serial port
  if (byteGPS == -1) {           // See if the port is empty yet
    //delay(100); 
    //gpsConnected=false;
  } 
  else {
    //gpsConnected=true;
    linea[conta]=byteGPS;        // If there is serial port data, it is put in the buffer
    conta++;                      
    //printByte(byteGPS); //print raw rx
    if (byteGPS==13){            // If the received byte is = to 13, end of transmission
      cont=0;
      bien=0;
      for (int i=1;i<7;i++){     // Verifies if the received command starts with $GPR
        if (linea[i]==comandoGPR[i-1]) bien++;
      }
      if(bien==6){               // If yes, continue and process the data
        for (int i=0;i<300;i++){
          if (linea[i]==','){    // check for the position of the  "," separator
            indices[cont]=i;
            cont++;
          }
          if (linea[i]=='*'){    // ... and the "*"
            indices[12]=i;
            cont++;
          }
        }

        //satellite lock
        char satLock;
        for (int j=indices[1];j<(indices[2]-1);j++){
          //if(printStuff) Serial.print(linea[j+1]); 
          //if(printStuff) Serial.println("");
          if(linea[j+1]=='V')satelliteLock=false;
          else satelliteLock=true;
          satLock=linea[j+1];
        }

        //latitude
        char NS;
        char tempLat[9];
        int tempLatCount=0;
        for (int j=indices[2];j<(indices[3]-1);j++){
          //print raw
          if(linea[j+1]!='.'){//remove decimal
            //if(printStuff) Serial.print(linea[j+1]); 
            tempLat[tempLatCount]=linea[j+1];
            tempLatCount++;
          }
        }

        //if(printStuff) Serial.println("");
        for (int j=indices[3];j<(indices[4]-1);j++){
          //if(printStuff) Serial.println(linea[j+1]); 
          NS=linea[j+1];
        }

        // myLatitude=strtol(tempLat);
        long myLat=strtol(tempLat, NULL, 10);
        myLatitude=decimalMinutes(myLat,NS);
        //myScreenLatitude=latToScreen(myLatitude);

        //longitude
        char EW;
        char tempLong[10];
        int tempLongCount=0;

        for (int j=indices[4];j<(indices[5]-1);j++){
          //print raw
          if(linea[j+1]!='.'){//remove decimal
            //if(printStuff) Serial.print(linea[j+1]); 
            tempLong[tempLongCount]=linea[j+1];
            tempLongCount++;
          }
        }
        //if(printStuff) Serial.println("");

        for (int j=indices[5];j<(indices[6]-1);j++){
          //if(printStuff) Serial.println(linea[j+1]); 
          EW=linea[j+1];
        }

        long myLong=strtol(tempLong, NULL, 10);
        myLongitude=decimalMinutes(myLong,EW);

        if (startingUp){
          //gps setup strings
          //Serial.print("$PMTK501,2*28\r\n");//turn on dgps
          //Serial.print("$PMTK313,1*2E\r\n");//turn on sb
          //Serial.print("$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n");//gprmc only
          startingUp=false;
          //gpsConnected=true;
          //set initial location wash
          wLatitude=40730849;
          wLongitude=-73997520;
        }
        if(satelliteLock && startingUp){
          //initDist= dist(myScreenLongitude,myScreenLatitude,wScreenLongitude,wScreenLatitude);
          initDist= dist(myLongitude,myLatitude,wLongitude,wLatitude);
          startingUp=false;
        }
      }
      conta=0; // Reset the buffer
      for (int i=0;i<300;i++){  
        linea[i]=' ';       
      }                 
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setZero(){
  delay(1000);
  //AccelX=522;//analogRead(0);
  //AccelY=442;//analogRead(2);
  //AccelZ=617;//analogRead(1);

  AccelX=analogRead(0);
  AccelY=analogRead(2);
  AccelZ=analogRead(1);

  compassZ=readaxis(0);
  zero=true;

  /*
   Serial.println(AccelX);
   Serial.println(AccelY);
   Serial.println(AccelZ);
   */
}




/*
void setDestination() {
 // put the radio in command mode:
 Serial.print("+++");
 // wait for the radio to respond with "OK\r"
 char thisByte = 0;
 
 //wait for OK
 while (thisByte != '\r') {
 if (Serial.available() > 0) {
 thisByte = Serial.read();
 }
 }
 
 //Serial.print("ATID2222,DH0,DL2,MY1,BD3,WR,CN\r");
 Serial.print("ATID2222,DH0,DL1,MY2,BD3,WR,CN\r");
 }
 */

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void sendPosition(){
  //send position
  Serial.print("$");
  Serial.print(myLatitude);
  Serial.print(",");
  Serial.print(myLongitude);
  Serial.print("*");
  //Serial.print('a');

}

void sendWayPosition(){
  //send position
  Serial.print("W");
  Serial.print(wLatitude);
  Serial.print(",");
  Serial.print(wLongitude);

}


void getPosition()
{
  //send for data
  //data format= $40729489, -73993662*
  int XBbyte=Serial.read();         // Read a byte of the serial port
  if (XBbyte != -1)
  {
    //Serial.println("reading");
    if(XBbyte=='a')
    {
      sendPosition(); 
    }
    if(XBbyte=='b')
    {
      sendWayPosition(); 
    }

    else
    {
      XBline[XBcounta]=XBbyte;        // If there is serial port data, it is put in the buffer
      XBcounta++;                      
      //printByte(XBbyte); //print raw rx

      if (XBbyte=='*')
      {
        //Serial.println("found end");
        XBcount=1;
        if (XBline[0]=='$')          //data starts after '$'
        {
          XBindices[0]=1;
          for (int i=0;i<30;i++)
          {
            if (XBline[i]==',')// "," separator
            {    
              XBindices[XBcount]=i;
              XBcount++;
            }
            if (XBline[i]=='*') //end transmission
            {    
              XBindices[XBcount]=i;
              XBcount++;
            }
          }

          //latitude
          char tempwLat[9];
          int tempwLatCount=0;
          for (int j=XBindices[0];j<(XBindices[1]);j++)
          {
            if(XBline[j+1]!='.')
            {  
              //remove decimal
              //if(printStuff) Serial.print(linea[j+1]); 
              tempwLat[tempwLatCount]=XBline[j];
              tempwLatCount++;

            }
          }

          long mywLat=strtol(tempwLat, NULL, 10);
          wLatitude=mywLat;

          //longitude
          char tempwLong[10];
          int tempwLongCount=0;

          for (int j=XBindices[1]+1;j<(XBindices[2]);j++){
            //print raw
            if(XBline[j+1]!='.'){//remove decimal
              //if(printStuff) Serial.print(linea[j+1]); 
              tempwLong[tempwLongCount]=XBline[j];
              tempwLongCount++;
              //Serial.print(XBline[j]);
            }
          }
          long wLong=strtol(tempwLong, NULL, 10);
          wLongitude=wLong;
          //Serial.println("wLong set");
        }
        XBcounta=0; // Reset the buffer
        for (int i=0;i<30;i++){  
          XBline[i]=' ';       
        }      
      }
    }
  }
}



void loop() {
  //set zero
  if(zero==false){
    setZero();
  }


  //tilt
  //if (millis() - lastAccelReading >= 5) {
  tiltX=smooth(analogRead(0)-AccelX,.9,tiltX);
  tiltY=smooth(analogRead(2)-AccelY,.9,tiltY);
  tiltZ=smooth(analogRead(1)-AccelZ,.9,tiltZ);
  //tiltX=analogRead(0);
  //tiltY=analogRead(2);
  //tiltZ=analogRead(1);
  //Serial.println((int)tiltX);
  lastAccelReading=millis();
  //}

  pitch=smooth(atan(tiltX/ sqrt(tiltY*tiltY+tiltZ*tiltZ) ), .7, pitch);
  roll=smooth(atan(tiltY/ sqrt(tiltX*tiltX+tiltZ*tiltZ) ), .7, roll);

  //readGPS();
  getPosition();


  //compass
  //x range 138 to 468 =330 range. adjusted to -165 to 165
  //x=readaxis(1)-290;// to 512 range
  // y range uses actual z axis magnet range is -270 to 70. adjust so range is -170 to 170
  //y=readaxis(2)+100;
  //z range 0-250 mapped to 0-170
  //z=constrain( (readaxis(0)+163)/250.0*170.0, 0, 170);


  //Serial.println((int)(pitch));
  /*
    float totalAccel=sqrt(tiltY*tiltY+tiltZ*tiltZ+tiltY*tiltY);
   float g=1.0;    
   if (totalAccel>g){
   //accelerating, not tilting
   }
   Serial.println((int)(totalAccel*100));
   */
  z=readaxis(0);
  //z=z/160.0*100.0;

  //x axis -95 to 95
  x=readaxis(1);//-65-(250-65)/2.0;
  //x= ( x/95.0*100.0 );
  x=x*cos(pitch)+y*sin(roll)*sin(pitch)-z*cos(roll)*sin(pitch);

  //y axis -86 to 86 range
  y=readaxis(2);//+177/2.0;
  //y=( y/86.0*100.0 );
  y=y*cos(roll)+z*sin(roll);


  //Serial.println((int)tiltXangle);
  //Serial.println(x);
  //Serial.println(y);
  //Serial.println(z);


  headingValue=smoothCompass(getSphericalHeading(x,y,z),.96,headingValue);
  //tiltValue=smooth(getSphericalTilt(x,y,z),.92,tiltValue);

  //Serial.println((int)headingValue);
  //Serial.println((int)tiltValue);




  //satelliteLock=true;
  //gpsConnected=true;

  if(gpsConnected==false){
    //----simulation mode
    //itp                    40729489, -73993662
    //washington square park 40730849, -73997520
    //796 dekalb             40692163, -73944355
    myLatitude=40729489;   
    myLongitude=-73993662;
  }


  //heading angle
  long  a= atan2((myLatitude-wLatitude),(myLongitude-wLongitude) )/(float)PI*1800.0+2700;//+1800.0;//-pi to pi
  angle=-a+headingValue;//translate angle to 0-3600
  if(angle>3600)angle=angle%3600;
  if(angle<0)angle=(angle+3600);
  //Serial.println(angle);

  //distance
  //if(satelliteLock==true){
  cDist=dist(myLongitude,myLatitude,wLongitude,wLatitude); // current dist
  distance=int((float)cDist/(float)initDist*350.0); //translate distance to 0-350
  /*
  if(cDist!=pDist){
   Serial.print("current dist: ");
   Serial.println(cDist);
   pDist=cDist;
   }
   */

  //angle=0;

  //compas only
  xpos=cos(angle/3600.0*TWO_PI);
  ypos=sin(angle/3600.0*TWO_PI);

  //tilt only
  //xpos= -sin( (tiltXangle/90.0*PI/4.0));
  //ypos= -sin( (tiltYangle/90.0*PI/4.0));

  //angle and tilt
  //xpos=smooth( cos((float)angle/3600.0*TWO_PI) +(tiltXmax/2.0-tiltX)/(float)tiltXmax , .75 , xpos );
  //ypos=smooth( sin((float)angle/3600.0*TWO_PI)+(tiltYmax/2.0-tiltY)/(float)tiltXmax , .75 , ypos );

  servoX  = xpos*(float)servoAngleRange+servoAngleMidPulseX;    // convert the analog value
  servoY = ypos*(float)servoAngleRange+servoAngleMidPulseY;    // convert the analog value

  //servoX2  =xpos*(float)servoAngleRange+servoAngleMidPulseX;    // convert the analog value
  //servoY2 = ypos*(float)servoAngleRange+servoAngleMidPulseY; 

  //}


  //no GPS loc
  if(satelliteLock==false && gpsConnected==true){
    servoX = servoAngleMidPulseX;    // neutral
    servoY = servoAngleMidPulseY;    // neutral
  }




  //servo
  if (millis() - lastPulse >= refreshTime) {
    //servoX
    digitalWrite(11, HIGH);  // Turn the motor on
    delayMicroseconds(servoX);
    digitalWrite(11, LOW);    // Turn the motor off
    //servoY
    digitalWrite(10, HIGH);   // Turn the motor on
    delayMicroseconds(servoY);  
    digitalWrite(10, LOW);    // Turn the motor off 
    /*
    digitalWrite(10, HIGH);  // Turn the motor on
     delayMicroseconds(servoX2);
     digitalWrite(10, LOW);    // Turn the motor off
     //servoY
     digitalWrite(11, HIGH);   // Turn the motor on
     delayMicroseconds(servoY2);  
     digitalWrite(11, LOW);    // Turn the motor off 
     */
    lastPulse = millis();           // save the time of the last pulse
  }
}