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

/* pinout
 *
 * motor driver:
 * CLK  <-> P1.4*
 * MOSI <-> P1.1*
 * MISO <-> P1.2*
 * CS   <-> P1.3
 * 
 * motion controller:
 * CLK  <-> P1.4*
 * MOSI <-> P1.1*
 * MISO <-> P1.2*
 * CS   <-> P1.0
 * 
 * NFC tag reader:
 * CLK  <-> P1.4*
 * MOSI <-> P1.1*
 * MISO <-> P1.2*
 * CS   <-> P1.6
 * RST  <-> P1.7
 * 
 * Lijndetectie:
 * front right <-> P2.0
 * front left  <-> P2.1
 * back right  <-> P2.2
 * back left   <-> P2.3
 * 
 * *) this is a shared SPI pin.
 * 
 * */

#define TEST 1

const uchar STEP_PINS = BIT4 | BIT5;
const uchar DIRr_PIN = BIT3;
const uchar DIRl_PIN = BIT2;
typedef enum Direction {Forward=0, Backward=(DIRl_PIN | DIRr_PIN), RightTurn=DIRr_PIN, LeftTurn=DIRl_PIN};

const uchar v = 36;
const ulong halfRound = 2800;

void stap(uint n, ulong t){
  ulong d = t;
  while(n != 0){
    n--;
    P2OUT |= STEP_PINS;
    P2OUT &= ~STEP_PINS;
    d=t;
    while(d != 0){
        d--;
        __delay_cycles(160);
    }
  }
}

void dir(enum Direction d){
  P2OUT &= ~(DIRr_PIN | DIRl_PIN); // set dir bits to 0
  P2OUT |= 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 |= STEP_PINS | DIRr_PIN | DIRl_PIN;

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

    __delay_cycles(16000000);

#if TEST == 1

    while(1){
        NFCAvailable();
    }

#endif
#if TEST == 0

    for(ii=2; ii>0; ii--){
        dir(Forward);
        stap(16000, v);
        __delay_cycles(160000);
        dir(RightTurn);
        stap(halfRound, v*2);
        __delay_cycles(160000);
        dir(Forward);
        stap(16000, v);
        __delay_cycles(160000);
        dir(LeftTurn);
        stap(halfRound, v*2);
        __delay_cycles(160000);
    }
    for(ii=2; ii>0; ii--){
        for(i=4; i>0; i--){
            dir(Forward);
            stap(1600, v);
            __delay_cycles(160000);
            dir(RightTurn);
            stap(halfRound/2, v*2);
            __delay_cycles(160000);
        }
        for(i=4; i>0; i--){
            dir(Forward);
            stap(1600, v);
            __delay_cycles(160000);
            dir(LeftTurn);
            stap(halfRound/2, v*2);
            __delay_cycles(160000);
        }
    }

#endif

    return 0;
}