#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

//integrating LJ code:///////////////////block #1 - start///////////////////////
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include "labjackusb.h"

//MO Defines how long the Feedback command is
#define FEEDBACK_COMMAND_LENGTH 16

//MO Defines how long the Feedback response is
#define FEEDBACK_RESPONSE_LENGTH 12

// Takes a buffer and calculates the checksum8 of it.
BYTE calculateChecksum8(BYTE* buffer);

// Takes a buffer and length, and calculates the checksum16 of the buffer.
int calculateChecksum16(BYTE* buffer, int len);

//******************************/* LabJack Related Helper Functions Protoypes */

//MO Demonstrates how to build the feedback packet.
void buildFeedbackBytes(BYTE * sendBuffer);

// Demonstrates how to check a response for errors.
int checkResponseForErrors(BYTE * recBuffer);

// Demonstrates how to parse the response of Feedback.
double parseFeedbackBytes(BYTE * recBuffer);
//integrating LJ code:///////////////////block #1 - END/////////////////////////


int main(int argc, char** argv)
{
    //integrating LJ code:///////////////////block #2 - START////////////////////
    //Setup the variables we will need.
    int r = 0; // For checking return values
    HANDLE devHandle = 0; //NOTE: a handle is an alias for a void pointer!

    //create two more buffers to hold the feedback send and recieve payload
    BYTE feedbackSendBuffer[FEEDBACK_COMMAND_LENGTH], feedbackRecieveBuffer[FEEDBACK_RESPONSE_LENGTH];

    // Open the U3 - they are calling a method which returns a HANDLE data type.
    devHandle = LJUSB_OpenDevice(1, 0, U3_PRODUCT_ID);

    //if handle does not point to a U3
    if( devHandle == NULL )
    {
        //this works. If no U3 is plugged I get this error.
        printf("Couldn't open U3. Please connect one and try again.\n");
        exit(-1);
    }

    //build the Feedback command
    buildFeedbackBytes(feedbackSendBuffer);
    //integrating LJ code:///////////////////block #2 - END/////////////////////

    ros::init(argc, argv, "jimmy_state_publisher");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);

    //has to be negetive to accuratly reflect my direction of rotation
    const double degreeToRadConst = -(M_PI/180);

    // robot state
    //MO COMMENTED OUT//double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;/////////////////////////////
    //of the two joints that are initialized to 0 (tilt & swivel), only swivel is
    //continious (which is what my LMS shaft is). So I will keep it at zero and
    // also keep the other variables.
    //double tinc = degree, rotating_lms_shaft_to_body_joint=0, angle=0, height=0, hinc=0.005;
    double rotating_lms_shaft_to_body_joint=0, lmsAngle=0;

    //more variables that are my joints
    double body_to_left_drive_wheel_joint=0, body_to_right_drive_wheel_joint=0,
    left_castor_base_to_body_joint=0, left_castor_wheel_to_plate_joint=0,
    right_castor_base_to_body_joint=0, right_castor_wheel_to_plate_joint=0;

    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "odom";
    //mo commented out//odom_trans.child_frame_id = "axis"; ////////////////////////////////////////////////////////
    //I understand that here we need the base link that forms the robot
    odom_trans.child_frame_id = "body_link";

    while (ros::ok()) {
        //update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(7); //resize array
        joint_state.position.resize(7); //resize array
        joint_state.name[0] ="rotating_lms_shaft_to_body_joint"; //name of joint
        joint_state.position[0] = rotating_lms_shaft_to_body_joint; //value
        joint_state.name[1] ="body_to_left_drive_wheel_joint";
        joint_state.position[1] = body_to_left_drive_wheel_joint;
        joint_state.name[2] ="body_to_right_drive_wheel_joint";
        joint_state.position[2] = body_to_right_drive_wheel_joint;
        joint_state.name[3] ="left_castor_base_to_body_joint";
        joint_state.position[3] = left_castor_base_to_body_joint;
        joint_state.name[4] ="left_castor_wheel_to_plate_joint";
        joint_state.position[4] = left_castor_wheel_to_plate_joint;
        joint_state.name[5] ="right_castor_base_to_body_joint";
        joint_state.position[5] = right_castor_base_to_body_joint;
        joint_state.name[6] ="right_castor_wheel_to_plate_joint";
        joint_state.position[6] = right_castor_wheel_to_plate_joint;

        //integrating LJ code:///////////////////block #3 - START///////////////////
        // This is a function in the driver "labjackusb.c".
        // short story: Write the command to the device.
        // Writes to a device with a 1 second timeout.  If the timeout time elapses and
        // no data is transferred the USB request is aborted and the call returns.
        // Returns the number of bytes written, or 0 on error and errno is set.
        // hDevice (arg 1) = The handle for your device. Handle data type
        // pBuff (arg 2) = The buffer to be written to the device. Byte data type
        // count = The number of bytes to write. unsigned long data type.
        // This function replaces the deprecated LJUSB_BulkWrite, which required the
        // endpoint.
        // LJUSB_Write( handle, sendBuffer, length of sendBuffer )
        //NOTE: the reason it is pointng to the return value of the method is because
        //the return indicates if we had a fault or if we have valid data. If you send
        // a BS command it will time out and go to the next statement....
        r = LJUSB_Write( devHandle, feedbackSendBuffer, FEEDBACK_COMMAND_LENGTH );

        //MO check if the system call was successful or not.
        if( r != FEEDBACK_COMMAND_LENGTH )
        {
            printf("#1 An error occurred when trying to write the buffer. The error was: %d\n", errno);
            // *Always* close the device when you error out.
            LJUSB_CloseDevice(devHandle);
            exit(-1);
        }
        //this is a function from the driver labjackusb.c.
        // Short: Read the result from the device.
        // Reads from a device with a 1 second timeout. If the timeout time elapses and
        // no data is transferred the USB request is aborted and the call returns.
        // Returns the number of bytes read, or 0 on error and errno is set.
        // hDevice (arg 1) = The handle for your device. Type Handle
        // pBuff (arg 2) = The buffer to be filled in with bytes from the device. Byte.
        // count (arg 3) = The number of bytes expected to be read. Unsigned long.
        // This function replaces the deprecated LJUSB_BulkRead, which required the
        // endpoint.
        // LJUSB_Read( handle, recBuffer, number of bytes to read)
        r = LJUSB_Read( devHandle, feedbackRecieveBuffer, FEEDBACK_RESPONSE_LENGTH );

        //checks to see if the correct number of bytes was recieved (or a value that
        // represents an error code).
        if( r != FEEDBACK_RESPONSE_LENGTH )
        {
            printf("#2 An error occurred when trying to read from the U3. The error was: %d\n", errno);
            LJUSB_CloseDevice(devHandle);
            exit(-1);
        }

        //Check the command for errors. Seems pretty straight forward... All the
        // check summing is already coded.
        if( checkResponseForErrors(feedbackRecieveBuffer) != 0 )
        {
            LJUSB_CloseDevice(devHandle);
            exit(-1);
        }

        //process the response from the feedback command. For now I am printing it
        // to the screen.
        lmsAngle = parseFeedbackBytes(feedbackRecieveBuffer);

        //convert our degree value to radians
        lmsAngle = lmsAngle * degreeToRadConst;
        //integrating LJ code:///////////////////block #3 - END/////////////////////

        //update transform - odom to be static offset from the origin
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = 1;
        odom_trans.transform.translation.y = 1;
        odom_trans.transform.translation.z = 0;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(0);


        //send the joint state and transform
        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);

        // Create new robot state
        //tilt += tinc;
        //if (tilt<-.5 || tilt>0) tinc *= -1;
        //height += hinc;
        //if (height>.2 || height<0) hinc *= -1;
        //swivel += degree;
        //angle += degree/4;

        //repeating what is done above
        //rotating_lms_shaft_to_body_joint += tinc;
        //if (rotating_lms_shaft_to_body_joint<-.5 || rotating_lms_shaft_to_body_joint>0) tinc *= -1;
        //height += hinc;
        //if (height>.2 || height<0) hinc *= -1;
        //rotating_lms_shaft_to_body_joint += degree;
        //angle += degree/4;

        rotating_lms_shaft_to_body_joint = lmsAngle;

        //publish zeroes to keep everything happy
        body_to_left_drive_wheel_joint=0;
        body_to_right_drive_wheel_joint=0;
        left_castor_base_to_body_joint=0;
        left_castor_wheel_to_plate_joint=0;
        right_castor_base_to_body_joint=0;
        right_castor_wheel_to_plate_joint=0;

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }

    //integrating LJ code:///////////////////block #4 - START///////////////////
    //Close the device.
    LJUSB_CloseDevice(devHandle);
    //integrating LJ code:///////////////////block #4 - END/////////////////////

    return 0;
}

