#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
 * 
 * motoren:
 * stapR <-> P1.6
 * dirR  <-> P2.5
 * stapL <-> 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

const uchar STEP_PINS = BIT4 | BIT5; // PORT 2
const uchar DIRr_PIN = BIT6; // PORT 1
const uchar DIRl_PIN = BIT7; // PORT 1
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;
  uint s = n-100;
  while(n != 0){
    n--;
    P2OUT |= STEP_PINS;
    P2OUT &= ~STEP_PINS;
    if(s < n || n < 100)
        d = 100; // a crude way of a accaleration
    else
        d = t;
    while(d != 0){
        d--;
         __delay_cycles(160);
    }
  }
  __delay_cycles(160000);
}

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 |= STEP_PINS;
    P1DIR |= 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(10000, v);
        dir(RightTurn);
        stap(halfRound, v*2);
        dir(Forward);
        stap(10000, v);
        dir(LeftTurn);
        stap(halfRound, v*2);
    }
    for(ii=2; ii>0; ii--){
        for(i=5; i>0; i--){
            dir(Forward);
            stap(1600, v);
            dir(RightTurn);
            stap(halfRound/2, v*2);
        }
        dir(Forward);
        stap(1600, v);
        dir(RightTurn);
        stap(halfRound, v*2);
        for(i=5; i>0; i--){
            dir(Forward);
            stap(1600, v);
            dir(LeftTurn);
            stap(halfRound/2, v*2);
        }
        dir(Forward);
        stap(1600, v);
        dir(LeftTurn);
        stap(halfRound, v*2);
    }

#endif

#if TEST == 2

    while(1){
        MCSetV(MC_MotorL, 1000);
        MCSetV(MC_MotorR, 1000);
        __delay_cycles(32000000);
        MCSetV(MC_MotorL, -1000);
        MCSetV(MC_MotorR, -1000);
    }

#endif

    return 0;
}