#include <msp430.h>
#include <stdint.h>
//.....
#define RED_LED_PIN             BIT0// on P1.0
//PIN 1 AND PIN 2 ON INCH_A AND INCH_B // on P1.1 and P1.2
#define POWER_SWITCH_PIN        BIT3// on P1.3
//PIN 4 UNUSED                      // on P1.4
#define POWER_RELAY             BIT5 // on P1.5
#define GREEN_LED_PIN           BIT6// on P1.6

//triac drivers on port 2
#define TRIAC_PORT_DIR          P2DIR
#define TRIAC_PORT_SEL          P2SEL
#define TRIAC_PORT_OUT          P2OUT
#define TRIAC_DRIVER_MASK       (BIT0 | BIT1 | BIT2 | BIT3)

//ADC10 inputs P1.1 and P1.2
#define INCH_A                  INCH_1
#define INCH_B                  INCH_2

//timer interrupt flags
#define TAFCC1                  0x02
#define TACOV                   0x0a

//Convenience
#define TOGGLE_LED_RED (P1OUT^= RED_LED_PIN);
#define SET_LED_RED (P1OUT|= RED_LED_PIN);
#define CLEAR_LED_RED (P1OUT&= ~RED_LED_PIN);

#define TOGGLE_LED_GREEN (P1OUT^= GREEN_LED_PIN);
#define SET_LED_GREEN (P1OUT|= GREEN_LED_PIN);
#define CLEAR_LED_GREEN (P1OUT&= ~GREEN_LED_PIN);

#define TOGGLE_TEST_PIN_1_3 (P1OUT^= TEST_OUT);
#define SET_TEST_PIN_1_3 (P1OUT|= TEST_OUT);
#define CLEAR_TEST_PIN_1_3 (P1OUT&= ~TEST_OUT);

#define RELAY_ON (P1OUT|=POWER_RELAY);
#define RELAY_OFF (P1OUT&= ~POWER_RELAY);

//adc
#define ADC_SET_A ADC10CTL1= INCH_A | ADC10DIV_0 |ADC10SSEL_3;
#define ADC_SET_B ADC10CTL1= INCH_B | ADC10DIV_0 |ADC10SSEL_3;

                               //SMCLK; /8; stopped; clear
#define TIMER_1_CLEAR (TA1CTL= TASSEL_2 | ID_3 | MC_0 | TACLR);


//state
#define STATE_NULL          0x0000
#define STATE_MODE_MASK     0x00ff

#define STATE_MEAS_DONE     0x0001 //end of one ADC measure
#define STATE_ACCUM         0x0002 // NOT USED
#define STATE_ENDSET        0x0004 // NOT USED
#define STATE_CALC_CAPS     0x0008 //done with measure both channels
#define STATE_TIMEOUT       0x0010 // NOT USED
#define STATE_START_MEAS    0x0020 // start next measure cycle; set by timer0
#define STATE_STOPPED       0x0040 // switch powered down by user
#define STATE_START         0x0080 // switch on, power up cycle
//last state mode set at    0x0080
//end STATE Mask

#define STATE_SAMPLE        0x0100 // Sampling one channel
#define STATE_FAULT         0x0200 // NOT USED as set but will catch

//measure the A or B channel
#define STATE_MEAS_A        0x1000
#define STATE_MEAS_B        0x2000
//get the peak for A or B done for the cycle
#define STATE_DONE_A        0x4000
#define STATE_DONE_B        0x8000


//substate
#define SUBSTATE_NULL       0x0000
#define SUBSTATE_POWER_ON   0x0001 // power switch detected in on position
#define SUBSTATE_DEBOUNCE   0x0002 // waiting for power switch debounce-> ON
#define SUBSTATE_START0     0x0004 // Beginning of start-up
#define SUBSTATE_START1     0x0008 // phase 2
#define SUBSTATE_WAIT       0x0010 // in case the power switch is on at boot
#define SUBSTATE_POWER_OFF  0x0020 // for delay of turning on TRIACs to brake motor
//variables
uint16_t usi_watchdog;
uint16_t green_flash;
uint16_t state;
uint16_t test;
uint16_t substate;
uint16_t debounce;

