//Includes of my libraries
#include <Switch.h>
#include <Analog.h>

/* 1) Defined */
//Leds default states
#define LED_RED 12
#define LED_GREEN 13
#define LED_RED_STATE HIGH
#define LED_GREEN_STATE LOW

//Switch minimal on interval
#define SWICH_MINIMAL_INTERVAL 10 //ms

//Debug mode
#define DEBUG_MODE true

//Trimmer minimal change to send new command
#define TRIMMER_MINIMAL_CHANGE 10 //minimal 10 in interval 0..1024

/* 2) Switches */
#define SWITCH_LEFT 2
#define SWITCH_UPPERLEFT 3
#define SWITCH_LOWERLEFT 4
#define SWITCH_UPPERRIGHT 5
#define SWITCH_LOWERRIGHT 6
#define SWITCH_RIGHT 7

Switch s1(SWITCH_LEFT, SWICH_MINIMAL_INTERVAL);
Switch s2(SWITCH_UPPERLEFT, SWICH_MINIMAL_INTERVAL);
Switch s3(SWITCH_LOWERLEFT, SWICH_MINIMAL_INTERVAL);
Switch s4(SWITCH_UPPERRIGHT, SWICH_MINIMAL_INTERVAL);
Switch s5(SWITCH_LOWERRIGHT, SWICH_MINIMAL_INTERVAL);
Switch s6(SWITCH_RIGHT, SWICH_MINIMAL_INTERVAL);

boolean switches_states[6] = {false, false, false, false, false, false};
int switches_commands[6] = {0x00,0x01,0x02,0x3,0x04,0x05};

/* 3) Buttons */
#define BUTTON_UPPERLEFT 8
#define BUTTON_LOWERLEFT 9
#define BUTTON_UPPERRIGHT 10
#define BUTTON_LOWERRIGHT 11

Switch b1(BUTTON_UPPERLEFT, SWICH_MINIMAL_INTERVAL);
Switch b2(BUTTON_LOWERLEFT, SWICH_MINIMAL_INTERVAL);
Switch b3(BUTTON_UPPERRIGHT, SWICH_MINIMAL_INTERVAL);
Switch b4(BUTTON_LOWERRIGHT, SWICH_MINIMAL_INTERVAL);

boolean buttons_states[4] = {false, false, false, false};
int buttons_commands[4] = {0x10,0x11,0x12,0x13};

/* 4) Trimmers */
#define TRIMMER_ONE_MIDDLE_POSITION 524   //stear forwards / backwards - middle position
#define TRIMMER_ONE_MIN_POSITION 414      //  - min position
#define TRIMMER_ONE_MAX_POSITION 646      //  - max position
#define TRIMMER_SIX_MIDDLE_POSITION 548  //stear left / right - middle position
#define TRIMMER_SIX_MIN_POSITION 426      //  - min position
#define TRIMMER_SIX_MAX_POSITION 670      //  - max position
#define TRIMMER_STEAR_MAX_VALUE 1023      //convert position of stear levers to <0, MAX) values; middle point has blind spot for better behaviour
#define TRIMMER_STEAR_BLIND_SPOT_SIZE 40  //size around middle position  <mid - MAX/2, mid + MAX/2> where output value is constant with value TRIMMER_STEAR_MAX_VALUE / 2

#define TRIMMER_MIN_CHANGE 4
#define TRIMMER_VALUE_SHIFT 0

#define TRIMMER_LEFT A0
#define TRIMMER_BOTTOMLEFT A1
#define TRIMMER_BOTTOM2 A2
#define TRIMMER_BOTTOM3 A3
#define TRIMMER_BOTTOMRIGHT A4
#define TRIMMER_RIGHT A5

Analog t1(TRIMMER_LEFT, TRIMMER_MIN_CHANGE, TRIMMER_STEAR_BLIND_SPOT_SIZE, TRIMMER_ONE_MIN_POSITION, TRIMMER_ONE_MAX_POSITION, TRIMMER_ONE_MIDDLE_POSITION, TRIMMER_STEAR_MAX_VALUE);
Analog t6(TRIMMER_RIGHT, TRIMMER_MIN_CHANGE, TRIMMER_STEAR_BLIND_SPOT_SIZE, TRIMMER_SIX_MIN_POSITION, TRIMMER_SIX_MAX_POSITION, TRIMMER_SIX_MIDDLE_POSITION, TRIMMER_STEAR_MAX_VALUE);

int trimmers_states[6] = {TRIMMER_ONE_MIDDLE_POSITION,0,0,0,0,TRIMMER_SIX_MIDDLE_POSITION}; //first and last (stearing levers) are in the middle
int trimmers_commands[6] = {0x20, 0x21,0x22,0x23,0x24,0x25};

/**********************************************************************************/
/**
 * Setup - preparations
 */
