#include "core.h"

#include "drivers\board.h"

void swap(UI32 *xp, UI32 *yp);
void selectionSort(UI32 arr[], UI32 n);

void main(CSTR arg)
{
  //ESP8266.Control(true);
  //ESP8266.Open(115200, false);
  //---
  HCSR04.Open(3);
  //---
  HC05.Open(38400, true);
  //---
  const int sensorDataSize = 10;
  UI32 sensorData[sensorDataSize] = {0};
     PWM.Open(0,490,2000);
     PWM.Open(1,490,2000);
     PWM.Open(2,490,2000);
     PWM.Open(3,490,2000);
     SERVO.Open(0);
  unsigned char inpData[10]={0};

  UI16 Result1=1000;
  UI16 Result2=1500;
  UI16 Result3=1500;
  UI16 Result4=1500;
  UI16 Result5=1000;
  //UART5.Open(9600,false);
  while(1)
  {


    for(int i = 0; i < sensorDataSize; i++)
    {
      sensorData[i]=HCSR04.Read(3);
      sensorData[i] /=10;
    }

    selectionSort(sensorData, sensorDataSize);
    UI32 data = sensorData[sensorDataSize/2];

    UI32 dataSize = 0;
   /* while(dataSize < 1)
    {
      dataSize += HC05.Read(inpData+dataSize,1 - dataSize);
    }
*/
    while(dataSize < 10)
    {
      dataSize += HC05.Read(inpData+dataSize,10 - dataSize);
        //Sleep(10);
    }
    /*
    if(inpData[0]=='D')
      Result1=1300;
    if(inpData[0]=='U')
      Result1=1800;
    if(inpData[0]=='L')
      Result2=1300;
    if(inpData[0]=='R')
      Result2=1800;
    if(inpData[0]=='F')
      Result3=1300;
    if(inpData[0]=='S')
      Result3=1800;
    if(inpData[0]=='W')
      Result4=1300;
    if(inpData[0]=='B')
      Result4=1800;
    if(inpData[0]=='1')
      Result5=1200;
    if(inpData[0]=='2')
      Result5=1500;
    if(inpData[0]=='3')
      Result5=1900;
*/
    //Sleep(1000);
    Result1=(UI16)inpData[0] <<8 | inpData[1];
    Result2=(UI16)inpData[2] <<8 | inpData[3];
    Result3=(UI16)inpData[4] <<8 | inpData[5];
    Result4=(UI16)inpData[6] <<8 | inpData[7];
    Result5=(UI16)inpData[8] <<8 | inpData[9];
    //HC05.Read(inpData,10);
    //UART5.Read(inpData,10);
    //HC05.Write(&data, 4);
   // Sleep(10);
    PWM.Write(0,Result3);
    PWM.Write(1,Result4);
    PWM.Write(2,Result5);
    PWM.Write(3,Result1);
    SERVO.Write(0,Result2*10);
    /*Sleep(1000);
    Result1=1000;
    Result2=1500;
    Result3=1500;
    Result4=1500;
    PWM.Write(0,Result1);
    PWM.Write(1,Result2);
    PWM.Write(2,Result3);
    PWM.Write(3,Result4);
*/
    //Sleep(100);
 }


}


void swap(UI32 *xp, UI32 *yp)
{
    UI32 temp = *xp;
    *xp = *yp;
    *yp = temp;
}

void selectionSort(UI32 arr[], UI32 n)
{
    UI32 i, j, min_idx;

    // One by one move boundary of unsorted subarray
    for (i = 0; i < n-1; i++)
    {
        // Find the minimum element in unsorted array
        min_idx = i;
        for (j = i+1; j < n; j++)
          if (arr[j] < arr[min_idx])
            min_idx = j;

        // Swap the found minimum element with the first element
        swap(&arr[min_idx], &arr[i]);
    }
}