#include <msp430.h>
#include "SPI.h"
#include "motorDriver.h"
//#include "motionController.h"

/* pinout
 *
 * motor driver:
 * CLK  <-> P1.4
 * MOSI <-> P1.1
 * MISO <-> P1.2
 * CS   <-> P1.3
 * 
 * motoren:
 * stepR <-> P1.6
 * dirR  <-> P2.5
 * stepL <-> P1.7
 * dirL  <-> P2.6
 * 
 * Lijndetectie:
 * front right <-> P2.0
 * front left  <-> P2.1
 * back right  <-> P2.2
 * back left   <-> P2.3
 * 
 * */

/*
 * 0: motor test
 * 1: NFC test
 * 2: motioncontroller test
 */
#define TEST 0

#define STEPr_PIN  BIT4
#define STEPl_PIN  BIT5 // PORT 2
#define DIRr_PIN   BIT6 // PORT 1
#define DIRl_PIN   BIT7 // PORT 1
enum Direction {Forward=0, Backward=(DIRl_PIN | DIRr_PIN), rotateRight=DIRr_PIN, rotateLeft=DIRl_PIN};
enum Speed {Fast=35, Normal=70, Slow=140, Stop=0}; // *10 us per step

const ulong halfRound = 2800;

/* delay(): wait the given time
 *
 * @pram tus (ulong): time to wait in ten microsecons (tus * 10us)
 */
void delay(ulong tus){
  while(tus != 0){
    tus--;
    __delay_cycles(160);
  }
}

//TODO: add more steps in accareration
/* move(): rotate the motors n steps on the fastest motor
 *
 * @pram n (uint): number of steps for the fastest motor
 * @pram SR (enum Speed): speed for right motor
 * @pram SL (enum Speed): speed for left motor
 */
void move(uint n, enum Speed SR, enum Speed SL){
  enum Speed speed[2];
  uchar start, fastest, slowest, fastPin, i;
  uint dev;

  speed[0] = SR;
  speed[1] = SL;

  // end of acceration (start on the fiven speed)
  start = n - 100;

  // get fastest speed
  if(speed[0] > speed[1]){
    fastest = 1;
    slowest = 0;
    fastPin = STEPl_PIN;
  }else{
    fastest = 0;
    slowest = 1;
    fastPin = STEPr_PIN;
  }

  if(speed[slowest] == Stop){
    dev = 65535; // this is not quite stop but close enough
  }else{
    dev = speed[fastest]/speed[slowest];
  }

  if(speed[fastest] == Stop){
    return; // fastest speed is stop, so dont need to run the motors
  }

  // acceleration
  while(n > start && n != 0){
    n--;

    if(speed[slowest] == Stop){
      P2OUT |= fastPin;
      P2OUT &= ~fastPin;
    }else{
      P2OUT |= (STEPl_PIN | STEPr_PIN);
      P2OUT &= ~(STEPl_PIN | STEPr_PIN);
    }

    delay(Slow);
  }

  // run on given speed
  i = dev;
  while(n != 0){
    n--;
    i--;

    if(i == 0){
      P2OUT |= (STEPl_PIN | STEPr_PIN);
      P2OUT &= ~(STEPl_PIN | STEPr_PIN);
      i = dev;
    }else{
      P2OUT |= fastPin;
      P2OUT &= ~fastPin;
    }

    if(n == 100){ // start deaccaleration
      speed[fastest] = Slow;
      if(speed[slowest] != Stop){
        dev = 1;
      }
    }
    delay(speed[fastest]);
  }
  __delay_cycles(160000);
}

/* dir(): set the directon to rotatate the motors
 *
 * @pram d (enum Direction): [Forward | Backward | rotateRight | rotateLeft]
 */
void dir(enum Direction d){
  P1OUT &= ~(DIRr_PIN | DIRl_PIN); // set dir bits to 0
  P1OUT |= d;
}


/**
 * main.c
 */

int main(void)
{
    int i, ii;
    WDTCTL = WDTPW | WDTHOLD;   // stop watchdog timer

    // set cpu op 16MHz
    BCSCTL1 = CALBC1_16MHZ;
    DCOCTL  = CALDCO_16MHZ;

    P2DIR |= STEPr_PIN | STEPl_PIN;
    P1DIR |= DIRr_PIN | DIRl_PIN;

    SPIInit();
    MDInit();
    //MCInit();
    //NFCInit();

    __delay_cycles(16000000);

    for(ii=2; ii>0; ii--){
      // forward
      dir(Forward);
      move(10000, Fast, Fast);
      dir(rotateRight);
      move(halfRound, Normal, Normal);
      // return
      dir(Forward);
      move(10000, Fast, Fast);
      dir(rotateLeft);
      move(halfRound, Normal, Normal);
    }
    for(ii=2; ii>0; ii--){
      for(i=5; i>0; i--){
        dir(Forward);
        move(1600, Fast, Fast);
        dir(rotateRight);
        move(halfRound/2, Normal, Normal);
      }
      dir(Forward);
      move(1600, Fast, Fast);
      dir(rotateRight);
      move(halfRound, Normal, Normal);
      for(i=5; i>0; i--){
        dir(Forward);
        move(1600, Fast, Fast);
        dir(rotateLeft);
        move(halfRound/2, Normal, Normal);
      }
      dir(Forward);
      move(1600, Fast, Fast);
      dir(rotateLeft);
      move(halfRound, Normal, Normal);
    }

    return 0;
}