/*
 * Simple autonomous navigation using GPS data
 *
 * Achim Walther, 20.03.2006
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
//#include "i2c-dev.h"

#define REAL 1        // set 0 for simulation

#define COMMANDS "zonda.route"  // command file

#if REAL
// definitions for real life
#define LOGNAME "/mnt/mmc/log/autodrive.log"
#define GPS "/dev/ttyS3"    // for real life
#define IMU "/dev/ttyS2"
#else
// definitions for test mode
#define LOGNAME "autodrive_sim.log"
#define GPS "gpsdev.log"    // for debugging
#define IMU "imudev.log"
#endif

#define NMEA_MAX    82      // max length of NMEA sentence
#define COMM_MAX    40      // max length of route commands

#define PI 3.14159265
#define DGR_TO_M 111200     // 1 degree is roughly 111200 meters

#define REACH_DIST 3.0      // max distance to target that counts as reached
#define MIN_SPEED 0.5       // minimum speed to consider a track value valid

#define VOLT 0.0332
#define KADC 0.01953125     // 5V / 256

const double dgrToRad = PI / 180.0;
const double radToDgr = 180.0 / PI;

FILE *gpsDev;
FILE *commandFile;

struct gps_fix_t {
    double latDgr;      // Latitude in degrees
    double lonDgr;      // Longitude in degrees
    double altM;        // Altitude in meters
    double trackDgr;    // Course made good (relative to true north)
    double speedKmpH;   // Speed over ground, kilometers / hour
    short  sats;        // number of sats used
    char lastId[6];     // the ID or type of NMEA sentence that was processed recently
} gps_fix;

struct target_t {
    double latDgr;      // target Latitude in degrees
    double lonDgr;      // target Longtitude in degrees
    double speedKmpH;   // target speed 
    short  set;         // 1 if a target location was set
} target;

int servoThrottle = 0;
int servoSteering = 0;
int autoMode = 0;
char ledStatus = 0;

struct timeval tim;

// all servo values in microseconds for PWM
#define SV_THROT 1      // throttle has servo channel 1
#define SV_STEER 2      // steering has servo channel 2
#define MIN_THROT 0     // min value for throttle servo
#define MAX_THROT 800   // max value for throttle servo
#define MAX_STEER 200   // max value for steering - only half the servo path is used
#define OFF_THROT 1100  // offset to get real PWM times for throttle servo
#define OFF_STEER 1568  // standard neutral PWM time for steering servo

// constants for PID control
#define TA 0.25       // time between GPS measurements
#define KP 2
#define KI 4
#define MINMAX_ESUM 800.0    // limit for eSum

#define KPST 1         // factor for steering control
#define KSPEED 5    // factor for speed dependend steering

#define LED_WAITING 0
#define LED_AUTO 1
#define LED_END 2
#define LED_ERROR 3

int servoDev;
int servoI2cAddr = 0x60;
int connDev;
int connI2cAddr = 0x50;

FILE *gpsDev;
FILE *logfile;

static short setServo(short servoNum, int value) {
    short result = 1;
    unsigned char buf[3];
    // convert int value into 2 bytes
    buf[0] = 0xAA;      // command move servo
    buf[1] = (servoNum<<4) + (value / 256);  // servo number plus high byte of servo value
    buf[2] = value % 256;
    printf("I2C write: 60 %02x %02x %02x\n", buf[0], buf[1], buf[2]);
    fprintf(logfile, "+++ I2C write: 60 %02x %02x %02x\n", buf[0], buf[1], buf[2]);
    fprintf(logfile, "$SVSET,%02x%02x*\n", buf[1], buf[2]);
    
    #if REAL
    if ( write(servoDev, buf, 3) != 3) {
        result = 0;
        fprintf(logfile, "--- I2C Send %x Failed\n", servoI2cAddr);
    }
    #endif

    return result;
}

static short setLEDs(short mode, int value) {
    short result = 1;
    char buf[1];
    
    if (value) {
        ledStatus |= 1 << (mode + 4);
    } else {
        ledStatus &= ~(1 << (mode +4));
    }
    buf[0] = ledStatus;
    
    // convert int value into 2 bytes
    printf("I2C write: 50 %02x\n", buf[0]);
    fprintf(logfile, "+++ I2C write: 50 %02x\n", buf[0]);
    
    #if REAL
    if ( write(connDev, buf, 1) != 1) {
        result = 0;
        fprintf(logfile, "--- I2C Send %x Failed\n", connI2cAddr);
    }
    #endif

    return result;
}

static void readServos() {
    int read_bytes = 7*2;
    char buf[read_bytes];
    int i;
    
    gettimeofday(&tim, NULL);
    double t1 = tim.tv_sec+(tim.tv_usec/1000000.0);  

    if ( read(servoDev, buf, read_bytes) != read_bytes) {
        fprintf(logfile, "--- I2C Send %x Failed\n", servoI2cAddr);
    }

    fprintf(logfile, "$SVRCP,%.2f,", t1);
    for (i=0; i<read_bytes; ) {
        fprintf(logfile, "%02x%02x",buf[i++], buf[i++]);
        if (i<read_bytes) fprintf(logfile, ",");
    }
    fprintf(logfile, "*\n");
    
    // get current mode from servo readings
    int modeUs = 256 * buf[6*2] + buf[6*2+1];
    autoMode = (abs(modeUs - 1500) > 250);
}

static void readADC() {
    int adcBytes=4;
    char buf[adcBytes];
    
    gettimeofday(&tim, NULL);
    double t1 = tim.tv_sec+(tim.tv_usec/1000000.0);  

    // --- read ADC values ---
    if (read(connDev, buf, adcBytes) != adcBytes) {
        fprintf(logfile, "--- I2C Read %x Failed\n", connI2cAddr);
    }

    fprintf(logfile, "$GSAUX,%.2f,%.2f,%.2f,%.2f,%.2f*\n", 
                     t1, buf[0]*KADC, buf[1]*KADC, buf[2]*VOLT, buf[3]*VOLT);
}

/*
 * copied from gpsd nmea_parse.c 
 */