//integrating LJ code:///////////////////block #5 - START///////////////////////
// Uses information from section 5.2.5 of the U3 User's Guide to make a Feedback
// packet.
// http://labjack.com/support/u3/users-guide/5.2.5
void buildFeedbackBytes(BYTE * sendBuffer)
{
    int checksum = 0; //MO initialize the integer

    // Build up the bytes
    //sendBuffer[0] = Checksum8 //done later on in method
    sendBuffer[1] = 0xF8; //must be 0xF8 according to datasheet
    sendBuffer[2] = 0x05; //
    sendBuffer[3] = 0x00; //must be 0x00 according to datasheet
    //sendBuffer[4] = Checksum16 (LSB)
    //sendBuffer[5] = Checksum16 (MSB)

    sendBuffer[6] = 0x00; //echo byte. used make sure results are in order.

    /*This is the IOtype which scans all the inputs.*/
    //Mask (bytes 7 to 9) - this is a write mask. If the bit is an output and set
    // to 1, it will be modified. if mask is set to 0 it will only be read. In
    // my case this is not an input so the mask is kind of useless.
    sendBuffer[7] = 29; //IOType PortDirWrite.
    sendBuffer[8] = 0; //mask of FIO are 00000000 - making them all read
    sendBuffer[9] = 0; //mask of EIO are 00000000 - making them all read
    sendBuffer[10] = 0; //mask of CIO are 0000 - making them all read

    //direction sets the point as an input or output. 1 is an input, 0 is an output.
    sendBuffer[11] = 0; //all FIO are 00000000 - making 0 to 7 inputs (respectivly).
    sendBuffer[12] = 0; //all EIO are 00000000 - making 0 to 7 inputs (respectivly).
    sendBuffer[13] = 0; //all FIO are 0000 - making 0 to 3 inputs (respectivly).

    sendBuffer[14] = 26; //IOType is PortStateRead
    sendBuffer[15] = 0; // padding byte

    // Calculate and set the checksum16
    checksum = calculateChecksum16(sendBuffer, FEEDBACK_COMMAND_LENGTH);
    sendBuffer[4] = (BYTE)( checksum & 0xff );
    sendBuffer[5] = (BYTE)( (checksum / 256) & 0xff );

    // Calculate and set the checksum8
    sendBuffer[0] = calculateChecksum8(sendBuffer);

    // The bytes have been set, and the checksum calculated. We are ready to
    // write to the U3.
}