void setup()
{
  /* 0) Serial for debugging */
  if( DEBUG_MODE )
  {
    Serial.begin(9600);
    Serial.println("Beginning inicialization...");
  }

  /* 1) Set leds */
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);

  /* 2) Default LED states */
  digitalWrite(LED_RED, LED_RED_STATE);
  digitalWrite(LED_GREEN, LED_GREEN_STATE);
  
  if( DEBUG_MODE ) Serial.println("Inicialization finished.");
}

/**********************************************************************************/
/**
 * Main loop
 */
void loop()
{
  // Switches
  readSwitches();
  
  // Buttons
  readButtons();

  // Trimmers
  readAnalog();

  /* TEST
  testPins();
  delay(1000); */
}

/**********************************************************************************/
/**
 * Read Switches
 */
inline void readSwitches()
{
   int i, cmd;
   boolean val;
   
   for( i = 0; i<sizeof(switches_states); i++)
   {     
      // get value
      switch(i)
      {
          case 0: val = s1.isPressed(); break;
          case 1: val = s2.isPressed(); break;
          case 2: val = s3.isPressed(); break;
          case 3: val = s4.isPressed(); break;
          case 4: val = s5.isPressed(); break;
          case 5: val = s6.isPressed(); break;          
      }

      // process value
      if( val!= switches_states[i] )
      {
          switches_states[i] = val;
          cmd = (val ? 1:0);          
          sendCommand(switches_commands[i], val, true);
      }      
   }
}

/**********************************************************************************/
/**
 * Read Buttons
 */
inline void readButtons()
{
   int i, cmd;
   boolean val;
   
   for( i = 0; i<sizeof(buttons_states); i++)
   {      
      //get value
      switch(i)
      {
          case 0: val = b1.isPressed(); break;
          case 1: val = b2.isPressed(); break;
          case 2: val = b3.isPressed(); break;
          case 3: val = b4.isPressed(); break;
      }

      // process value      
      if( val!= buttons_states[i] )
      {
          buttons_states[i] = val;
          cmd = (val ? 1:0);
          sendCommand(buttons_commands[i], cmd, true);
      }      
   }
}

/**********************************************************************************/
/**
 * Read Analog
 */
inline void readAnalog()
{
   int i, cmd;
   int val;
   
   for( i = 0; i<sizeof(trimmers_states); i++)
   {          
      // get value
      switch(i)
      {
          case 0: val = t1.getPosition(); break;
          case 1: val = analogRead(TRIMMER_BOTTOMLEFT); break;
          case 2: val = analogRead(TRIMMER_BOTTOM2); break;
          case 3: val = analogRead(TRIMMER_BOTTOM3); break;
          case 4: val = analogRead(TRIMMER_BOTTOMRIGHT); break;
          case 5: val = t6.getPosition(); break;          
      }

      // process value
      val = val >> TRIMMER_VALUE_SHIFT; 
      if( abs(val - trimmers_states[i]) >= TRIMMER_MIN_CHANGE )
      {
          trimmers_states[i] = val;   
          sendCommand(trimmers_commands[i], val, false);
      }      
   }
}


/**********************************************************************************/
/**
 * Send command
 */
void sendCommand(int command, int data, boolean booleanValue)
{
  if( DEBUG_MODE )
  {
    Serial.print("Command: 0x");
    Serial.print(command, DEC);
    Serial.print(", data: 0x");
    Serial.println(data, DEC);
  }
}

/**********************************************************************************/
/**
 * Test pins using serial print output + change diodes
 */
void testPins()
{
   Serial.println("\nAnalog inputs");
   Serial.print( analogRead(A0) );
   Serial.print(", ");
   Serial.print( analogRead(A1) );
   Serial.print(", ");
   Serial.print( analogRead(A2) );
   Serial.print(", ");
   Serial.print( analogRead(A3) );
   Serial.print(", ");
   Serial.print( analogRead(A4) );
   Serial.print(", ");
   Serial.println( analogRead(A5) );
   
   Serial.println("\nDigital inputs");
   Serial.print( digitalRead(2) ); Serial.print(", ");
   Serial.print( digitalRead(3) ); Serial.print(", ");
   Serial.print( digitalRead(4) ); Serial.print(", ");
   Serial.print( digitalRead(5) ); Serial.print(", ");
   Serial.print( digitalRead(6) ); Serial.print(", ");
   Serial.print( digitalRead(7) ); Serial.print(", ");
   Serial.print( digitalRead(8) ); Serial.print(", ");
   Serial.print( digitalRead(9) ); Serial.print(", ");
   Serial.print( digitalRead(10) ); Serial.print(", ");
   Serial.println( digitalRead(11) );   
   
   int a = digitalRead(12);
   int b = digitalRead(13);   
   a = (a==0 ? 1: 0);
   b = (b==0 ? 1: 0);
   digitalWrite(12, a);
   digitalWrite(13, b);
}