/*
 * led.c
 *
 *  Created on: Jan 24, 2019
 *      Author: wn
 */

#include "led.h"
#include "PontCoopScheduler.h"
#include <msp430g2553.h>
#include <stdlib.h>


/*
    row0: P1.3
    row1: P1.4
    row2: P1.5
    row3: P1.7
*/

static inline void selectRow(uint8_t row) {
    switch (row) {
        case 0:
            P1DIR &= ~(BIT3 | BIT4 | BIT7);
            P1OUT &= ~(BIT3 | BIT4 | BIT7);
            P1DIR |= BIT2;
            P1OUT |= BIT2;
            break;
        case 1:
            P1DIR &= ~(BIT2 | BIT4 | BIT7);
            P1OUT &= ~(BIT2 | BIT4 | BIT7);
            P1DIR |= BIT3;
            P1OUT |= BIT3;
            break;
        case 2:
            P1DIR &= ~(BIT2 | BIT3 | BIT7);
            P1OUT &= ~(BIT2 | BIT3 | BIT7);
            P1DIR |= BIT4;
            P1OUT |= BIT4;
            break;
        case 3:
            P1DIR &= ~(BIT2 | BIT3 | BIT4);
            P1OUT &= ~(BIT2 | BIT3 | BIT4);
            P1DIR |= BIT7;
            P1OUT |= BIT7;
            break;
    }
}

/*
    col0, blue: P1.5
    col0, red:  P2.0
    col1, blue: P2.2
    col1, red:  P2.1
    col2, blue: P2.4
    col2, red:  P2.3
    col3, blue: P2.5
    col3, red:  P1.6
*/

typedef enum { BLUE = 0, RED = 1 } tColor;

void selectCol(uint8_t column, tColor color) {
    if (color == BLUE) {
        switch (column) {
            case 0:
                P1DIR &= ~(BIT6);
                P1OUT &= ~(BIT6);
                P2DIR &= ~(BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                P2OUT &= ~(BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                P1DIR |= BIT5;
                P1OUT &= ~BIT5;
                break;
            case 1:
                P1DIR &= ~(BIT5 | BIT6);
                P1OUT &= ~(BIT5 | BIT6);
                P2DIR &= ~(BIT0 | BIT1 | BIT4 | BIT3 | BIT5); 
                P2OUT &= ~(BIT0 | BIT1 | BIT4 | BIT3 | BIT5); 
                P2DIR |= BIT2;
                P2OUT &= ~BIT2;
                break;
            case 2:
                P1DIR &= ~(BIT5 | BIT6);
                P1OUT &= ~(BIT5 | BIT6);
                P2DIR &= ~(BIT0 | BIT2 | BIT1 | BIT3 | BIT5); 
                P2OUT &= ~(BIT0 | BIT2 | BIT1 | BIT3 | BIT5); 
                P2DIR |= BIT4;
                P2OUT &= ~BIT4;
                break;
            case 3:
                P1DIR &= ~(BIT5 | BIT6);
                P1OUT &= ~(BIT5 | BIT6);
                P2DIR &= ~(BIT0 | BIT2 | BIT1 | BIT3 | BIT4); 
                P2OUT &= ~(BIT0 | BIT2 | BIT1 | BIT3 | BIT4); 
                P2DIR |= BIT5;
                P2OUT &= ~BIT5;
                break;
        }
    } else {
        switch (column) {
            case 0:
                P1DIR &= ~(BIT5 | BIT6);
                P1OUT &= ~(BIT5 | BIT6);
                P2DIR &= ~(BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                P2OUT &= ~(BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                P2DIR |= BIT0;
                P2OUT &= ~BIT0;
                break;
            case 1:
                P1DIR &= ~(BIT5 | BIT6);
                P1OUT &= ~(BIT5 | BIT6);
                P2DIR &= ~(BIT0 | BIT2 | BIT4 | BIT3 | BIT5); 
                P2OUT &= ~(BIT0 | BIT2 | BIT4 | BIT3 | BIT5); 
                P2DIR |= BIT1;
                P2OUT &= ~BIT1;
                break;
            case 2:
                P1DIR &= ~(BIT5 | BIT6);
                P1OUT &= ~(BIT5 | BIT6);
                P2DIR &= ~(BIT0 | BIT2 | BIT1 | BIT4 | BIT5); 
                P2OUT &= ~(BIT0 | BIT2 | BIT1 | BIT4 | BIT5); 
                P2DIR |= BIT3;
                P2OUT &= ~BIT3;
                break;
            case 3:
                P1DIR &= ~(BIT5);
                P1OUT &= ~(BIT5);
                P2DIR &= ~(BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                P2OUT &= ~(BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                P1DIR |= BIT6;
                P1OUT &= ~BIT6;
                break;
        }
    }
}

void ledExec() {

    static uint8_t rowNum = 0;
    static uint8_t colNum = 0;
    static tColor color = BLUE;

    selectRow(rowNum);
    selectCol(colNum, color);

    if (color == BLUE) {
        color = RED;
    } else {
        color = BLUE;
        rowNum++;
        if (rowNum > 3) {
            rowNum = 0;
            colNum++;
            if (colNum > 3) {
                colNum = 0;
            }
        }
    }

}

void ledInit() {
    //selectRow(0);
    //selectCol(0, BLUE);

    schAdd(ledExec, NULL, 0, 10);
}