// uncomment the following line to build ROS version
//#define ROS_MODE
#include "Wire.h"
#include <MPU6050_light.h>

#ifdef ROS_MODE

#include <ros.h>
#include <std_msgs/Int64MultiArray.h>
#include <std_msgs/Bool.h>
#define MSG_DATA msg.data
#else
#define MSG_DATA msg
#endif



// define this macro as 1 or 2 depending on the required build
// this variation from 1 to 2 can be by touching this source code, or the compiler command line could include a definition, 
// in which case the macro will already be defined and thus the #ifndef will skip this local definition
#ifndef MOTOR_ID
#define MOTOR_ID 1
#endif


// I tried to use stringify here, but as usual with C macros, I gave up before getting it to work!!!
// #define READY_MESSAGE "ready" #MOTOR_ID
#if MOTOR_ID == 1
#define LISTEN_MESSAGE "pattern1"
#define READY_MESSAGE "ready1"
#else
#define LISTEN_MESSAGE "pattern2"
#define READY_MESSAGE "ready2"
#endif
// better, uses less ram
#if MOTOR_ID == 1
#define LISTEN_MESSAGE "p1"
#define READY_MESSAGE "r1"
#else
#define LISTEN_MESSAGE "p2"
#define READY_MESSAGE "r2"
#endif


const byte dirPinA = 4;
const byte stepPinA = 5;
const byte dirPinB = 6;
const byte stepPinB = 7;

#define CMD_ONCE 0
#define CMD_CYCLE 1
#define MAX_MOTION_ARRAY 32

enum waveformStage { positiveFalling, negativeFalling, negativeRising, positiveRising, pausing };

int motor_state;
int rotation_direction;

// TEST message hard coded here
// the first number will be the movement type, CMD_ONCE (0) or CMD_CYCLE (1) 
// the second integer is quantity of position pairs which will be followed by N pairs of values
// each pair comprises the number of steps to perform followed by the time intervale to pause after each step
// a full command, and the size of the declared array, will therefore be 2+2N integers

int msg[8] = { 0, 3, 30, 1000, 60, 3000, 100, 5000};
int motionArrayLimit[MAX_MOTION_ARRAY];
int motionArrayTimes[MAX_MOTION_ARRAY];
int motionArrayLen = 0;
int motionCmd = CMD_ONCE;
struct profileProgress {
    bool channelA;
    int currentStepPosition;
    waveformStage currentWaveformStage;
    int currentProfileCount;
    int currentProfileIndex;
    int profileIndexDelta ;
    int negateSteps;
    int stepDirection ;
    bool moving;
    bool newConfiguration ;
    long configTime;
    bool steppedFirstHalf;
};

struct profileProgress profileStateA, profileStateB;

bool changedSpeed = false;

#define CMD_OFFSET 0
#define COUNT_OFFSET 1
#define STEPS_OFFSET 2

int periodA = 5000;
int periodB = 15345;
int remainingA;
int remainingB;

MPU6050 mpu(Wire);

void initProgress(struct profileProgress *str) {
    str->currentStepPosition = 0;
    str->currentWaveformStage = waveformStage::positiveRising;
    str->currentProfileCount = 0;
    str->currentProfileIndex = 0;
    str->profileIndexDelta = 1;
    str->negateSteps = 1;
    str->stepDirection = 1;
    str->moving = false;
    str->newConfiguration = false;
    str->configTime = 0;
    str->steppedFirstHalf = false;
}

ISR(TIMER1_COMPA_vect) {
  if (profileStateA.moving) {
    TCNT1 = 0;                  //First, set the timer back to 0 so it resets for next interrupt
    // counter reach current A value
    remainingB -= remainingA;
    remainingA = profileStateA.configTime;
    OCR1A = remainingA;
    OCR1B = remainingB;
    digitalWrite(dirPinA, (profileStateA.stepDirection == 1));
    if (!profileStateA.steppedFirstHalf) {
        // after the first half-period delay, raise the signal
        digitalWrite(stepPinA, HIGH);
        profileStateA.steppedFirstHalf = true;
    }
    else {
        // after the second half-period delay, lower the signal
        digitalWrite(stepPinA, LOW);
        profileStateA.steppedFirstHalf = false;
        doneOneStep(true, &profileStateA);
    }
  }
}
ISR(TIMER1_COMPB_vect) {
  if (profileStateB.moving) {
    TCNT1 = 0;                  //First, set the timer back to 0 so it resets for next interrupt
        // counter reach current A value
    remainingA -= remainingB;
    remainingB = profileStateB.configTime;
    OCR1A = remainingA;
    OCR1B = remainingB;
    digitalWrite(dirPinB, (profileStateB.stepDirection == 1));
    if (!profileStateB.steppedFirstHalf) {
        // after the first half-period delay, raise the signal
        digitalWrite(stepPinB, HIGH);
        profileStateB.steppedFirstHalf = true;
    }
    else {
        // after the second half-period delay, lower the signal
        digitalWrite(stepPinB, LOW);
        profileStateB.steppedFirstHalf = false;
        doneOneStep(true, &profileStateB);
    }
  }
}