////////////////////////////////////code from web to convert////////////////////////////////////////////////////

// this method came from (in the comments):
//https://www.daniweb.com/programming/software-development/code/216355/gray-code-conversion
//it does an amazing job of conversion from Gray code to Binary!!!!!
unsigned int gray_to_binary(unsigned int bits)
{

  bits ^= bits >> 16; // remove if word is 16 bits or less
  bits ^= bits >>  8; // remove if word is 8 bits or less
  bits ^= bits >>  4;
  bits ^= bits >>  2;
  bits ^= bits >>  1;
  return bits;
}

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

// Parses the Feedback packet into something useful.
double parseFeedbackBytes(BYTE * recBuffer)
{
    //store the encoder status in variables of type short. In my machine (W530)
    // they are 4 bytes wide (FFFFFFFF)
    //int fio = recBuffer[9]; //FIO - don't care about it so it's commented out.
    short eio = recBuffer[10]; //EIO - care about bits 0 to 5 (inclusive)
    short cio = recBuffer[11]; //CIO - care about all 4 bits (0 to 3).

    /*  //for testing only
    printf("  Results of Feedback:\n");
    printf("  NOTE: 1 == no signal && 0 == signal recieved\n");
    printf("  Result on element 9 (NOT USED - FIO bbbb bbbb) = %02x\n", element9 );
    printf("  Result on element 10 (EIO bbbb bbbb)= %02x\n", element10 );
    printf("  Result on element 11 (CIO bbbb) = %01x\n", element11);
    */

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //CIO3 is 2^9 (MSB), CIO2 is 2^8, CIO1 is 2^7, and CIO0 is 2^6. By multiplying
    //CIO by 0x100 (256 decimal or 2^8) we shift the CIO values 8 bits to the left;
    //this is a prerequisite to concat CIO and EIO
    cio = recBuffer[11];
    //printf("CIO state pre-shift: = %08x\n", cio);///////////TEST ONLY
    cio = cio * 0x100;
    //printf("CIO state post shift: = %08x\n", cio); //////////TEST ONLY

    //now that CIO is represented by the 3rd LSB byte, lets concat CIO and EIO.
    //Basically EIO will now occupy the 1st & 2nd LSB byte.
    eio = recBuffer[10];
    //printf("EIO state: = %08x\n", eio);/////////////////////TEST ONLY
    short cioAndEioConcated = cio + eio;
    //printf("CIO + EIO state: = %08x\n", cioAndEioConcated);//////////TEST ONLY

    //I got everything working with if statements, but now I am opting to do it
    // with bitwise operators. This shifts all the bits over 2 bits
    short cioAndEioRightShifted = cioAndEioConcated >> 2;
    //printf("cio and eio after bitshifting right 2 bits: %08x\n", cioAndEioRightShifted); /////TEST ONLY

    //the final value due to size of short (4 bytes on my machine) looks like
    //this: 00000xxx (where the xxx is the encoder status). I am going to convert
    // all the zeros to F's. This is a prerequisite to make all the lit LEDs on
    //the optocoupler board appear as 1 in the code instead of 0 (current state)
    short cioAndEioPadded = cioAndEioRightShifted + 0xFFFFFC00;
    //printf("Value with leading zeroes to F's: %08x\n", cioAndEioPadded);///////////TEST ONLY

    //now I can use the ~ bitwise operator to flip all the 1's to zeros, and zeros
    // to 1's. this will make everything easier as currely an encoder high is
    //shown as a 0... which is frustrating.
    short cioAndEioFinalValue = ~cioAndEioPadded;
    //printf("final value with 1's representing a high: %08x\n", cioAndEioFinalValue);////////TEST ONLY

    //At this point I need to convert the Gray code to binary. I found the code
    // that does it in the most efficient way. See method above this one, called
    // gray_to_binary(unsigned int). I have also discovered that when we convert
    //Gray code to binary, the binary is proportional to degrees (as it grows
    // from 0 to 1023, the degrees increase proportionally from 0 to 360).
    short binaryFinalValue = gray_to_binary(cioAndEioFinalValue);
    //printf("binary final value: %08x\n", binaryFinalValue); ////TEST ONLY

    //now that we have the binary value, and I verified that the Gray code does
    //convert to binary, and the binary increases proportionally to position.
    double binaryValueAsDouble = binaryFinalValue;
    double degrees = (binaryValueAsDouble * 360) / 1024;

    //final output in degrees:
    //printf("The LMS is pointing at: %04f\n\n\n\n\n\n\n", degrees);

    return degrees;
}


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