//
uint16_t measure_a;
uint16_t measure_b;
uint16_t accum_a;
uint16_t accum_b;

uint16_t cap_bank;

uint32_t ratio;

// .....................................................
//#define NO_WATCHDOG
//#define WD_AS_TIMER
// .....................................................
int main(void)
{
    WDTCTL= WDT_ADLY_250;
    usi_watchdog= 0;
    //enable the watchdog interrupt
    //IE1= WDTIE;

    state= STATE_NULL;
    substate= SUBSTATE_NULL;
    measure_a= 0;
    measure_b= 0;
    accum_a= 0;
    accum_b= 0;
    //
    cap_bank= TRIAC_DRIVER_MASK; //start with all caps on
    TRIAC_PORT_DIR= TRIAC_DRIVER_MASK;

    //1Mhz clock
    BCSCTL1 = CALBC1_1MHZ; // Set range
    DCOCTL = CALDCO_1MHZ; // SMCLK = DCO = 1MHz

    //LED and TEST_OUT, etc.....
    P1OUT= 0;
    P1DIR|= RED_LED_PIN | GREEN_LED_PIN | POWER_RELAY;

    //TRIAC_PORT_DIR|= cap_bank; //all four pins

    //using Timer0 for cycle delay
    //SMCLK; /2; stopped; clear
    TA0CTL= TASSEL_2 | ID_1 | MC_0 | TACLR;
    TA0CCR0= 3000;

    //using Timer1 shared
    TIMER_1_CLEAR;

    //ADC10
    //set:   Vref/Gnd  sh_02; ref2.5v; ref_on; adc ON; enable interrupt
    ADC10CTL0= SREF_1 | ADC10SHT_2 | REF2_5V | REFON | ADC10ON | ADC10IE;
    ADC_SET_A; // INCH_A to start

    state= STATE_STOPPED;
    substate= SUBSTATE_WAIT;

    _enable_interrupt( );

    for( ; ; )
    {
        if( state & STATE_FAULT )
        {
            RELAY_OFF;
            continue;
        }
        if( !( P1IN & POWER_SWITCH_PIN) )
        {
            RELAY_OFF;
            state= STATE_STOPPED;
            substate&= ~SUBSTATE_WAIT;
            if( !(substate & SUBSTATE_POWER_OFF) )
            {
                substate|= SUBSTATE_POWER_OFF;
                TIMER_1_CLEAR;
                TA1CCR0= 50000; //wait msec before turning on all TRIACs
                TA1CTL|= MC_1 | ID_3 | TAIE;
            }
        }
        else if(substate & SUBSTATE_WAIT)
            continue;

        if( state & STATE_SAMPLE )//special case, do now
        {
            ADC10CTL0 |= ENC | ADC10SC;         // Sampling and conversion start
            state&= ~STATE_SAMPLE;
        }

       switch( state & STATE_MODE_MASK )
        {
        case STATE_START_MEAS:
            state= STATE_SAMPLE | STATE_MEAS_A;
            ADC_SET_A;
            accum_a= 0;
            accum_b= 0;
            break;

        case STATE_MEAS_DONE:
            if(state & STATE_MEAS_A)
            {
                if( measure_a > accum_a )
                    accum_a= measure_a;

                else if( measure_a > 22 && measure_a < (accum_a >> 1) )
                    state|= STATE_DONE_A;

                state&= ~(STATE_MEAS_A | STATE_MEAS_DONE);

                if( !( state & STATE_DONE_B) )
                {
                    ADC_SET_B;
                    state|= STATE_MEAS_B;
                }
            }
            else if(state & STATE_MEAS_B)
            {
                if( measure_b > accum_b )
                    accum_b= measure_b;

                else if(  measure_b > 22 && measure_b < (accum_b >> 1) )
                    state|= STATE_DONE_B;

                state&= ~(STATE_MEAS_B | STATE_MEAS_DONE);

                if( !( state & STATE_DONE_A))
                {
                    ADC_SET_A;
                    state|= STATE_MEAS_A;
                }
            }
            if( state & (STATE_MEAS_A | STATE_MEAS_B))
                state|= STATE_SAMPLE;

            else
            {
                state&= ~STATE_MODE_MASK;//TODO
                state|= STATE_CALC_CAPS;
                //timer to next AC cycle
                TA0CTL|= MC_1 | TAIE;
            }
            state&= ~STATE_MEAS_DONE;
            break;

        case STATE_CALC_CAPS:
//            ratio= accum_a >> 4 / accum_b >> 4;
            if( accum_a < accum_b  ) //pull cap out
            {
                cap_bank>>= 1;
                TRIAC_PORT_OUT= cap_bank;//TODO if using other P2 pins
            }
            else if( accum_a > accum_b << 1 && cap_bank < 0x0007)
            {
                cap_bank<<= 1;
                cap_bank|= 1;
                TRIAC_PORT_OUT= cap_bank;//TODO if using other P2 pins
            }
            measure_a= 0;
            measure_b= 0;
            state= STATE_NULL;
            break;

        case STATE_START:
            if(substate & SUBSTATE_START0)
                continue;

            else if(substate & SUBSTATE_START1)
            {
                TRIAC_PORT_OUT= TRIAC_DRIVER_MASK;
                CLEAR_LED_RED;
                substate&= ~SUBSTATE_START1;
                state= STATE_START_MEAS;
            }
            else
            {
                RELAY_OFF;
                for(;;);
            }
            break;

        case STATE_STOPPED:
        {
            CLEAR_LED_GREEN;
            if( P1IN & POWER_SWITCH_PIN )//check if the switch is on
            {
                if( !(substate & SUBSTATE_DEBOUNCE) )
                {
                    debounce= 0;
                    substate|= SUBSTATE_DEBOUNCE;
                }
                else
                    ++debounce;
            }
            else
                substate&= ~SUBSTATE_DEBOUNCE;

            if( debounce > 4000 && substate & SUBSTATE_DEBOUNCE ) //turn it on!!!!
            {
                state= STATE_START;
                substate&= ~(SUBSTATE_DEBOUNCE | SUBSTATE_POWER_OFF);
                substate|= SUBSTATE_START0;
                TRIAC_PORT_OUT= 0;
                cap_bank= TRIAC_DRIVER_MASK; //start with all caps on
                RELAY_ON; //and make sure all is well!!!!!

                TA1CCR0= 900; //wait a couple of cycles before starting cap_switches
                TIMER_1_CLEAR;
                TA1CTL|= MC_1 | TAIE;
                SET_LED_RED;
            }
            break;
        }
        default:
            break;
        }//switch
    }
}

