#include "mbed.h"
#include "light.h"
#include "speed.h"
#include "CAN_Receive.h"
#include "CAN_Send.h"
DigitalOut Motor_Precharge_Relay (D8);
DigitalOut Motor_Contactor_Relay (D9);
DigitalOut Array_Precharge_Relay (D10);
DigitalOut Array_Contactor_Relay (D11);
DigitalIn Array (D0);
DigitalIn Motor (D1);
void Array_Relay_ON ();
void Array_Relay_OFF ();
void Motor_Relay_ON ();
void Motor_Relay_OFF ();
int  Motor_Switch_Position = 0; //0 = OFF, 1 = BPS/LV, 2 = Array, 3 = Motor
int main()
{
   Thread Can_R(CAN_Receive);
   wait(1);
   Thread CAN_S(CAN_Send);
   wait(2);
   Thread Indicators(lights);
   wait(3);
   Thread Speed(speed);
   while (1)
   {
switch (Motor_Switch_Position) {
   case 0 :
            //pc.printf("case0\n");
            Array_Relay_OFF ();
            Array? Motor_Switch_Position = 0 : Motor_Switch_Position = 2;
            wait_ms(100);
            break;
   case 2 : //pc.printf("case2\n");
            Array_Relay_ON ();
            if (Motor == 0) Motor_Switch_Position = 3;
            if (Array == 0) Motor_Switch_Position = 4;
            if (Motor & Array) Motor_Switch_Position = 0;
            wait_ms(100);
            break;
   case 3 :
            //pc.printf("case3\n");
            Motor_Relay_ON ();
            Motor? Motor_Switch_Position = 4 : Motor_Switch_Position = 5;
            wait_ms(100);
            break;
   case 4 : Motor_Relay_OFF ();
            if (Motor == 0) Motor_Switch_Position = 3;
            if (Array & Motor) Motor_Switch_Position = 0;
            //pc.printf("case4\n");
            wait_ms(100);
            break;
   case 5 : if(Motor == 0)Motor_Switch_Position = 5;
            if((Motor == 1) & (Array == 0)) Motor_Switch_Position = 4;
            //pc.printf("case5\n");
            wait_ms(100);
            break;
            }
   }
}
void
Motor_Relay_ON ()
{
 Motor_Precharge_Relay = 1;
 wait (5);
 Motor_Contactor_Relay = 1;
 return;
}
void
Motor_Relay_OFF ()
{
 Motor_Contactor_Relay = 0;
 Motor_Precharge_Relay = 0;
 return;
}
void
Array_Relay_ON ()
{
 Array_Precharge_Relay = 1;
 wait (5);
 Array_Contactor_Relay = 1;
 return;
}
void
Array_Relay_OFF ()
{
 Array_Contactor_Relay = 0;
 Array_Precharge_Relay = 0;
 return;
}