/* Morphle Labs Embedded System Design Interview

Stepper motor drives an 8mm screw, i.e. - every rotation of the stepper moves the object by 8mm. 

Primer on stepper motor trapezoidal profile 
https://www.st.com/resource/en/application_note/an2044-operating-principals-for-practispin-stepper-motor-motion-control-stmicroelectronics.pdf

Interview Tasks
1. Make appropriate hardware connections between components 
in the simulation
  a. Arduino to Stepper Driver
  b. Stepper Driver to Stepper
  c. Stepper driver should be in 16 uStepping, controlled via GPIO pins of Arduino


Test Code
  a. Motor should execute a trapezoidal motion profile.

  b. Consider Deceleration values to be same as acceleration values

  c. Cover edge cases and print on serial, if edge case encountered
    i. Triangle profile 
    ii. Acceleration over 400 mm/s2, in that case use 400 mm/s2. 
    iii. Velocity over 100 mm/s, in that case use 100 mm/s
    iv. Other edge cases you may think are relevant (Bonus Points)

  d. There shoud be distance guarentee i.e. the motor should travel the exact distance
     that was given to it as an input.

  e. The time taken by the motor to execute the trapezoidal profile should be as close as
     possible to the theoreticaly calculated time.


Tips
1. Hover over the component, click the "?" symbol to know more 
about the functionality with examples.

2. Do checkout the examples in depth, you may find solutions 
that can be directly integrated to solve this 



Test Runs for self-evaluation

3. For a distance of 100mm, 400 mm/s2 acceleration, 100 mm/s velocity, the time 
taken to execute the motion will be 1.25s. 

4. If you do 2 or more motions, in opposing directions - 
for example 6mm clockwise + 4mm counter clockwise = 
2mm as the current position

4. Change the uStepping from 16 to 8, and run test no. 3
Time taken to execute motion should remain the same.


*/
#define MD_PIN_EN (0)
#define MD_PIN_MS1 (1)
#define MD_PIN_MS2 (2)
#define MD_PIN_MS3 (3)
#define MD_PIN_SLP (4)
#define MD_PIN_STP (5)
#define MD_PIN_DIR (6)

uint8_t motor_stepping = 16;   /* Motor microstepping to set MS pins */
uint16_t current_displacement_mm = 0;
uint16_t required_displacement_mm = 0;
////////////////////////////////////////////////////////////////////////////

uint32_t isr_tick_count;
uint32_t isr_tick_at_deccelaraion_start;

uint32_t acceleration_mm_tick2 = 100;
uint32_t velocity_mm_sec = 400;
uint32_t diplacement_mm  = 100;


// float    acceleration_mm_sec2 = 100;
uint32_t mm_to_steps = 400; 
uint32_t isr_tick_freq = 2000;
float    current_position_steps;
uint32_t counter_motor_steps = 0;

// void set_direction(bool direction)
// {
//   if(required_displacement_mm > current_displacement_mm)
//     digitalWrite(MD_PIN_DIR, HIGH); /* Setting direction clockwise to move forward */ 
//   else if (required_displacement_mm < current_dispalcement_mm)
//     digitalWrite(MD_PIN_DIR, LOW); /* Setting direction clockwise to move forward */ 
//   else /*required_displacement_mm == current_dispalcement_mm  */
//   {
//     digitalWrite(MD_PIN_STP, LOW); /* */
//     digitalWrite(MD_PIN_EN, HIGH);
//   }

// }

void setup() {
  // put your setup code here, to run once:
  pinMode(MD_PIN_EN, OUTPUT);
  digitalWrite(MD_PIN_EN, LOW);

  /* Set the usteping of 16*/ 
  pinMode(MD_PIN_MS1, OUTPUT);
  digitalWrite(MD_PIN_MS1, HIGH);
  pinMode(MD_PIN_MS2, OUTPUT);
  digitalWrite(MD_PIN_MS2, HIGH);
  pinMode(MD_PIN_MS2, OUTPUT);
  digitalWrite(MD_PIN_MS3, HIGH);

  /* Sleep pin active HIGH */
  pinMode(MD_PIN_SLP, OUTPUT);
  digitalWrite(MD_PIN_SLP, HIGH);

  /* Sets the direction */
  pinMode(MD_PIN_DIR, OUTPUT);
  digitalWrite(MD_PIN_DIR, HIGH);
  // ser_diretion(uint16_t required_displacement) /* 0:counterclockwise, 1: clockwise */

  /* Step input pin, default LOW */
  pinMode(MD_PIN_STP, OUTPUT);
  digitalWrite(MD_PIN_STP, LOW);
  // ser_diretion(uint16_t required_displacement) /* 0:counterclockwise, 1: clockwise */

  pinMode(13, INPUT);
  /* Set Interrput */
  setup_interrupt();
}

inline void setup_interrupt(void)
{
  cli(); // stop interrupts
  TCCR1A = 0; // set entire TCCR1A register to 0
  TCCR1B = 0; // same for TCCR1B
  TCNT1  = 0; // initialize counter value to 0
  // set compare match register for 2000000 Hz increments
  OCR1A = 124; // = (16000000 / 8) /(1 * 20000) - 1 (must be <65536)
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS12, CS11 and CS10 bits for 8 prescaler
  TCCR1B |= (0 << CS12) | (1 << CS11) | (1 << CS10);
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);
  sei(); // allow interrupts
}



ISR(TIMER1_COMPA_vect)
{
  //interrupt commands for TIMER 1 here
  TCNT1=0;
  // current_position_steps = current_position_steps + acceleration_mm_sec2 * isr_tick_count;
  // current_velocity_steps_tick = 
  if(current_position_steps < 5000) 
    current_position_steps = current_position_steps + (float(( 2 * acceleration_mm_tick2 * isr_tick_count * mm_to_steps / isr_tick_freq)) / float (isr_tick_freq));
  else if(current_position_steps < 35000)
  {
    current_position_steps = current_position_steps + (float (velocity_mm_sec * mm_to_steps) / float (isr_tick_freq));
    isr_tick_at_deccelaraion_start = isr_tick_count;
  }
  else if(current_position_steps <= 40000)
    current_position_steps = current_position_steps + (float (velocity_mm_sec * mm_to_steps) / float (isr_tick_freq)) - (float(( 2 * acceleration_mm_tick2 * (isr_tick_count - isr_tick_at_deccelaraion_start)  * mm_to_steps / isr_tick_freq)) / float (isr_tick_freq));

  isr_tick_count++;  
}

inline void set_pulse_motor(void)
{ 
  digitalWrite(MD_PIN_STP, HIGH);
  delayMicroseconds(10);
  digitalWrite(MD_PIN_STP, LOW);
  // delayMicroseconds(1);
  // digitalWrite(13, HIGH);
  counter_motor_steps++;
}

uint32_t last_isr_tick_count;

void loop() {
  // put your main code here, to run repeatedly:

  

  digitalWrite(13, HIGH);
  if((counter_motor_steps <= current_position_steps))
  {
    set_pulse_motor();
   // last_isr_tick_count = isr_tick_count;
  }

}