short nmea_checksum(char *sentence, unsigned char *correct_sum)
/* is the checksum on the specified sentence good? */
{
    unsigned char sum = '\0';
    char c, *p = sentence, csum[3];

    while ((c = *p++) != '*' && c != '\0')
    sum ^= c;
    if (correct_sum)
        *correct_sum = sum;
    (void)snprintf(csum, sizeof(csum), "%02X", sum);
    return(toupper(csum[0])==toupper(p[0]))&&(toupper(csum[1])==toupper(p[1]));
}

static void procLatLon(char *field[])
/* process a pair of latitude/longitude fields starting at field index BEGIN */
{
    double lat, lon, d, m;
    char str[20], *p;
    
    if (*(p = field[0]) != '\0') {
        strncpy(str, p, 20);
        (void)sscanf(p, "%lf", &lat);
        m = 100.0 * modf(lat / 100.0, &d);
        lat = d + m / 60.0;
        p = field[1];
        if (*p == 'S')
            lat = -lat;
        if (gps_fix.latDgr != lat)
            gps_fix.latDgr = lat;
    }
    if (*(p = field[2]) != '\0') {
        strncpy(str, p, 20);
        (void)sscanf(p, "%lf", &lon);
        m = 100.0 * modf(lon / 100.0, &d);
        lon = d + m / 60.0;

        p = field[3];
        if (*p == 'W')
            lon = -lon;
        if (gps_fix.lonDgr != lon)
            gps_fix.lonDgr = lon;
    }
}

void nmea_parse(char *sentence)
// parse an NMEA sentence, unpack it into a session structure
{
    unsigned char buf[NMEA_MAX+1];

    int count;
    unsigned int i;
    char *p, *field[NMEA_MAX];
    unsigned char sum;

    if (!nmea_checksum(sentence+1, &sum)) {
        fprintf(logfile, "--- Bad NMEA checksum: '%s' should be %02X\n", sentence, sum);
        return;
    }

    // make an editable copy of the sentence
    strncpy((char *)buf, sentence, NMEA_MAX);
    // discard the checksum part
    for (p = (char *)buf; (*p != '*') && (*p >= ' '); ) ++p;
    *p = '\0';
    // split sentence copy on commas, filling the field array
    for (count = 0, p = (char *)buf; p != NULL && *p != '\0'; ++count, p = strchr(p, ',')) {
    *p = '\0';
    field[count] = ++p;
    }

    strcpy(gps_fix.lastId, field[0]);
    
    if (strcmp(field[0], "GPGGA") == 0) {
        // process GGA sentence
        
        procLatLon(&field[2]);
        if (field[9][0] != '\0')
            gps_fix.sats = atof(field[7]);
            gps_fix.altM = atof(field[9]);
        fprintf(logfile, "+++ Lat %f Lon %f Sats %d\n", gps_fix.latDgr, gps_fix.lonDgr, gps_fix.sats);
    } else if (strcmp(field[0], "GPVTG") == 0) {
        // process VTG sentence
        
        gps_fix.trackDgr = atof(field[1]);
        gps_fix.speedKmpH = atof(field[7]);
        
        fprintf(logfile, "+++ Track %f Speed %f\n", gps_fix.trackDgr, gps_fix.speedKmpH);
    }
} 

