/***************************************************************
 * DriveTest.cpp
 *
 * Implements remote-controlled robotic platform based on
 * DFRobot Cherokey 4WD and running on Raspbeery Pi 3
 *
 * Hardware PWM for motor speed and GPIOs are implemented using WiringPi library:
 * http://wiringpi.com/
 *
 * Communication with Slave Arduino is done using I2C bus, based on example from Peter Mount's blog:
 * https://blog.retep.org/2014/02/15/connecting-an-arduino-to-a-raspberry-pi-using-i2c/
 *
 * This program should be compiled using command:
 *   g++ drivetest.cpp -o drivetest -l wiringPi
 *
 * 
 * rev. 1.0   - 2016.06.06
 *    - initial version
 *
 * boredman@BoredomProjects.net
 *
 ***************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <termios.h>
#include <unistd.h>
#include <stdint.h>
#include <iostream>
#include <sys/select.h>

#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <fcntl.h>

#include <wiringPi.h>

//#define clrscr() printf("\e[1;1H\e[2J") // Clear screen.

// assign pins using Broadcom pin numbering scheme:
const int pin_motor_pwm_right = 13;  // BCM 13  WPi 23
const int pin_motor_dir_right =  5;  // BCM 5   WPi 21
const int pin_motor_pwm_left  = 12;  // BCM 12  WPi 26
const int pin_motor_dir_left  =  6;  // BCM 6   WPi 22


const int speed_step = 30;
const int turn_step = 30;
const int speed_top = 700;
const int turn_top = 1000;
const int pwm_min = 300;


// Slave Arduino i2c address
#define I2C_ADDRESS 0x42

// The I2C bus: This is for V2 and V3 pi's. For V1 Model B you need i2c-0
static const char *i2cDevName = "/dev/i2c-1";


class cRobot
{
    public:
    enum eI2Command {
        CMD_SETLED = 1,
        CMD_STATUS = 2,
        CMD_SETSHD = 3
    };
    enum eChargerState {
        CHGR_OFF = 10,
        CHGR_INIT = 11,
        CHGR_CHARGE = 12,
        CHGR_TRICKLE = 13,
    };
    struct {
        uint16_t battery_miV;
        int16_t  battery_miA;
        uint16_t vsupply_miV;
        uint16_t charger_state;   // min allocation size is 2 bytes!
    } data;
} robot;


/***************************************************************
 * Change mode of STDIN behaviour to no buffering and no echo.
 *
 * http://cboard.cprogramming.com/linux-programming/51531-faq-cached-input-mygetch.html?highlight=kbhit
 ***************************************************************/
void changemode(int dir)
{
    static struct termios oldt, newt;

    if ( dir == 1 )
    {
        tcgetattr( STDIN_FILENO, &oldt);
        newt = oldt;
        newt.c_lflag &= ~( ICANON | ECHO );
        tcsetattr( STDIN_FILENO, TCSANOW, &newt);
    }
    else
        tcsetattr( STDIN_FILENO, TCSANOW, &oldt);
}


/***************************************************************
 * Detect keyboard hits and report immediately (without Enter key)
 * requires prior call to chagemode(1);
 *
 * http://forums.fedoraforum.org/showthread.php?t=172337
 ***************************************************************/
int kbhit(void)
{
    struct timeval tv;
    fd_set read_fd;

    /* Do not wait at all, not even a microsecond */
    tv.tv_sec=0;
    tv.tv_usec=0;

    /* Must be done first to initialize read_fd */
    FD_ZERO(&read_fd);

    /* Makes select() ask if input is ready:
     * STDIN_FILENO (=0) is the file descriptor for stdin    */
    FD_SET(STDIN_FILENO, &read_fd);

    /* The first parameter is the number of the
     * largest file descriptor to check + 1. */
    if(select(STDIN_FILENO+1, &read_fd, NULL, /*No writes*/NULL, /*No exceptions*/&tv) == -1)
        return 0;  /* An error occured */

    /* read_fd now holds a bit map of files that are
     * readable. We test the entry for the STDIN_FILENO (= 0). */
    if(FD_ISSET(STDIN_FILENO, &read_fd))
        /* Character pending on stdin */
        return 1;

    /* no characters were pending */
    return 0;
}


/***************************************************************
 * MAIN function
 *
 *   no parameters
 ***************************************************************/
