/* EXTREMELY CRUDE ULTRABORG SERVO EXAMPLE */

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

//// Constants /////
#define BUFFER_MAX              (32)
#define I2C_ERROR_OK            (0)
#define I2C_ERROR_FAILED        (1)
#define I2C_ERROR_PARTIAL       (2)
#define HANDLE_UNINITALISED     (-1)
#define UB_COMMAND_SET_PWM1     (5)     // Set the PWM duty cycle for drive #1 (16 bit)
#define UB_DEFAULT_PWM_MAX      (4000)  // Servo maximum limit when set to default 
#define UB_DEFAULT_PWM_MIN      (2000)  // Servo minimum limit when set to default
#define UB_DEFAULT_I2C_ADDRESS  (0x36)  // I2C address set by default (before using SET_I2C_ADD)

#ifndef FALSE
#define FALSE                   (0)
#endif
#ifndef TRUE
#define TRUE                    (!FALSE)
#endif

///// Storage /////
uint8_t bufIn[BUFFER_MAX];
uint8_t bufOut[BUFFER_MAX];
char pathI2C[20];
int hI2C = HANDLE_UNINITALISED;

///// Macros /////
#define DIAG_PRINT(...)         do { if (diagnosticPrint) { printf(__VA_ARGS__); } } while (FALSE)

///// User values /////
int busNumber = 1;
int addressUltraBorg = UB_DEFAULT_I2C_ADDRESS;
int diagnosticPrint = TRUE;
int UbServo1Min = UB_DEFAULT_PWM_MIN;
int UbServo1Max = UB_DEFAULT_PWM_MAX;

///// I2C handling functions /////
int SendI2C(int hI2C, int bytes, uint8_t *pData) {
    int rc = write(hI2C, pData, bytes);
    if (rc == bytes) {
        return I2C_ERROR_OK;
    } else if (rc < 0) {
        DIAG_PRINT("I2C ERROR: Failed to send %d bytes!\n", bytes);
        return I2C_ERROR_FAILED;
    } else {
        DIAG_PRINT("I2C ERROR: Only sent %d of %d bytes!\n", rc, bytes);
        return I2C_ERROR_PARTIAL;
    }
}

int RecvI2C(int hI2C, int bytes, uint8_t *pData) {
    int rc = read(hI2C, pData, bytes);
    if (rc == bytes) {
        return I2C_ERROR_OK;
    } else if (rc < 0) {
        DIAG_PRINT("I2C ERROR: Failed to read %d bytes!\n", bytes);
        return I2C_ERROR_FAILED;
    } else {
        DIAG_PRINT("I2C ERROR: Only read %d of %d bytes!\n", rc, bytes);
        return I2C_ERROR_PARTIAL;
    }
}

///// UltraBorg functions /////

// Setup the I2C for talking to UltraBorg
int UbInitialise(void) {
    int i;

    DIAG_PRINT("Loading UltraBorg on bus %d\n", busNumber);

    // Clean buffers
    for (i = 0; i < BUFFER_MAX; ++i) {
        bufIn[i] = 0xCC;
        bufOut[i] = 0xCC;
    }

    // Open I2C device
    snprintf(pathI2C, sizeof(pathI2C)-1, "/dev/i2c-%d", busNumber);
    hI2C = open(pathI2C, O_RDWR);
    if (hI2C < 0) {
        DIAG_PRINT("I2C ERROR: Failed to open bus %d, are we root?\n", busNumber);
        return I2C_ERROR_FAILED;
    }

    if (ioctl(hI2C, I2C_SLAVE, addressUltraBorg) < 0) {
        DIAG_PRINT("I2C ERROR: Failed to set target address to %d!\n", targetAddress);
        return I2C_ERROR_FAILED;
    }
    return I2C_ERROR_OK;
}

// Sets the drive position for servo output #1
// 0 is central, -1 is maximum left, +1 is maximum right
void UbSetServoPosition1(float position) {
    float powerOut = (position + 1.0) / 2.0;
    unsigned int pwmDuty = (unsigned int)((powerOut * (UbServo1Max - UbServo1Min)) + UbServo1Min);

    bufOut[0] = UB_COMMAND_SET_PWM1;
    bufOut[1] = (uint8_t)((pwmDuty >> 8) & 0xFF);
    bufOut[2] = (uint8_t)(pwmDuty & 0xFF);
    if (SendI2C(hI2C, 3, bufOut) != I2C_ERROR_OK) {
        DIAG_PRINT("I2C ERROR: Failed setting servo #1 position");
    }
}

///// User example /////
int main(void) {
    // Setup I2C bus ready
    if (UbInitialise() != I2C_ERROR_OK) {
        return I2C_ERROR_FAILED;
    }

    // Set minimum servo position
    UbSetServoPosition1(-1.0f);
    
    // Set central servo position
    UbSetServoPosition1(0.0f);

    // Set maximum servo position
    UbSetServoPosition1(1.0f);

    return I2C_ERROR_OK;
}