// .....................................................
#pragma vector=ADC10_VECTOR
__interrupt void ADC10_ISR (void)
{
    ADC10CTL0&= ~ENC;         //conversion stop MUST to change INCH_X
    if( state & STATE_MEAS_A)
        measure_a = ADC10MEM;

    else
        measure_b = ADC10MEM;

    state|= STATE_MEAS_DONE;
}

// .....................................................
#pragma vector=WDT_VECTOR
__interrupt void wdt_interrupt( )
{
}

// .....................................................
#pragma vector=TIMER0_A1_VECTOR
__interrupt void TIMER0_A1_ISR( )
{
    state= STATE_START_MEAS;
    TA0CTL= TASSEL_2 | ID_1 | MC_0 | TACLR;// | TAIE;
    return;

}

// .....................................................
//when TA1CCR0 hit
#pragma vector=TIMER1_A1_VECTOR
__interrupt void TIMER1_A1_ISR( )
{
    TOGGLE_LED_GREEN;
    //klll this timer
    TA1CTL&= ~( MC_1 | TAIFG );
    TA1CCTL0&= ~CCIFG;

    if(state & STATE_START)
    {
        substate&= ~SUBSTATE_START0;
        substate|= SUBSTATE_START1;
        return;
    }
    if(state & STATE_STOPPED && substate & SUBSTATE_POWER_OFF )
    {
        TRIAC_PORT_OUT= TRIAC_DRIVER_MASK;
        return;
    }
}

// .....................................................
#pragma vector=TIMER0_A0_VECTOR
__interrupt void TIMER0_A0_ISR( )
{
    RELAY_OFF;
    for(;;);
}

// .....................................................
#pragma vector=TIMER1_A0_VECTOR
__interrupt void TIMER1_A0_ISR( )
{
    RELAY_OFF;
    for(;;);
}