void gotoTarget(void) 
{
    char sentence[NMEA_MAX];
    
    double lastDistanceM = REACH_DIST;
    double eSpeed, ySpeed, eSum = 0.0;

    printf("goto target %f %f\n", target.latDgr, target.lonDgr);
    fprintf(logfile, "+++ goto target %f %f\n", target.latDgr, target.lonDgr);
    
    // read GPS until the position is near the target
    short reached = 0;
    while ((fgets(sentence, NMEA_MAX, gpsDev) != NULL) && !reached) {
        if (sentence[0] == '$') {
        fprintf(logfile, sentence);
        nmea_parse(sentence);
        //printf("lastId: %s\n", gps_fix.lastId);
        if (strcmp(gps_fix.lastId, "GPGGA") == 0) {
        
            readServos();
            setLEDs(LED_AUTO, autoMode);
            
            // approximation of distance from current position to target            
            double diffLatM = DGR_TO_M * (target.latDgr-gps_fix.latDgr);
            double diffLonM = DGR_TO_M * (target.lonDgr-gps_fix.lonDgr) * cos (gps_fix.latDgr * dgrToRad);
            double distanceM = sqrt (diffLatM * diffLatM + diffLonM * diffLonM);
            
            // calculate bearing to target
            double bearingDgr = 90.0 - radToDgr* atan2 (diffLatM, diffLonM);

            //printf("diffLatM: %f  diffLonM: %f  lastDistanceM: %f distanceM: %f\n", diffLatM, diffLonM, lastDistanceM, distanceM);
            fprintf(logfile, "+++ distanceM %f  bearingDgr %f\n", distanceM, bearingDgr);
            
            // target is reached if the distance is < REACH_DIST and increasing
            // that is to make sure we passed the nearest point
            if ((lastDistanceM < REACH_DIST) && (distanceM > lastDistanceM)) {
                reached = 1;
            }
            lastDistanceM = distanceM;
            
            if (!reached && autoMode && gps_fix.speedKmpH > MIN_SPEED) {
                // control steering servo to reach target
                
                // could not find a way to get modulo (%) to work with doubles...
                if (bearingDgr > 360.0) bearingDgr -= 360.0;
                else if (bearingDgr < 0.0) bearingDgr += 360.0;
                
                double diffTrack = (bearingDgr - gps_fix.trackDgr);
                if (diffTrack > 180.0) diffTrack -= 360.0;
                else if (diffTrack < -180.0) diffTrack += 360.0;
                
                // compute sqrt of distance to steering to avoid oszilation when close to target
                servoSteering = (int) (KPST * sqrt(distanceM) * diffTrack);
                
                // limit servo steering using MAX and MIN values depending on speed
                int maxS = MAX_STEER;
                if (gps_fix.speedKmpH > KSPEED) {
                    maxS = KSPEED / gps_fix.speedKmpH;
                }
                
                // assume symmetric steering with minS = -maxS
                if (servoSteering > maxS) servoSteering = maxS;
                else if (servoSteering < -maxS) servoSteering = -maxS;

                fprintf(logfile, "+++ diffTrack %f  servoSteering %d\n", diffTrack, servoSteering);
                // write to I2C
                servoSteering += OFF_STEER;

                setServo(SV_STEER, servoSteering);
            }

            readADC();

            fflush(logfile);
            
            #if (REAL == 0)            
            // for debugging
            usleep(250000);
            #endif

        } // if (strcmp(gps_fix.lastId, "GPGGA") == 0)

        if (strcmp(gps_fix.lastId, "GPVTG") == 0) {
            // get current speed and control servo to reach target speed
            // use a PI control loop
            
            eSpeed = target.speedKmpH - gps_fix.speedKmpH;  // difference between target and current speed
            eSum += eSpeed;    // summarize error
            // limit eSum
            if (eSum > MINMAX_ESUM) eSum = MINMAX_ESUM;
            else if (eSum < -MINMAX_ESUM) eSum = -MINMAX_ESUM;
            
            // calculate control value y
            ySpeed = target.speedKmpH * (KP * eSpeed + KI * TA * eSum);
            
            // limit servo throttle
            if (ySpeed > MAX_THROT)
                ySpeed = MAX_THROT;
            else if (ySpeed < MIN_THROT)
                ySpeed = MIN_THROT;
            
            servoThrottle = (int) ySpeed + OFF_THROT;

            fprintf(logfile, "+++ eSpeed %f  ySpeed %f  servoThrottle %d\n", eSpeed, ySpeed, servoThrottle);
            
            // write to I2C
            setServo(SV_THROT, servoThrottle);

        } // if (strcmp(gps_fix.lastId, "GPVTG") == 0)
        
    } // if (sentence[0] == '$')
    } // while
}


