
// --------------------------------------------
// 개요    : 셋업
// --------------------------------------------
void setup() {
    randomSeed(analogRead(0));
    resetTimer();
    artecRobotSetup();
    artecRobotMain();
}

void loop() {}

// --------------------------------------------
// 개요    : 리스트로부터의 삭제 처리
// 인수    : struct _cell_t *p  리스트의 포인터
//         : int pos            리스트에서 삭제하는 위치
// 되돌림  : 성공：0, 에러：-1
// --------------------------------------------
int listDelete(struct cell_t* p, int pos)
{
  // 삭제 위치가 0 이하일 경우, 에러를 보낸다 (아무 처리도 하지않음)
  if (pos <= 0) { return (-1); }
  // 삭게 위치가 리스트의 길이보다 클 경우, 에러를 보낸다 (아무 처리도 하지않음)
  int l = listLength(p);        // 리스트 길이를 취득
  if (l < pos) { return (-1); } 

  cell_t *target, *before;      // 삭제하는 요소와 그 전의 요소
  target = p->next;             // 선두 다음의 요소를 설정
  before = NULL;
  if (target == NULL) return (-1);  // 이미 삭제할 요소가 없을 경우, 에러를 보낸다.
  // 삭제 대상이 되는 요소로 이동한다.
  before = p;
  for (int i = 0;i < pos-1;i++) {
    if (target->next == NULL) return (-1);  // 삭제 대상이 되는 요소가 없을 경우, 에러를 보낸다.
    before = target;        // 삭제대상이 되는 요소의 하나 앞의 요소를 퇴피
    target = target->next;  // 삭제 대상이 되는 요소의 경신
  }
  // 삭제 대상이 되는 요소가 존재할 경우
  before->next = target->next;  // 대상의 하나 앞의 요소에 대상의 다음 요소를 설정
  delete target;  // 대상을 삭제
  return(0);
}

// --------------------------------------------
// 개요    : 리스트에의 추가 처리
// 인수    : struct _cell_t *p  리스트의 포인터
//         : int    data        추가 데이터
// 되돌림  : 성공：0, 에러：-1
// --------------------------------------------
int listAdd(struct cell_t* p, float data)
{
  cell_t *elm, *last;
  // 리스트 요소의 확보
  elm = new cell_t;
  // 요소 확보에 실패할 경우
  if(elm == NULL) {
    // 에러를 보낸다.
    return(-1);
  }
  // last에 리스트의 종단을 설정
  last = p;
  for (;;) {
    if (last->next == NULL) break;
    last = last->next;
  }
  // 리스트의 맨 끝에 추가하는 요소의 설정
  elm->data = data;
  elm->next = NULL;
  last->next = elm;
  return(0);
}

// --------------------------------------------
// 개요    : 리스트 길이의 취득
// 인수    : struct _cell_t *p  리스트의 포인터
// 되돌림  : 리스트 길이
// --------------------------------------------
int listLength(struct cell_t* p)
{
  struct cell_t *last;
  // 리스트 맨 끝으로 이동
  last = p;
  int length = 0;
  for (;;) {
    if (last->next == NULL) break;
    last = last->next;
    length++;
  }
  // 리스트 맨 끝에 추가하는 요소의 설정
  return(length);
}

// --------------------------------------------
// 개요    : 리스트 요소의 취득
// 인수    : struct _cell_t *p  리스트의 포인터
//         : int    pos         리스트 요소의 취득 위치
// 되돌림  : 리스트 요소、요소가 존재하지 않는 경우에는 0을 보낸다
// --------------------------------------------
float listItem(struct cell_t *p, int pos)
{
  // 취득 위치가 0 이하일 경우, 0을 보낸다
  if (pos <= 0) { return (0); }
  // 취득 위치가 리스트의 길이보다 큰 경우, 0을 보낸다.
  int l = listLength(p);    // 리스트 길이를 취득
  if (l < pos) { return (0); }

  struct cell_t *target;    // 취득할 요소
  target = p;               // 선두의 요소를 설정
  // 취득 대상이 되는 요소로 이동한다
  for (int i = 0;i < pos;i++) {
    target = target->next;  // 취득 대상이 되는 요소의 경신
  }
  return target->data;
}

// --------------------------------------------
// 개요    : 리스트에의 삽입 처리
// 인수    : struct _cell_t *p  리스트의 포인터
//         : int pos             삽입하는 위치
//         : float data          삽입하는 데이터
// 되돌림  : 성공：0, 에러：-1
// --------------------------------------------
int listInsert(struct cell_t *p, int pos, float data)
{
  // 삽입 위치가 0 이하일 경우, 에러를 보낸다 (아무것도 안한다.)
  if (pos <= 0) { return (-1); }
  // 삽입위치가 리스트의 길이+1보다 클 경우, 에러를 보낸다 (아무것도 안한다)
  int l = listLength(p);  // 리스트 길이를 취득
  if (l+1 < pos) { return (-1); } 
  // 삽입 위치가 리스트의 종단일 경우
  if (l+1 == pos) {
    // 리스트의 종단에 추가한다.
    listAdd(p, data);
    return (0);
  }

  struct cell_t *item, *target, *before;  // 삽입하는 요소, 삽입하는 위치의 요소와 그 전의 요소
  // 리스트 요소의 확보
  item = new cell_t;
  // 요소 확보에 실패할 경우
  if(item == NULL) { return(-1); }

  target = p;
  // 삽입 대상이 되는 요소에 이동한다.
  for (int i = 0;i < pos;i++) {
    before = target;        // 삽입 대상이 된는 요소의 하나 앞의 요소를 퇴피
    target = target->next;  // 삽입 대상이 되는 요소의 경신
  }
  // 삽입 대상이 되는 요소가 존재하는 경우
  item->data = data;    // 요소 데이터의 설정
  item->next = target;  // 다음 요소를 설정
  before->next = item;  // 하나 앞의 요소에 대상 다음의 요소를 설정
  return(0);
}

