#include "msp430g2553.h"
#include <math.h>     
#define INPUT_0 INCH_2 //x
#define INPUT_1 INCH_3 //y
#define INPUT_2 INCH_4 //z


void copy_C2D (void)
{
  char *Flash_ptrC;                         // Segment C pointer
  unsigned int i;
  char dummy[64];
  int *Flash_ptr;
  Flash_ptrC = (char *) 0x10BD;             // Initialize Flash segment C pointer
  for (i=0; i<48; i++){
    dummy[i]=*Flash_ptrC;
    Flash_ptrC=Flash_ptrC +1;
  }
  Flash_ptr = (int *) 0x10C0;
  FCTL1 = FWKEY + ERASE;
  FCTL3=FWKEY + LOCKA;
  *Flash_ptr=0;
  FCTL1 = FWKEY + WRT;   
  Flash_ptrC = (char *) 0x10C0;  
    for (i=0; i<48; i++)
          {
            *Flash_ptrC=dummy[i];
            Flash_ptrC=Flash_ptrC +1;            
  }
  Flash_ptrC = (char *) 0x107D;
    for (i=0; i<64; i++){
    dummy[i]=*Flash_ptrC;
    Flash_ptrC=Flash_ptrC +1;
  }
  Flash_ptrC = (char *) 0x1080;
  FCTL1 = FWKEY + ERASE;
  FCTL3=FWKEY + LOCKA;
  *Flash_ptrC=0;
  FCTL1 = FWKEY + WRT; 
    Flash_ptrC = (char *) 0x1080;  
    for (i=0; i<64; i++)
          {
            *Flash_ptrC=dummy[i];
            Flash_ptrC=Flash_ptrC +1;            
  }
  Flash_ptrC = (char *) 0x103D;
    for (i=0; i<64; i++){
    dummy[i]=*Flash_ptrC;
    Flash_ptrC=Flash_ptrC +1;
  }
  Flash_ptrC = (char *) 0x1040;
  FCTL1 = FWKEY + ERASE;
  FCTL3=FWKEY + LOCKA;
  *Flash_ptrC=0;
  FCTL1 = FWKEY + WRT; 
    Flash_ptrC = (char *) 0x1040;  
    for (i=0; i<64; i++)
          {
            *Flash_ptrC=dummy[i];
            Flash_ptrC=Flash_ptrC +1;            
  }
  Flash_ptrC = (char *) 0x1000;
    for (i=0; i<61; i++){
    dummy[i]=*Flash_ptrC;
    Flash_ptrC=Flash_ptrC +1;
  }
  Flash_ptrC = (char *) 0x1000;
  FCTL1 = FWKEY + ERASE;
  FCTL3=FWKEY + LOCKA;
  *Flash_ptrC=0;
  FCTL1 = FWKEY + WRT; 
    Flash_ptrC = (char *) 0x1003;  
    for (i=0; i<61; i++)
          {
            *Flash_ptrC=dummy[i];
            Flash_ptrC=Flash_ptrC +1;            
  }
  FCTL1 = FWKEY;                            // Clear WRT bit
  FCTL3 = FWKEY + LOCK+ LOCKA;                     // Set LOCK bit
}
void erase_segment (int address)

{  
  int *Flash_ptr;
  Flash_ptr = (int *) address;
  FCTL1 = FWKEY + ERASE;
  FCTL3=FWKEY + LOCKA;
  *Flash_ptr=0;
  FCTL3=FWKEY +LOCK + LOCKA;
}
void write_SegC (char value, int i)
{
  char *Flash_ptr;                          // Flash pointer
  Flash_ptr = (char *) 0x1000+i;              // Initialize Flash pointer
  FCTL3 = FWKEY + LOCKA;                            // Clear Lock bit
  FCTL1 = FWKEY + WRT;                      // Set WRT bit for write operation 
    *Flash_ptr = value;                   // Write value to flash
  FCTL1 = FWKEY;                            // Clear WRT bit
  FCTL3 = FWKEY + LOCK+ LOCKA;                     // Set LOCK bit
}