void configureStep() {
    cli();                      //stop interrupts for till we make the 
    TCCR1A = 0;			//Reset entire TCCR1A register
    TCCR1B = 0;			//Reset entire TCCR1B register
    TCCR1B |= B00000010;		//Set CS11 to 1 so we get Prescalar = 64 .. providing CTC input at 4us
    TCNT1 = 0;			//Reset Timer 1 value to 0
    TIMSK1 |= B00000110;        //Timer interupt mask1 ... bit 2 enable int B, bit 1 enable int A..Set OCIE1A to 1 so we enable compare match A 
    remainingA = periodA;
    remainingB = periodB;
    OCR1A = periodA;
    OCR1B = periodB;
    sei();                   // Enable global interrupt
}


void configureStep(struct profileProgress *currentState) {
    int dirPin = (currentState->channelA)? dirPinA : dirPinB;
    if (currentState->stepDirection > 0) {
//        digitalWrite(dirPin, HIGH);
    }
    else {
//        digitalWrite(dirPin, LOW);
    }
     cli();                      //stop interrupts for till we make the 
    TCCR1A = 0;			//Reset entire TCCR1A register
    TCCR1B = 0;			//Reset entire TCCR1B register
    TCCR1B |= B00000011;		//Set CS11 & CS10 to 1 so we get Prescalar = 64 .. providing CTC input at 4us
    TCNT1 = 0;			//Reset Timer 1 value to 0
    TIMSK1 |= B00000110;        //Set OCIE1A and B to 1 so we enable compare match A 
    if (currentState->channelA) {
        OCR1A = remainingA = currentState->configTime;
    } else {
        OCR1B = remainingB = currentState->configTime;
    }
    sei();                   // Enable global interrupt
}

void doneOneStep(bool actuallyStepped, struct profileProgress *currentState) {
    bool lclNewConfiguration = false;
    if (actuallyStepped) { // as opposed to being called to just initiate the stepping process
        currentState->currentStepPosition += currentState->stepDirection;  // increment or decrement to be able to compare with the stored limits
                                // as these are expressed as positive values only for the rising section of the curve
    } else {
        lclNewConfiguration = true; // actuallyStepped == true means we are called from the setup code, so need to kick off the cycle
    }
    currentState->currentProfileIndex = (currentState->profileIndexDelta > 0) ? currentState->currentProfileCount : (motionArrayLen - 1) - currentState->currentProfileCount;
    int stepLimit = (currentState->profileIndexDelta > 0) ? motionArrayLimit[currentState->currentProfileIndex] : (currentState->currentProfileIndex == 0)? 0 : motionArrayLimit[currentState->currentProfileIndex -1];
    int stepsRemaining = (stepLimit - (currentState->currentStepPosition * currentState->negateSteps) ) * currentState->profileIndexDelta;
    if (stepsRemaining <= 0) {
        // reached the limit for this section of the curve, so time to switch to the next
        lclNewConfiguration = true;
        currentState->currentProfileCount++;
//        digitalWrite(changePin, changedSpeed);
//        changedSpeed = !changedSpeed;

        if (currentState->currentProfileCount >= motionArrayLen) {
            // got to the end of the motion profile
            currentState->currentProfileCount = 0; // so, will start again

            switch (currentState->currentWaveformStage) {
            case waveformStage::positiveFalling:
                currentState->currentWaveformStage = waveformStage::negativeFalling; // next section, now set the increments accordingly
                currentState->profileIndexDelta = 1; // then proceed through the profile in normal direction
                currentState->stepDirection = -1; // but each movement is further negative
                currentState->negateSteps = -1;
                break;
            case waveformStage::negativeFalling:
                currentState->currentWaveformStage = waveformStage::negativeRising; // next section, now set the increments accordingly
                currentState->profileIndexDelta = -1; // then proceed through the profile in inverse direction
                currentState->stepDirection = 1; // but each movement is less negative, ie positive
                currentState->negateSteps = -1;
                break;
            case waveformStage::negativeRising:
                currentState->currentWaveformStage = waveformStage::positiveRising; // next section, now set the increments accordingly
                currentState->profileIndexDelta = 1; // then proceed through the profile in normal direction
                currentState->stepDirection = 1; // and each movement is positive
                currentState->negateSteps = 1;
                break;
            case waveformStage::positiveRising:
                currentState->currentWaveformStage = waveformStage::pausing; // next section, now set the increments accordingly
                currentState->moving = false;
                ackMovementCompletedNotStarted(true);
                break;
            }
        }
    }
    if (lclNewConfiguration) {
      currentState->currentProfileIndex = (currentState->profileIndexDelta > 0) ? currentState->currentProfileCount : (motionArrayLen - 1) - currentState->currentProfileCount;
      currentState->configTime = motionArrayTimes[currentState->currentProfileIndex];  //mut be pre-divide by 2, divide by 4...convert us to half cycle of 4us per clock
      currentState->newConfiguration = true;
      configureStep(currentState);
    }
}

