#include "vxWorks.h"
#include "sysLib.h"
#include "taskLib.h"
#include "stdio.h"
#include "stdlib.h"
#include "cinterface.h"
#include "wdLib.h"
#include "semLib.h"

char state;
char sizeState2;
char sizeState1;
int blockSizeTaskFront;
int gateControlTaskFront;
int countBlocksTask;
int sizeStorageFront = 0;
int smallBlockCountFront = 0;
int largeBlockCountFront = 0;
int queFront = 0;
int blockSizeTimerFrontRes;
int gateControlTimerFrontRes;
int gateControlTimerFrontRes2;
int countBlocksTimerRes;
WDOG_ID gateControlTimerFront;
WDOG_ID gateControlTimerFront2;
WDOG_ID countBlocksTimer;
WDOG_ID blockSizeTimerFront;
SEM_ID gateControlFrontSem;
SEM_ID countBlocksSem;
SEM_ID blockSizeFrontSem;

void timerCallBack (void)
{
 semGive(blockSizeFrontSem);
 blockSizeTimerFrontRes = wdStart(blockSizeTimerFront, 0.7 * sysClkRateGet(), (FUNCPTR)timerCallBack, 0);
}

void timerCallBack1 (void)
{
 semGive(gateControlFrontSem);
}

void timerCallBack2 (void)
{
 semGive(gateControlFrontSem);
}

void timerCallBack3 (void)
{
 semGive(countBlocksSem);
}


void countBlocks(void)
{
 while(1)
  {
   resetCountSensor(0);
   state =  readCountSensor(0);
  if(state == 1)
   {
    largeBlockCountFront = largeBlockCountFront + 1;
    printf("Large Block Count =  %d \n", largeBlockCountFront);
    printf("small Block Count =  %d \n", smallBlockCountFront);
   }
   semTake(countBlocksSem, WAIT_FOREVER);
 }
}

void gateControlFront(void)
{
 while(1)
 {
  printf("close gate \n");
  queFront = 0;
  setGates(1);
  taskDelay(1.45 * sysClkRateGet());
  setGates(0);
  semTake(gateControlFrontSem, WAIT_FOREVER);
 }
}

void blockSizeFront(void)
{
    while(1)
    {
      resetSizeSensors(0);
      sizeState2 = readSizeSensors(0); /* read the size sensors on the front belt*/
      printf("sizesensor read 1 =  %d \n", sizeState2);


     if(sizeState2 == 1 && sizeStorageFront == 0)
     {
       sizeStorageFront = 1;
       printf("sizesensor read 2 =  %d \n", sizeState2);

       resetSizeSensors(0);
       taskDelay(0.45 * sysClkRateGet());
       sizeState2 = readSizeSensors(0);
     }
     if(sizeState2 == 0 && sizeStorageFront ==1)
     {
       smallBlockCountFront = smallBlockCountFront + 1;
       resetSizeSensors(0);
       printf("sizesensor read 3 =  %d \n", sizeState2);

       sizeStorageFront = 0;
       printf("Small block detected \n");
       queFront = queFront + 1;

       if(queFront == 2)
    {
      gateControlTimerFrontRes2 = wdStart(gateControlTimerFront2, 3.45 * sysClkRateGet(),(FUNCPTR)timerCallBack2, 0);
    }
       else
       {
           gateControlTimerFrontRes = wdStart(gateControlTimerFront, 3.45 * sysClkRateGet(),(FUNCPTR)timerCallBack1, 0);
       }
     }

else if(sizeState2 == 3 && sizeStorageFront == 1)
     {
       resetSizeSensors(0);
       sizeStorageFront = 0;
       printf("Large block detected \n");

       countBlocksTimerRes = wdStart(countBlocksTimer, 5 * sysClkRateGet(),(FUNCPTR)timerCallBack3, 0);
     }
       semTake(blockSizeFrontSem, WAIT_FOREVER);
 }
}

void main(void)
{
 startMotor();

 blockSizeFrontSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY);
 gateControlFrontSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY);
 countBlocksSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY);

 gateControlTimerFront = wdCreate();
 gateControlTimerFront2 = wdCreate();
 countBlocksTimer = wdCreate();
 blockSizeTimerFront = wdCreate();

 blockSizeTimerFrontRes = wdStart(blockSizeTimerFront, 0.7* sysClkRateGet(),(FUNCPTR)timerCallBack, 0);

 gateControlTaskFront = taskSpawn("Gate control task Front", 100, 0, 20000, (FUNCPTR)gateControlFront, 0,0,0,0,0,0,0,0,0,0);
 countBlocksTask = taskSpawn("Count blocks task", 100, 0, 20000, (FUNCPTR)countBlocks, 0,0,0,0,0,0,0,0,0,0);
 blockSizeTaskFront = taskSpawn("Block size task Front", 150, 0, 20000, (FUNCPTR)blockSizeFront, 0,0,0,0,0,0,0,0,0,0);
}