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

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


#define SIZE 4

tColor matrix[SIZE][SIZE] = {
    { OFF, OFF, OFF, OFF }, 
    { OFF, OFF, OFF, OFF }, 
    { OFF, OFF, OFF, OFF },
    { OFF, OFF, OFF, OFF }
};

void ledSetMatrix(uint8_t col, uint8_t row, tColor color) {
    if ((col < SIZE) && (row < SIZE)) {
        matrix[col][row] = color;
    }
}

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

static inline void selectRow(uint8_t row) {
    switch (row) {
        case 0:
            P1OUT &= ~(BIT3 | BIT4 | BIT7);
            P1OUT |= BIT2;
            break;
        case 1:
            P1OUT &= ~(BIT2 | BIT4 | BIT7);
            P1OUT |= BIT3;
            break;
        case 2:
            P1OUT &= ~(BIT2 | BIT3 | BIT7);
            P1OUT |= BIT4;
            break;
        case 3:
            P1OUT &= ~(BIT2 | BIT3 | BIT4);
            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
*/


void selectCol(uint8_t column, tColor color) {
    switch (color) {
        case BLUE:
            switch (column) {
                case 0:
                    P1OUT |= BIT6;
                    P2OUT |= BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5; 

                    P1OUT &= ~BIT5;
                    break;
                case 1:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT0 | BIT1 | BIT4 | BIT3 | BIT5); 

                    P2OUT &= ~BIT2;
                    break;
                case 2:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT0 | BIT2 | BIT1 | BIT3 | BIT5); 

                    P2OUT &= ~BIT4;
                    break;
                case 3:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT0 | BIT2 | BIT1 | BIT3 | BIT4); 

                    P2OUT &= ~BIT5;
                    break;
            }
            break;
        case RED:
            switch (column) {
                case 0:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 

                    P2OUT &= ~BIT0;
                    break;
                case 1:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT0 | BIT2 | BIT4 | BIT3 | BIT5); 

                    P2OUT &= ~BIT1;
                    break;
                case 2:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT0 | BIT2 | BIT1 | BIT4 | BIT5); 

                    P2OUT &= ~BIT3;
                    break;
                case 3:
                    P1OUT |= (BIT5);
                    P2OUT |= (BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 

                    P1OUT &= ~BIT6;
                    break;
            }
            break;
        case OFF:
            switch (column) {
                case 0:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                    break;
                case 1:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                    break;
                case 2:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                    break;
                case 3:
                    P1OUT |= (BIT5 | BIT6);
                    P2OUT |= (BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5); 
                    break;
            }
            break;
    }
}


void testledExec() {

    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 ledExec() {
    static uint8_t rowNum = SIZE;
    static uint8_t colNum = SIZE;

    colNum++;
    if (colNum >= SIZE) {
        colNum = 0;
        rowNum++;
        if (rowNum >= SIZE) {
            rowNum = 0;
        }
    }

    tColor color = matrix[colNum][rowNum];
    selectCol(colNum, OFF);
    selectRow(rowNum);
    selectCol(colNum, color);
}

void ledInit() {
    // rows
    P1DIR |= BIT2 | BIT3 | BIT4 | BIT7;

    // columns and colors
    P1DIR |= BIT5 | BIT6;
    P2DIR |= BIT0 | BIT2 | BIT1 | BIT4 | BIT3 | BIT5; 


    // schAdd(testledExec, NULL, 0, 500);
    // schAdd(ledExec, NULL, 0, 1);
}