//******************************MO - Finish************************************

// Checks the response for any errors.
int checkResponseForErrors(BYTE * recBuffer)
{
    if(recBuffer[0] == 0xB8 && recBuffer[1] == 0xB8)
    {
        // If the packet is [ 0xB8, 0xB8 ], that's a bad checksum.
        printf("The U3 detected a bad checksum. Double check your checksum calculations and try again.\n");
        return -1;
    }
    else if (recBuffer[1] != 0xF8 || recBuffer[2] != 0x03 || recBuffer[3] != 0x00)
    {
        //printf("the results of buffer 1: %d \n", recBuffer[1]); //248
        //printf("the results of buffer 2: %d \n", recBuffer[2]); //3
        //printf("the results of buffer 3: %d \n", recBuffer[3]); //0

        // Make sure the command bytes match what we expect.
        printf("Got the wrong command bytes back from the U3.\n");
        return -1;
    }

    // Calculate the checksums.
    int checksum16 = calculateChecksum16(recBuffer, FEEDBACK_RESPONSE_LENGTH);
    BYTE checksum8 = calculateChecksum8(recBuffer);

    if ( checksum8 != recBuffer[0] || recBuffer[4] != (BYTE)( checksum16 & 0xff ) || recBuffer[5] != (BYTE)( (checksum16 / 256) & 0xff ) ) {
        // Check the checksum
        printf("Response had invalid checksum.\n%d != %d, %d != %d, %d != %d\n", checksum8, recBuffer[0], (BYTE)( checksum16 & 0xff ), recBuffer[4], (BYTE)( (checksum16 / 256) & 0xff ), recBuffer[5] );
        return -1;
    }
    else if ( recBuffer[6] != 0 ) {
        // Check the error code in the packet. See section 5.3 of the U3
        // User's Guide for errorcode descriptions.
        printf("Command returned with an errorcode = %d\n", recBuffer[6]);
        return -1;
    }

    //all must be ok with the response (checksum passed)
    return 0;

}

/* ---------------- Buffer Helper Functions Definitions ---------------- */
// Calculates the checksum8
BYTE calculateChecksum8(BYTE* buffer){
    int i; // For loops
    int temp; // For holding a value while we working.
    int checksum = 0;

    for( i = 1; i < 6; i++){
        checksum += buffer[i];
    }

    temp = checksum/256;
    checksum = ( checksum - 256 * temp ) + temp;
    temp = checksum/256;

    return (BYTE)( ( checksum - 256 * temp ) + temp );
}

// Calculates the checksum16
int calculateChecksum16(BYTE* buffer, int len){
    int i;
    int checksum = 0;

    for( i = 6; i < len; i++){
        checksum += buffer[i];
    }

    return checksum;
}
//integrating LJ code:///////////////////block #5 - END/////////////////////