unsigned int delay ( unsigned int x)

  {
    unsigned int i,j;
        for (i = 0; i<= x; i++)
        {
            for(j=0;j<=1000; j++);
        }
        return 0;
}
unsigned int analogRead(unsigned int pin) {
    ///unsigned long vout=0;
    //unsigned int i;
    ADC10CTL0 = ADC10ON + ADC10SHT_3 + SREF_0;
    ADC10CTL1 = ADC10SSEL_0 + pin;
        if (pin==INCH_3){
            ADC10AE0 = 0x08;
        }
        else if(pin==INCH_4){
            ADC10AE0 = 0x10;
        }
        else if (pin==INCH_2){
            ADC10AE0 = 0x04;
        }

        ADC10CTL0 |= ENC + ADC10SC;  

        while (1) {
            if (((ADC10CTL0 & ADC10IFG)==ADC10IFG)) {
                  ADC10CTL0 &= ~(ADC10IFG +ENC);
    break;
  }
}
return ADC10MEM;
}

unsigned int analogReadavg(unsigned int pin) {
    unsigned long vout=0;
    unsigned int i;
    ADC10CTL0 = ADC10ON + ADC10SHT_3 + SREF_0;
    ADC10CTL1 = ADC10SSEL_0 + pin;
        if (pin==INCH_3){
            ADC10AE0 = 0x08;
        }
        else if(pin==INCH_4){
            ADC10AE0 = 0x10;
        }
        else if (pin==INCH_2){
            ADC10AE0 = 0x04;
        }
        
        for (i=0; i<=400; i++){
        ADC10CTL0 |= ENC + ADC10SC;  
        vout=ADC10MEM+vout;
        
        }
        vout=vout/400;
        while (1) {
            if (((ADC10CTL0 & ADC10IFG)==ADC10IFG)) {
                  ADC10CTL0 &= ~(ADC10IFG +ENC);
                  
    break;
  }
}
return vout;
}
void main(void) {

WDTCTL = WDTPW + WDTHOLD; //Stop Watchdog Timer
unsigned long  adcvaluex=0; //variable for storing digital value of CH3 input
unsigned long adcvaluey=0; //variable for storing digital value of CH4 input
unsigned long adcvaluez=0; //variable for storing digital value of CH5 input
unsigned long adcvaluex0=0; //variable for storing digital value of CH3 input
unsigned long adcvaluey0=0; //variable for storing digital value of CH4 input
unsigned long adcvaluez0=0; //variable for storing digital value of CH5 input
unsigned long  adc=0;
P1DIR = 0x23; //P1.0=>LED1 and P1.1=>LED2
P2DIR = 0x03;
P2OUT = 0x00;
P1OUT = 0x21;
//PISEL = 0x40;
P1DIR&=~BIT6; //P1.6 is used as Input Pin 
P2DIR&=~BIT3; //P2.3 is used as Input Pin 
P2DIR&=~BIT4; //P2.4 is used as Input Pin 
P2DIR&=~BIT5; //P2.5 is used as Input Pin

unsigned int U,I,O,P,K=130,J=0,Y=81;                            // initialize value  
unsigned long DIP=0; 
unsigned long DIPSET=0;
U=P1IN&BIT6;    //variable description for input pins from dip switch
I=P2IN&BIT3;    //variable description for input pins from dip switch
O=P2IN&BIT4;    //variable description for input pins from dip switch
P=P2IN&BIT5;    //variable description for input pins from dip switch
    if (P1IN & U){      //if first dip bit is set then one value is assigned
        DIP=8;
        J=1;
    }
    if (P2IN & P){      //if first dip bit is set then two value is assigned
        DIP=DIP+4;
    }
    if (P2IN & O){      //if first dip bit is set then four value is assigned
        DIP=DIP+2;
    }
    if (P2IN & I){      //if first dip bit is set then eight value is assigned
        DIP=DIP+1;
        
    }
switch(DIP)
{
case 0:                 //cases to assign correct digital value to corresponding dip value
       {
        DIPSET=0x8;
        break;
}
case 1:
        {
        DIPSET=0xF;
        break;
}
case 2:
        {
        DIPSET=0x16;
        break;
}
case 3:
        {
        DIPSET=0x1E;
        break;
}
case 4:
        {
        DIPSET=0x2D;
        break;
}
case 5:
        {
        DIPSET=0x3C;
        break;
}
case 6:
        {
        DIPSET=0x78;
        break;
}
case 7:
        {
        DIPSET=0xB6;
        break;
}
case 8:
        {
        DIPSET=0x01;
        break;
}
case 9:
        {
        DIPSET=0x02;
        break;
}
case 10:
        {
        DIPSET=0x04;
        break;
}
case 11:
        {
        DIPSET=0x07;
        break;
}
case 12:
        {
        DIPSET=0x0F;
        break;
}
case 13:
        {
        DIPSET=0x16;
        break;
}
case 14:
        {
        DIPSET=0x1e;
        break;
}
case 15:
        {
        DIPSET=0x3c;
        break;
}
  
}
P1OUT=0x23;
delay(100);
P1OUT=0x21;
delay(8);
    adcvaluex0 = analogReadavg( INPUT_0 );// Read the analog input from channel 3 
    adcvaluey0 = analogReadavg( INPUT_1 );// Read the analog input from channel 4
    adcvaluez0 = analogReadavg( INPUT_2 );// Read the analog input from channel 5
while (1){
  if (J==0){ 

  adc=0;  
  adcvaluex = analogRead( INPUT_0 );// Read the analog input from channel 3  
  adcvaluey = analogRead( INPUT_1 );// Read the analog input from channel 4
  adcvaluez = analogRead( INPUT_2 );// Read the analog input from channel 5

    if (adcvaluex<= adcvaluex0){
        adcvaluex= (adcvaluex0-adcvaluex)+adcvaluex0;    // takes the complement of the adcvalue if the value falls below 0g's
    }
    if (adcvaluey<= adcvaluey0){
        adcvaluey= (adcvaluey0-adcvaluey)+adcvaluey0;   // takes the complement of the adcvalue if the value falls below 0g's
    }    
    if (adcvaluez<= adcvaluez0){
        adcvaluez= (adcvaluez0-adcvaluez)+adcvaluez0;   // takes the complement of the adcvalue if the value falls below 0g's
    } 
    adcvaluex = adcvaluex - adcvaluex0;
    adcvaluey = adcvaluey - adcvaluey0;
    adcvaluez = adcvaluez - adcvaluez0;
    if (adcvaluex >= adcvaluez & adcvaluey){
        adc=adcvaluex;   // takes the complement of the adcvalue if the value falls below 0g's
    }      
    if (adcvaluey >= adcvaluex & adcvaluez){
        adc=adcvaluey;   // takes the complement of the adcvalue if the value falls below 0g's
    }    
    if (adcvaluez >= adcvaluex & adcvaluey){
        adc=adcvaluez;   // takes the complement of the adcvalue if the value falls below 0g's
    }    
         if (adc > DIPSET ){  //put in the dip setting value here when ready
             P1OUT = 0x01;// Glow LED2 if channel4 input > channel5 input
                 for(;;){
                   P1OUT = 0x02; 
                   P2OUT = 0x01;
                   delay(50);      //blink delay function thing
                               //add the output or lack of output to relay
                    
                    P1OUT = 0x00;  
                    delay(300);
                 } 
               
        }
         K=K-1;
         if (K==30){
            P1OUT = 0x21;
            
         }
         if (K==10){
           P1OUT = 0x20;
           
           K=150;
         }
         delay(2);
  } 
  if (J==1){
     P1OUT = 0x23;
    while(1){
         unsigned long output=0; 
         unsigned long count=0; 
         erase_segment (0x1000);
         erase_segment (0x1040);
         erase_segment (0x1080);
         erase_segment (0x10C0);         
          while (Y>1){          
                  adcvaluex = analogRead( INPUT_0 );// Read the analog input from channel 

                  adcvaluex=adcvaluex/4;
                  write_SegC(adcvaluex, count); 
                  count = count  + 0x01;
                  adcvaluey = analogRead( INPUT_1 );// Read the analog input from channel 4

                  adcvaluey=adcvaluey/4;
                  write_SegC(adcvaluey, count); 
                  count = count  + 0x01;
                  adcvaluez = analogRead( INPUT_2 );// Read the analog input from channel 5

                  adcvaluez=adcvaluez/4;
                  write_SegC(adcvaluez, count);
                  count = count  + 0x01; 
                  delay(5);
                  Y=Y-1;
          }
              P1OUT = 0x20;  
          while(1){  
         unsigned int i;
 
    delay(4);
    copy_C2D();
    int newrmsx=0,newrmsy=0,newrmsz=0;
    adcvaluex = analogRead( INPUT_0 );
    newrmsx=adcvaluex;
    newrmsx=newrmsx/4;
    adcvaluey = analogRead( INPUT_1 );
    newrmsy=adcvaluey;
    newrmsy=newrmsy/4;
    adcvaluez = analogRead( INPUT_2 );
    newrmsz=adcvaluez; 
    newrmsz=newrmsz/4;    
    count=0;
    write_SegC(newrmsx, count);
    count=count+1;
    write_SegC(newrmsy, count);
    count=count+1;
    write_SegC(newrmsz, count);
    char *Flash_ptr;
    Flash_ptr = (char *) 0x1000;
    unsigned long xtot=0,ytot=0,ztot=0,xnew=0,ynew=0,znew=0; 
    for(i=0;i<= 79;i++){   
      xnew=(*Flash_ptr);
      xtot=xtot+(xnew*xnew);
      Flash_ptr=Flash_ptr+1;
      ynew=(*Flash_ptr);
      ytot=ytot+(ynew*ynew);
      Flash_ptr=Flash_ptr+1;
      znew=(*Flash_ptr);
      ztot=ztot+(znew*znew);
      Flash_ptr=Flash_ptr+1; 
    }

      xtot=sqrt(xtot/80)*0x04;
      ytot=sqrt(ytot/80)*0x04;
      ztot=sqrt(ztot/80)*0x04;
       if (xtot< adcvaluex0){
                  xtot= (adcvaluex0-xtot)+adcvaluex0;   // takes the complement of the adcvalue if the value falls below 0g's
                  }  
      if (ytot< adcvaluey0){
                  ytot= (adcvaluey0-ytot)+adcvaluey0;   // takes the complement of the adcvalue if the value falls below 0g's
                  } 
       if (ztot< adcvaluez0){
                  ztot= (adcvaluez0-ztot)+adcvaluez0;   // takes the complement of the adcvalue if the value falls below 0g's
                  } 
      xtot = xtot - adcvaluex0;
      ytot = ytot - adcvaluey0;
      ztot = ztot - adcvaluez0;         
     
      if (xtot >= ytot & ztot){
       output=xtot;
      }
      if (ytot >= xtot & ztot){
       output=ytot;
      }
      if (ztot >= xtot & ytot){
       output=ztot;
      }    
         if (output > DIPSET ){  //put in the dip setting value here when ready
             P1OUT = BIT0;// Glow LED2 if channel4 input > channel5 input
                 for(;;){
                   P1OUT = 0x02; 
                   P2OUT = 0x01;
                   delay(50);      //blink delay function thing
                               //add the output or lack of output to relay
                    
                    P1OUT = 0x00;  
                    delay(300);
                 } 
               
        }
             K=K-1;
               if (K==125){
                P1OUT = 0x20;
                
               }
         if (K==110){
           P1OUT = 0x21;
           K=130;
         }
          }
       }
    }
   }
 }