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

#define RotSensor S2
#define Thr 50
#define Hyst 10
#define MaxTurns 6

#define VoltCorrect 1.02

#define SpikeMax 6
#define CurrentSpike 80
#define VoltageSpike 80

#define MechGear 3

#define FILE_NAME "data.csv"
#define FILE_SIZE 20000


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


long Timer[MaxTurns];
string str = "";


task main()
{
    byte handle;
    unsigned int result,cnt;
    int SpikeCount = 0;
    int current, voltage, oldcurrent, oldvoltage;
    
    SetSensor(RotSensor, SENSOR_ROTATION);
    SetSensorLowspeed(SensorPort);
    
    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));
    }
    long t0=CurrentTick();
    MotOn;
    oldcurrent = PM_ReadPresentCurrent(SensorPort,ADDR);
    oldvoltage = PM_ReadPresentVoltage(SensorPort,ADDR);
    string s;
    for (int i=0; i<800; i++)
    {
      current = PM_ReadPresentCurrent(SensorPort,ADDR);
      voltage = PM_ReadPresentVoltage(SensorPort,ADDR);
      if((abs(current-oldcurrent) < CurrentSpike) && (abs(voltage-oldvoltage) < VoltageSpike))
      {
        s=StrCat(NumToStr(CurrentTick()-t0),",", NumToStr(Sensor(RotSensor)),",", NumToStr(voltage),",", NumToStr(current));
        WriteLnString(handle, s, cnt);
        oldcurrent = current;
        oldvoltage = voltage;
        SpikeCount=0;
      }
      else if (SpikeCount < SpikeMax) SpikeCount++;
      else
      {
        s=StrCat(NumToStr(CurrentTick()-t0),",", NumToStr(voltage),",", NumToStr(current));
        WriteLnString(handle, s, cnt);
        oldcurrent = current;
        oldvoltage = voltage;
        SpikeCount=0;
      }
    }
    MotOff;
}