int main(int argc, char *argv[])
{
    #if (REAL == 0)
    printf ("Simulation mode!\n");
    #endif

    printf("Logging data to file %s\n", LOGNAME);
    
    // open logfile
    if ( (logfile = fopen(LOGNAME, "a")) == NULL) { 
        printf("Can't open %s\n", LOGNAME);
        exit(1);
    }

    #if REAL
    // --- open I2C devices ---
    int adapter_nr = 0; /* probably dynamically determined */
    char filename[20];

    printf("opening servo switch...\n");
    
    // open i2c device servoswitch
    sprintf(filename,"/dev/i2c-%d",adapter_nr);
    if ((servoDev = open(filename,O_RDWR)) < 0) {
        printf("ERRNO: %s\n",strerror(errno));
        exit(1);
    }
    
    // set slave address
    if (ioctl(servoDev, 0x0703, servoI2cAddr) < 0) {
        printf("ERRNO: %s\n",strerror(errno));
        exit(1);
    }

    printf("opening gumstix connector...\n");
    
    // open i2c device gumstix connector
    sprintf(filename,"/dev/i2c-%d",adapter_nr);
    if ((connDev = open(filename,O_RDWR)) < 0) {
        printf("ERRNO: %s\n",strerror(errno));
        exit(1);
    }
    
    // set slave address
    if (ioctl(connDev, 0x0703, connI2cAddr) < 0) {
        printf("ERRNO: %s\n",strerror(errno));
        exit(1);
    }
    #endif
    
    // --- open GPS device ---
    printf("start reading GPS...\n");
    
    char sentence[NMEA_MAX];
    char command[COMM_MAX];

    if ((gpsDev = fopen(GPS, "r")) <= 0) {
        printf("ERR %s: %s\n", GPS, strerror(errno));
        exit(1);
    }
    
    if ((commandFile = fopen(COMMANDS, "r")) <= 0) {
        printf("ERR %s: %s\n", COMMANDS, strerror(errno));
        setLEDs(LED_ERROR, 1);
        exit(1);
    }

    #if REAL
    printf("starting in 10 seconds: ");
    int i;
    for (i=9; i>=0; i--) {
        printf("%d ", i);
        fflush(stdout);
        sleep(1);
    }
    #endif

    int sats = 0;
    setLEDs(LED_END, 0);
    setLEDs(LED_WAITING, 1);
    
    while ((fgets(sentence, NMEA_MAX, gpsDev) != NULL) && (sats < 5)) {
        if (sentence[0] == '$') {
            fprintf(logfile, sentence);

            if (strncmp(sentence, "$GPGGA", 6) == 0) {
                nmea_parse(sentence);
                sats = gps_fix.sats;
                
                readServos();
                readADC();
                setLEDs(LED_AUTO, autoMode);
            }
        }
    }
    setLEDs(LED_WAITING, 0);
    
    printf("Number of Sats >= 5\n");
    fprintf(logfile, "+++ Number of Sats >= 5\n");
    
    // wait until autoMode
    while (!autoMode) {
        readServos();
        setLEDs(LED_AUTO, autoMode);
        usleep(250000);
    }
   
    // main loop - process all commands in file until EOF or stop command
    while (fgets(command, COMM_MAX, commandFile) != NULL) {
        if (command[0] != '\0') {
            printf("command: %s\n", command);
            fprintf(logfile, "+++ command: %s\n", command);
            // split sentence copy on blanks, filling the field array
            int count;
            char *p, *args[2];
            
            p = strchr((char *)command, ' ');
            count = 0;
            while (p != NULL && *p != '\0') {
                *p = '\0';
                args[count++] = ++p;
                p = strchr(p, ' ');
            }

            if (strncmp(command, "stop", 4) == 0) {
                // exit
                break;
            }
            
            if (strcmp(command, "speed") == 0) {
                // set target speed
                target.speedKmpH = atof(args[0]);
            }
            
            if (strcmp(command, "goto") == 0) {
                // set target position
                target.latDgr = atof(args[0]);
                target.lonDgr = atof(args[1]);
                target.set = 1;
            }
            
            if (target.set) {
                gotoTarget();
            }
        }
    }

    printf("stopping...\n");
    fprintf(logfile, "+++ stopping...\n");

    // set speed = 0
    setServo(SV_THROT, OFF_THROT);
    // set steering neutral
    setServo(SV_STEER, OFF_STEER);

    setLEDs(LED_END, 1);

    // close all devices
    //fclose(servoDev);
    //fclose(connDev);
    
    fclose(gpsDev);
    fclose(commandFile);

}