void doRising(struct profileProgress *currentState) {
    currentState->currentWaveformStage = waveformStage::positiveRising;
    currentState->profileIndexDelta = 1; // then proceed through the profile in normal direction
    currentState->stepDirection = 1; // and each movement is positive
    currentState->negateSteps = 1;
    currentState->currentProfileCount = 0;
    doneOneStep(false, currentState); // start the stepping process, false as we havent actualy stepped
    currentState->moving = true;
   
}
void startCycle(struct profileProgress *currentState) {
    currentState->currentWaveformStage = waveformStage::positiveFalling;
    currentState->profileIndexDelta = -1; // then proceed through the profile in normal direction
    currentState->stepDirection = -1; // and each movement is positive
    currentState->negateSteps = 1;
    currentState->currentProfileCount = 0;
    doneOneStep(false, currentState); // start the stepping process, false as we havent actualy stepped
    currentState->moving = true;
   
}

bool rx_desired_motor_pattern_cb(const int *msg)
{
    bool retv = false;
    // the message to be used here will arrive as an array of 32 bit integers
    // the first number will be the movement type, CMD_ONCE or CMD_CYCLE 
    // the second integer is quantity of position pairs which will be followed by N pairs of values
    // each pair comprises the number of steps to perform followed by the time intervale to pause after each step
    // a full command will therefore be 2+2N integers
    // the maximum array size - governed by the storage declaration- is MAX_MOTION_ARRAY , 32
    //
    // the motion command is assumed to 
    if ((MSG_DATA[CMD_OFFSET] == CMD_ONCE || MSG_DATA[CMD_OFFSET] == CMD_CYCLE) && MSG_DATA[COUNT_OFFSET] <= MAX_MOTION_ARRAY) {
        ackMovementCompletedNotStarted(false);
        motionArrayLen = MSG_DATA[COUNT_OFFSET];
        int accessIdx = STEPS_OFFSET;
        for (int idx = 0; idx < motionArrayLen; idx++) {
            motionArrayLimit[idx] = MSG_DATA[accessIdx++];
            motionArrayTimes[idx] = MSG_DATA[accessIdx++];
        }
        retv = true;
    }
    else {
        // ignore command, or perhaps immediately say 'done'
        retv = false;
    }
}


// debug code
#ifdef ROS_MODE
//
#else
bool commandCompleted = false;
#endif
// sends an acknowledgement message, false for starting, true to say compleed
void ackMovementCompletedNotStarted(bool completed) {
#ifdef ROS_MODE
    ready_msg.data = completed;
    ready_pub.publish(&ready_msg); // to indicate that motor is moving
#else
    commandCompleted = completed;
#endif
}


#ifdef ROS_MODE
ros::Subscriber<std_msgs::Int64MultiArray> rx_desired_motor_pattern_sub(LISTEN_MESSAGE, rx_desired_motor_pattern_cb); //desired_motor_state0 for motor0 and desired_motor_state1 for motor1 
#else
//
#endif


void setup()
{
  Serial.begin(115200); // Any baud rate should work
    motor_state = 0;
    rotation_direction = 1;
    pinMode(dirPinA, OUTPUT);
    pinMode(stepPinA, OUTPUT);
    pinMode(dirPinB, OUTPUT);
    pinMode(stepPinB, OUTPUT);

    Wire.begin();
  mpu.begin();
  mpu.calcGyroOffsets();
  
#ifdef ROS_MODE
    nh.initNode();
    nh.subscribe(rx_desired_motor_pattern_sub);
    nh.advertise(ready_pub);
#else
    initProgress(&profileStateA);
    profileStateA.channelA = true;
    initProgress(&profileStateB);
    profileStateB.channelA = false;
    rx_desired_motor_pattern_cb(&msg[0]);
    doRising(&profileStateA);
    doRising(&profileStateB);
#endif
    
}

void showInt(int val) {
  Serial.print(val);Serial.print(" ");
}
bool cycleStarted = false;
void loop()
{
#ifdef ROS_MODE
    nh.spinOnce();
#else
//    if (commandCompleted) {
//        MSG_DATA[CMD_OFFSET] = CMD_CYCLE;
//        rx_desired_motor_pattern_cb(&MSG_DATA[0]);
//    }
  if (!cycleStarted) {
    if(!profileStateA.moving) {
      startCycle(&profileStateA);
      cycleStarted = true;
    }
  } else {
    if (!profileStateA.moving) {
      if (profileStateB.currentWaveformStage == waveformStage::negativeFalling) {
        startCycle(&profileStateA);
      }
    }
    if (!profileStateB.moving) {
      if (profileStateA.currentWaveformStage == waveformStage::positiveRising) {
        startCycle(&profileStateB);
      }
    }
  }

  mpu.update();
  showInt(mpu.getAngleX() * 100);
  showInt(mpu.getAngleY() * 100);
  showInt(mpu.getAngleZ() * 100);
  showInt(mpu.getAccX() * 100);
  showInt(mpu.getAccY() * 100);
  Serial.println(mpu.getAccZ() * 100);
  delay(100);

#endif
}