int main(int argc, char* argv[])
{

    wiringPiSetupGpio();

    pinMode(pin_motor_dir_right, OUTPUT);
    digitalWrite(pin_motor_dir_right, LOW);

    pinMode(pin_motor_dir_left, OUTPUT);
    digitalWrite(pin_motor_dir_left, LOW);

    pwmSetMode(PWM_MODE_MS);
    pwmSetClock(32);

    pinMode(pin_motor_pwm_right, PWM_OUTPUT);
    pwmWrite(pin_motor_pwm_right, 0);

    pinMode(pin_motor_pwm_left, PWM_OUTPUT);
    pwmWrite(pin_motor_pwm_left, 0);

    // I2C: Connecting
    int i2cfd;
    if ((i2cfd = open(i2cDevName, O_RDWR)) < 0) {
        fprintf(stderr, "\nI2C: Failed to access %d\n", i2cDevName);
        exit(2);
    }
    // I2C: acquiring bus to device
    if (ioctl(i2cfd, I2C_SLAVE, I2C_ADDRESS) < 0) {
        fprintf(stderr, "\nI2C: Failed to acquire bus access/talk to slave 0x%x\n", I2C_ADDRESS);
        exit(3);
    }


    int speed = 0;
    int turn = 0;

    int pwm_right, pwm_left;
    int dir_right, dir_left;

    unsigned int last_time;

    // turn off line buffering and echoing for STDIN
    changemode(1);

    while(1)
    {
        // every 0.5sec, when no keystrokes detected, print battery status
        if( ! kbhit() )
        {
            if ( (millis() - last_time) > 500 )
            {
                last_time = millis();

                uint8_t cmd = cRobot::CMD_STATUS;
                if( write(i2cfd, &cmd, 1) != 1 )
                {
                    printf("\nI2C: Error during writing command");
                    changemode(0);  // restore normal behaviour for STDIN
                    exit(4);
                }
                usleep(1000);

                if (read(i2cfd, (char*)&(robot.data), sizeof(robot.data)) != sizeof(robot.data))
                {
                    printf("\nI2C: Error during reading data");
                    changemode(0);  // restore normal behaviour for STDIN
                    exit(5);
                }

                printf("\e[s"); // save current cursor position
                printf("   { Vbat=%0.2f Ibat=%0.2f Vs=%0.2f st:%s }  ", 
                        robot.data.battery_miV/1024.0,
                        robot.data.battery_miA/1024.0,
                        robot.data.vsupply_miV/1024.0,
                        robot.data.charger_state==cRobot::CHGR_OFF ? "DISCHARGE" :  \
                        robot.data.charger_state==cRobot::CHGR_INIT ? "INIT" :      \
                        robot.data.charger_state==cRobot::CHGR_CHARGE ? "CHARGE" :  \
                        robot.data.charger_state==cRobot::CHGR_TRICKLE ? "TRICKLE" : "???" );
                printf("\e[u"); // restore old cursor position
                fflush(stdout);
            }
            continue;
        }

        // process keyboard hits
        uint8_t c = getchar();

        if( c == 'q' || c == 'x' )
        {
            changemode(0);  // restore normal behaviour for STDIN
            printf("\n");
            exit(0);
        }
        else if( c == ' ' )	// SPACE
        {
            speed = 0;
            turn = 0;
        }
        else if( c == 27 )	// esc sequence
        {
            c = getchar();
            c = getchar();
            if( c == 65 )    	// UP
            {
                speed += speed_step;
                if( speed > speed_top )
                    speed = speed_top;
            }
            else if( c == 66 )	// DOWN
            {
                speed -= speed_step;
                if( speed < -speed_top )
                    speed = -speed_top;
            }
            else if( c == 67 )	// RIGHT
            {
                turn -= turn_step;
                if( turn < -turn_top )
                    turn = -turn_top;
            }
            else if( c == 68 )	// LEFT
            {
                turn += turn_step;
                if( turn > turn_top )
                    turn = turn_top;
            }
        }
        
        // translate from {speed & turn} to {pwm_left & pwm_right}
        pwm_right  = speed + turn;
        if( pwm_right < 0 )
        {
            pwm_right = -pwm_right + pwm_min;
            dir_right = HIGH;
        }
        else if( pwm_right > 0 )
        {
            pwm_right = pwm_right + pwm_min;
            dir_right = LOW;
        }

        pwm_left  = speed - turn;
        if( pwm_left < 0 )
        {
            pwm_left = -pwm_left + pwm_min;
            dir_left = HIGH;
        }
        else if( pwm_left > 0 )
        {
            pwm_left = pwm_left + pwm_min;
            dir_left = LOW;
        }

        // send data to motors
        pwmWrite(pin_motor_pwm_right, pwm_right);
        pwmWrite(pin_motor_pwm_left, pwm_left);
        digitalWrite(pin_motor_dir_right, dir_right);
        digitalWrite(pin_motor_dir_left, dir_left);

        // print status
        printf("\n[ S=%-4d T=%-4d | PL=%-4d PR=%-4d | DL: %c  DR: %c ]", 
                speed, turn, pwm_left, pwm_right, dir_left?'v':'^', dir_right?'v':'^');
        fflush(stdout);
    }

}