// --------------------------------------------
// 개요    : 리스트의 요소의 치환처리
// 인수    : struct _cell_t *p  리스트의 포인터
//         : int pos            치환하는 위치
//         : float data         치환하는 데이터
// 되돌림  : 성공：0, 에러：-1
// --------------------------------------------
int listReplace(struct cell_t *p, int pos, float data)
{
  // 치환 위치가 0 이하일 경우, 에러를 보낸다 (아무것도 안한다.)
  if (pos <= 0) { return (-1); }
  // 치환 위치가 리스트 길이보다 큰 경우, 에러를 보낸다. (아무것도 안한다)
  int l = listLength(p);  // 리스트 길이를 취득
  if (l < pos) { return (-1); } 

  struct cell_t *target;  // 치환하는 요소

  target = p;
  // 치환 대상이 되는 요소에 이동한다.
  for (int i = 0;i < pos;i++) {
    target = target->next;  // 치환 대상이 되는 요소의 경신
  }
  // 치환 대상이 되는 요소가 존재하는 경우
  target->data = data;      // 요소 데이터의 설정
  return(0);
}

// --------------------------------------------
// 개요    : 리스트의 요소에 지정 데이터가 존재하는가 ?
// 인수    : struct _cell_t *p  리스트의 포인터
//         : float data         검색하는 데이터
// 되돌림  : 존재한다：true, 존재하지 않는다：false
// --------------------------------------------
bool listIsContain(struct cell_t *p, float data)
{
  struct cell_t *elm = p;
  // 리스트의 전요소에 대해 data를 검색한다
  for (;;) {
    // 리스트 맨 끝에 도달하면 break
    if (elm->next == NULL) break;
    // 리스트의 다음 요소를 취득
    elm = elm->next;
    // 리스트에 data가 존재하는 경우, true를 보낸다.
    if (elm->data == data) return true;
  }
  // 리스트에 data가 존재하지 않는 경우, false를 보낸다.
  return false;
}

// --------------------------------------------
// 개요    : 반올림 처리
//         : float  arg    인수
// 되돌림  : 연산 결과
// --------------------------------------------
int scratchRound(float arg)
{
  return round(arg);
}

// --------------------------------------------
// 개요    : 산술연산처리
// 인수    : byte   opeID  조작ID
//         : float  arg    인수
// 되돌림  : 연산 결과
// --------------------------------------------
float math(byte opeID, float arg)              // 산술처리
{
  float result;
  switch (opeID) {
    case SQRT:
      result = sqrt(arg);
    break;
    case ABS:     // |n|
      result = abs(arg);
    break;
    case SIN:     // sin(n)
    {
      float rad = arg * PI / 180.0;
      result = sin(rad);
    }
    break;
    case COS:     // cos(n)
    {
      float rad = arg * PI / 180.0;
      result = cos(rad);
    }
    break;
    case TAN:     // tan(n)
    {
      float rad = arg * PI / 180.0;
      result = tan(rad);
    }
    break;
/*
    case ASIN:    // arcsin(n)
    case ACOS:    // arccos(n)
    case ATAN:    // arctan(n)
    break;
*/
    case LN:      // loge
      result = log(arg);
    break;
    case LOG:     // log10
      result = log10(arg);
    break;
    case POWE:    // e^
      result = exp(arg);
    break;
    case POW10:   // 10^
      result = pow(10, arg);
    break;
    default:
      result = 0;
    break;
    
  }
  return result;
}

// --------------------------------------------
// 개요    : 타이머 수치의 취득
// 되돌림  : 타이머의 수치(sec)
// --------------------------------------------
float getTimer()
{
  return ((millis() - StartTime) / 1000.0);
}

// --------------------------------------------
// 개요    : 타이머 수치의 리셋
// --------------------------------------------
void resetTimer()
{
  StartTime = millis();
}

// --------------------------------------------
// Outline    : Temperatur sensor
// --------------------------------------------
// average
#define NumOfTS (8)
float Average[NumOfTS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
unsigned long Total[NumOfTS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
unsigned long PrevTime[NumOfTS] = { 0, 0, 0, 0, 0, 0, 0, 0 };
float GetTemperatureSensorValue(byte connector)
{
  byte index = connector;
  // updating with an interval of 100ms
  unsigned long ct = millis();
  if (ct < PrevTime[index]) { PrevTime[index] = 0; }
  if (ct - PrevTime[index] > 100) {
    float value = (((board.GetTemperatureSensorValue(connector) / 1024.0) * 3.3) - 0.5) / 0.01;
    Total[index] += 1;
    Average[index] += (value - Average[index]) / Total[index];
    if (Total[index] > 150) { Total[index] = 50; }
    PrevTime[index] = ct;
  } else {
  }

  return (Average[index]);
}

// --------------------------------------------
// 概要    : ジャイロセンサーの取得
// --------------------------------------------
double GetGyroscopeValue(byte connector)
{
  int val = board.GetGyroscopeValue(connector);
  if ((connector == X_AXIS) || (connector == Y_AXIS) || (connector == Z_AXIS)) {
    return ((double)val / 16384.0);
  }
  if ((connector == GX_AXIS) || (connector == GY_AXIS) || (connector == GZ_AXIS)) {
    return ((double)val / 131.072);
  }
}


