#define MotOn OnFwd(OUT_A, 100);
#define MotOff Off(OUT_A);

#define VoltCorrect 1.02
#define AngleSensor IN_2
#define FILE_NAME "data.csv"
#define FILE_SIZE 10000

long eventtime[];
int event[];


byte SensorPort  =  IN_1;
byte ADDR    = 0x12;
#include "PM-lib.nxc"


void initevents()
{
ArrayInit(eventtime, 0, 20);
ArrayInit(event, 0, 20);
eventtime[0]=2000;
event[0]=-100;
eventtime[1]=4000;
event[1]=0;
eventtime[2]=6000;
event[2]=100;
eventtime[3]=8000;
event[3]=0;
eventtime[4]=10000;
event[4]=-1000;
}


// HiTechnic Angle sensor
int SensorAngle()
{
  int count;
  byte cmndbuf[];                 // buffer for outbound I2C command
  byte respbuf[];                 // buffer for inbound I2C response

  int a;
  bool fSuccess;

  ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x42;            // to set read address Version Number
  count=2;                      // 8 bytes to read
  fSuccess = I2CBytes(AngleSensor, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  if (fSuccess) {
    a = respbuf[0]*2 + respbuf[1] ;
  } else {
    a = -1;
  }
  return a;
}


task main()
{
    byte handle;
    unsigned int result,cnt;

    SetSensorLowspeed(SensorPort);
    SetSensorLowspeed(AngleSensor);

  Wait(10);

	ShowSensorInfo(ADDR, SensorPort);

	PM_SendCommand(SensorPort, ADDR, 'R');

  Wait(1000);
  ClearScreen();
  
    initevents();

    DeleteFile(FILE_NAME);
    result = CreateFile(FILE_NAME, FILE_SIZE, handle);
    if(result != LDR_SUCCESS)
    {
      TextOut(0, LCD_LINE1, "File create");
      TextOut(0, LCD_LINE2, "Err");
      NumOut(0, LCD_LINE3, result);
      until(ButtonPressed(BTNCENTER, false));
    }
    MotOn;
    string s;
    long t;
    OnFwd(OUT_B, 100);
    Wait(1000);
    long t0=CurrentTick();
    PlayTone(600, 200);
    int j=0;
    for (int i=0; i<1000; i++)
    {
      t=CurrentTick()-t0;
      // NumOut(0, LCD_LINE3, t);
      // Wait(50);

      if(t>=eventtime[j])
      {
        if(event[j]<-200) break;
        //PlayTone(1000, 100);
        OnFwd(OUT_B, event[j]);
        j++;
      }
      int a=SensorAngle();
      if(a>200) a=360-a;
      s=StrCat(NumToStr(t),",", NumToStr(PM_ReadPresentCurrent(SensorPort,ADDR)),",", NumToStr(a));
//      s=StrCat(NumToStr(CurrentTick()-t0),",", NumToStr(PM_ReadPresentCurrent(SensorPort,ADDR)));

      WriteLnString(handle, s, cnt);
    }
    MotOff;
    PlayTone(400, 200);
    Wait(1000);

}