/**
 * Prints the temperature information for our sensors onto the touchscreen.
 * @returns Time it took to run the function
 */
unsigned long Ohmbrewer::Thermostat::displayThermTemp(Ohmbrewer::Screen *screen) {
    unsigned long start = micros();
    //"Temp °C:  88.0  90.0"
    //         current target
    // If current == target, we'll default to yellow, 'cause we're golden...
    uint16_t color = screen->YELLOW;

    if(getSensor()->getTemp()->c() > getTargetTemp()->c()) {
        // above target temp
        color = screen->RED;
    } else if(getSensor()->getTemp()->c() < getTargetTemp()->c()) {
        // below target temp
        color = screen->CYAN;
    }
    //Label
    screen->setTextColor(screen->WHITE, screen->DEFAULT_BG_COLOR);
    screen->print("Temp ");
    screen->writeDegree();
    screen->print("C:");
    //Temps
    getSensor()->getTemp()->displayTempC(color, screen);
    screen->print(" "); //margin
    getTargetTemp()->displayTempC(screen->YELLOW, screen);

    screen->resetTextColor();

    screen->println("");

    return micros() - start;
}

double PS3USB::getAngle(Angle a) {
        if (PS3Connected) {
                double accXval;
                double accYval;
                double accZval;

                // Data for the Kionix KXPC4 used in the DualShock 3
                const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
                accXval = -((double)getSensor(aX) - zeroG);
                accYval = -((double)getSensor(aY) - zeroG);
                accZval = -((double)getSensor(aZ) - zeroG);

                // Convert to 360 degrees resolution
                // atan2 outputs the value of -π to π (radians)
                // We are then converting it to 0 to 2π and then to degrees
                if (a == Pitch) {
                        double angle = (atan2(accYval, accZval) + PI) * RAD_TO_DEG;
                        return angle;
                } else {
                        double angle = (atan2(accXval, accZval) + PI) * RAD_TO_DEG;
                        return angle;
                }
        } else
                return 0;
}

void leftTurn()
{
  //set the motors to turn the robot
  setMotorSpeed(-140,0);
  setMotorSpeed(140,1);
  
  //we will stay in this loop whilst both sensors cannot see a line
  while((getSensor(4) > 630) || (getSensor(5) > 630))
  {
    delay(5);
  }
  
  delay(100);
  
  //at this point the robot is facing the slightly wrong direction
  //so we turn for a bit more
  while((getSensor(4) < 630) && (getSensor(5) < 630))
  {
    delay(5); 
  }
  
  //the robot has compleated a 90 degrees turn
  //stop the motors
  setBoth(0);
}

unsigned int Device::getNumPixels() const
{
  unsigned int numPixels = 0;
  for (unsigned int nsens = 0; nsens < getNumSensors(); nsens++)
    numPixels += getSensor(nsens)->getNumX() * getSensor(nsens)->getNumY();
  return numPixels;
}

void AksenMain(void) {
	lcd_puts("Hallo");
	while(1) {/*
		lcd_cls();
		lcd_ubyte(analog(0));
		lcd_setxy(0, 4);
		lcd_ubyte(analog(1));
		lcd_setxy(0, 8);
		lcd_ubyte(analog(2));
		lcd_setxy(0, 12);
		lcd_ubyte(analog(3));
		lcd_setxy(1, 0);
		lcd_ubyte(analog(4));
		lcd_setxy(1, 4);
		lcd_ubyte(analog(5));
		lcd_setxy(1, 8);
		lcd_ubyte(analog(6));
		lcd_setxy(1, 12);
		lcd_ubyte(analog(7));
		sleep(50);/**/
		
		lcd_setxy(1,0);
		lcd_ubyte(getSensor(0));
		lcd_ubyte(getSensor(1));
		lcd_ubyte(getSensor(2));
		lcd_ubyte(getSensor(3));
		lcd_ubyte(getSensor(4));
		lcd_ubyte(getSensor(5));
		lcd_ubyte(getSensor(6));
		lcd_ubyte(getSensor(7));
		lcd_ubyte(getSensor(8));
	}
}

/**
 * Controls all the inner workings of the PID functionality
 * Should be called by _timer
 *
 * Controls the heating element Relays manually, overriding the standard relay
 * functionality
 *
 * The pid is designed to Output an analog value, but the relay can only be On/Off.
 *
 * "time proportioning control"  it's essentially a really slow version of PWM.
 * first we decide on a window size. Then set the pid to adjust its output between 0 and that window size.
 * lastly, we add some logic that translates the PID output into "Relay On Time" with the remainder of the
 * window being "Relay Off Time"
 *
 * PID Adaptive Tuning
 * You can change the tuning parameters.  this can be
 * helpful if we want the controller to be agressive at some
 * times, and conservative at others.
 *
 */
void Ohmbrewer::Thermostat::doPID(){
    getSensor()->work();

    setPoint = getTargetTemp()->c();        //targetTemp
    input = getSensor()->getTemp()->c();//currentTemp
    double gap = abs(setPoint-input);   //distance away from target temp
    //SET TUNING PARAMETERS
    if (gap<10) {  //we're close to targetTemp, use conservative tuning parameters
        _thermPID->SetTunings(cons.kP(), cons.kI(), cons.kD());
    }else {//we're far from targetTemp, use aggressive tuning parameters
        _thermPID->SetTunings(agg.kP(), agg.kI(), agg.kD());
    }
    //COMPUTATIONS
    _thermPID->Compute();
    if (millis() - windowStartTime>windowSize) { //time to shift the Relay Window
        windowStartTime += windowSize;
    }
    //TURN ON
    if (getState() && gap!=0) {//if we want to turn on the element (thermostat is ON)
        //TURN ON state and powerPin
        if (!(getElement()->getState())) {//if heating element is off
            getElement()->setState(true);//turn it on
            if (getElement()->getPowerPin() != -1) { // if powerPin enabled
                digitalWrite(getElement()->getPowerPin(), HIGH); //turn it on (only once each time you switch state)
            }
        }
        //RELAY MODULATION
        if (output < millis() - windowStartTime) {
            digitalWrite(getElement()->getControlPin(), HIGH);
        } else {
            digitalWrite(getElement()->getControlPin(), LOW);
        }
    }
    //TURN OFF
    if (gap == 0 || getTargetTemp()->c() <= getSensor()->getTemp()->c() ) {//once reached target temp
        getElement()->setState(false); //turn off element
        getElement()->work();
//        if (getElement()->getPowerPin() != -1) { // if powerPin enabled
//            digitalWrite(getElement()->getPowerPin(), LOW); //turn it off too TODO check this
//        }

        // Notify Ohmbrewer that the target temperature has been reached.
        Publisher pub = Publisher(new String(getStream()),
                                  String("msg"),
                                  String("Target Temperature Reached."));
        pub.add(String("last_read_time"),
                String(getSensor()->getLastReadTime()));
        pub.add(String("temperature"),
                String(getSensor()->getTemp()->c()));
        pub.publish();
    }
}

//backs up the robot back onto the junction
void backUp()
{
  delay(500);
  
  setBoth(-80);
  
  while( (getSensor(4) < 630) || (getSensor(5) < 630) )
  {
    delay(50);
  }
  
  setBoth(0);  
}

void main()
{

        
//Assumption: Motor A is right motor. Motor B is left motor.
//Assumption: reflect 1 is right sensor, reflect 2 is left sensor.

  initialize();
        wait1Msec(200);
//int ticks = 4000;
        while(getSensor(BUTTON) != 1){}
        wait1Msec(200);
        while(getSensor(BUTTON) != 0){}
  resetTimer();
        

  while (time1() < 4100)
    lineFollow();

//Hypothetically, this loop should get us to the
//the point in course C where we are parallel with exit 2.
//Now we turn left.


  turnRight(1150);

//We have now turned left and should be pointing at exit 2. 
setMotor(MOTOR_A, -100);
setMotor(MOTOR_B, 100);

  while (getSensor(BUMPER) != 1);
  wait1Msec(200);
  
  setMotor(MOTOR_A, 100);
  setMotor(MOTOR_B, -100);
  wait1Msec(880);
  
  turnLeft(900);
  
  setMotor(MOTOR_A, -100);
  setMotor(MOTOR_B, 100);

//Drive forward to exit 2.
        while(getSensor(BUTTON) != 1){}
        wait1Msec(200);
        while(getSensor(BUTTON) != 0){}
        
        setMotor(MOTOR_A, 0);
        setMotor(MOTOR_B, 0);

}

void main() {
    initialize();

    // Safety precaution - ensure car is not moving yet.
    setMotor(MOTOR_A, 0);
    setMotor(MOTOR_B, 0);
    
    // Wait for button press to start.
    while (getSensor(BUTTON) == 0);
    while (getSensor(BUTTON) == 1);
    
    // Modify this value for each course.
    int courseNumber = 1;
    drive(courseNumber);
}

void main (void)
{
  initialize();
  wait1Msec(200);
  
  //initialize values
  int pwr = 15;
  int sensorState = -1;
  //determine these values by running the calibration program first
  int threshold1 = 150;
  int threshold2 = 150;
  int delay = 10;
  
  //initialize motors and LED indicators
  setMotor (MOTOR_A, 0);
  setMotor (MOTOR_B, 0);
  LEDnum(0);
  LEDoff(RED_LED);
  LEDoff(GREEN_LED);

  //waits for a button press
  while(getSensor(BUTTON) == 0);
  wait1Msec(100);
  while(getSensor(BUTTON) == 1);
  
  resetTimer(); 
  while(getSensor(BUTTON)== 0 && time100() < 64) { 
  //line follows for 6.4 seconds
    if (sensorState != getSensorState(threshold1, threshold2)) { 
    //only change direction if conditions are different than before. 
      sensorState = getSensorState(threshold1,threshold2);
      if (sensorState == 0) { //go straight
        drive(15, delay);
      } else if (sensorState == 1){
        rightTurn(15,delay);
      } else if (sensorState == 2){
        leftTurn(15, delay);
      } else if (sensorState == 3){
        rightTurn(15, delay);
      }  
    }
  } 
  //hard code exit 2
  rightTurn(pwr,2400);
  drive(pwr,9400);
  leftTurn(pwr,1500);
  drive(pwr,20000);
}

void SensorHandlerOmnicam::_calibrationCallback(const sensor_msgs::ImageConstPtr& raw_image){
  SensorOmnicam* s = static_cast<SensorOmnicam*>(getSensor());
  if(!s){
    ROS_ERROR("No sensor set");
    return;
  }
  string frame_id = raw_image->header.frame_id;
  tf::StampedTransform transform;
  geometry_msgs::TransformStamped humanReadableAndNiceTransform;
  try{
    ros::Time timestamp;
    // Get transformation
    _tfListener->lookupTransform("/base_link", frame_id, raw_image->header.stamp,transform);
    _isCalibrated = true;
    tf::transformStampedTFToMsg(transform, humanReadableAndNiceTransform);
  }
  catch(tf::TransformException & ex){
    ROS_ERROR("%s", ex.what());
  }
  
  if(_isCalibrated){
    g2o::Vector7d vectorQT;
    vectorQT[0] = humanReadableAndNiceTransform.transform.translation.x;
    vectorQT[1] = humanReadableAndNiceTransform.transform.translation.y;
    vectorQT[2] = humanReadableAndNiceTransform.transform.translation.z;
    vectorQT[3] = humanReadableAndNiceTransform.transform.rotation.x;
    vectorQT[4] = humanReadableAndNiceTransform.transform.rotation.y;
    vectorQT[5] = humanReadableAndNiceTransform.transform.rotation.z;
    vectorQT[6] = humanReadableAndNiceTransform.transform.rotation.w;
    g2o::ParameterCamera* camParam = dynamic_cast<g2o::ParameterCamera*>(s->parameter());
    assert(camParam && " parameter  not set" );
    camParam->setOffset(g2o::internal::fromVectorQT(vectorQT));
  }
}

void Arduilink::send(unsigned int _id, const char* _msg) {
	SensorItem* sensor = getSensor(_id);
	if (sensor == NULL) return; // TODO return error
	if (sensor->writter != NULL)
		sensor->writter(_msg);
	// TODO else return warning
}

void FakeSMCPlugin::stop(IOService* provider)
{
    HWSensorsDebugLog("[stop] removing handler");
    if (kIOReturnSuccess != storageProvider->callPlatformFunction(kFakeSMCRemoveKeyHandler, true, this, NULL, NULL, NULL))
        HWSensorsFatalLog("failed to remove handler from storage provider");
    
    HWSensorsDebugLog("[stop] releasing tachometers");
    // Release all tachometers
    if (OSCollectionIterator *iterator = OSCollectionIterator::withCollection(sensors)) {
        while (OSString *key = OSDynamicCast(OSString, iterator->getNextObject())) {
            if (FakeSMCSensor *sensor = getSensor(key->getCStringNoCopy())) {
                if (sensor->getGroup() == kFakeSMCTachometerSensor) {
                    UInt8 index = index_of_hex_char(sensor->getKey()[1]);
                    HWSensorsInfoLog("releasing Fan%X", index);
                    if (!releaseFanIndex(index))
                        HWSensorsErrorLog("failed to release Fan index: %d", index);
                }
            }
        }
        OSSafeRelease(iterator);
    }
    
    HWSensorsDebugLog("[stop] releasing sensors collection");
    sensors->flushCollection();
	
	super::stop(provider);
}

/**
 *  For internal use, do not override
 *
 */
void FakeSMCPlugin::stop(IOService* provider)
{
    HWSensorsDebugLog("removing handler");

    if (OSCollectionIterator *iterator = OSCollectionIterator::withCollection(keyStore->getKeys())) {
        while (FakeSMCKey *key = OSDynamicCast(FakeSMCKey, iterator->getNextObject())) {
            if (key->getHandler() == this) {
                if (FakeSMCSensor *sensor = getSensor(key->getKey())) {
                    if (sensor->getGroup() == kFakeSMCTachometerSensor) {
                        UInt8 index = index_of_hex_char(sensor->getKey()[1]);
                        HWSensorsDebugLog("releasing Fan%X", index);
                        keyStore->releaseFanIndex(index);
                    }
                }

                key->setHandler(NULL);
            }
        }
        OSSafeRelease(iterator);
    }

    HWSensorsDebugLog("releasing sensors collection");

    sensors->flushCollection();

    super::stop(provider);
}

void DS18B20_Controller::completeSensorReadout() {
  // initialise OK sensors to state NOK and leave other states untouched:
  for(uint8_t i=0; i<numSensors; i++) {
    if(sensors[i]->sensorStatus == DS18B20_SENSOR_OK) {
      sensors[i]->sensorStatus = DS18B20_SENSOR_NOK;
    }
    sensors[i]->currentTemp  = ACF_UNDEFINED_TEMPERATURE;
  }
  
  uint8_t addr[DS18B20_SENSOR_ID_BYTES];
  
  while(oneWire->search(addr)) {  
    if (OneWire::crc8(addr, DS18B20_SENSOR_ID_BYTES-1) != addr[DS18B20_SENSOR_ID_BYTES-1]) {
      continue;
    }
    
    uint8_t data[DS18B20_SENSOR_READOUT_BYTES];
    if (! readSensorScratchpad(addr, data)) {
      continue;
    }
    
    DS18B20_Sensor *sensor = getSensor(addr);
    if (sensor != NULL) {
      DS18B20_Readout readout = getCelcius(data);
      #ifdef DEBUG_DS18B20
        Serial.print(F("DEBUG_DS18B20: Sensor "));
        Serial.print(sensor->label);
      #endif
      
      // only set temperature and state for NOK sensors and AUTO_ASSIGNED sensors (leave others untouched):
      if (sensor->sensorStatus == DS18B20_SENSOR_ID_AUTO_ASSIGNED) {
        sensor->currentTemp  = readout.celcius;
        #ifdef DEBUG_DS18B20
          char buf[MAX_DS18B20_SENSOR_ID_STR_LEN];
          Serial.print(F(": ID AUTO ASSIGNED, "));
          Serial.println(formatTemperature(readout.celcius, buf));
        #endif
        
      } else if (sensor->sensorStatus == DS18B20_SENSOR_NOK) {
        // Ensure temperature is plausible:
        if((sensor->rangeMin == ACF_UNDEFINED_TEMPERATURE || readout.celcius >= sensor->rangeMin) 
            && (sensor->rangeMax == ACF_UNDEFINED_TEMPERATURE || readout.celcius <= sensor->rangeMax)) {
          sensor->sensorStatus = DS18B20_SENSOR_OK;
          sensor->currentTemp  = readout.celcius;
          #ifdef DEBUG_DS18B20
            char buf[MAX_DS18B20_SENSOR_ID_STR_LEN];
            Serial.print(F(": OK, "));
            Serial.println(formatTemperature(readout.celcius, buf));
          #endif
        } else {
          #ifdef DEBUG_DS18B20
            Serial.println(F(": NOK"));
          #endif
        }
      }
    } 
  }
  oneWire->reset_search();
}

// line following function (black line on white ground)
void stayOnTrack (int PWR) // PWR is the desired power for the motors
{
  setMotor(MOTOR_A, -PWR);
  setMotor(MOTOR_B, PWR);
  
  LEDnum(0);
  while(getSensor(BUMPER) == 0) // make sure it hasn't crashed
  {
    //Both sensors detect white -> Go straight
    while (getSensor(REFLECT_1) >= 200 && getSensor(REFLECT_2) >= 200
           && getSensor(BUMPER) == 0)
    {
      
      setMotor(MOTOR_A, -PWR);
      setMotor(MOTOR_B, PWR);
      LEDnum(7);
    }
      
    //REFLECT_1 detects black, REFLECT_2 detects white -> Pivot right
    if (getSensor(REFLECT_1) < 200 && getSensor(REFLECT_2) >= 200)
    {
      setMotor(MOTOR_A, PWR); 
      setMotor(MOTOR_B, PWR);
      LEDnum(1);
    }
    
    //REFLECT_1 detects white, REFLECT_2 detects black -> Turn left
    if (getSensor(REFLECT_1) >= 200 && getSensor(REFLECT_2) < 200)
    {
      setMotor(MOTOR_A, -PWR);
      setMotor(MOTOR_B, PWR / 1.5); 
      LEDnum(0);
    }
   
    //Both detect black -> Pivot Right
    if(getSensor(REFLECT_1) < 200 && getSensor(REFLECT_2) < 200)
    {
    	setMotor(MOTOR_A, PWR); 
     	setMotor(MOTOR_B, PWR);
			
    }
    wait1Msec(10);

    
  }
}

void Device::addSensor(Sensor* sensor)
{
  assert(sensor && "Device: can't add a null sensor");
  if (_numSensors > 0 && getSensor(_numSensors - 1)->getOffZ() > sensor->getOffZ())
    throw "Device: sensors must be added in order of increazing Z position";
  _sensors.push_back(sensor);
  _sensorMask.push_back(false);
  _numSensors++;
}

Sensor_t *getSensorFromIndex(unsigned int index) {
    char module;
    unsigned int id;

    module = (index / TRAIN_SENSOR_COUNT) + 'A';
    id = index % TRAIN_SENSOR_COUNT + 1;

    return getSensor(module, id);
}

void rightTurn()
{
  setMotorSpeed(190,0);
  setMotorSpeed(-145,1);
  
  while((getSensor(4) > 630) || (getSensor(5) > 630))
  {
    delay(5);
  }
  
  delay(100);
  
  while((getSensor(4) < 630) && (getSensor(5) < 630))
  {
    delay(5); 
  }
  setBoth(0);
}

/**
 * Publishes updates to Ohmbrewer, etc.
 * This function is called by update().
 * @param args The argument string passed into the Particle Cloud
 * @param argsMap A map representing the key/value pairs for the update
 * @returns The time taken to run the method
 */
int Ohmbrewer::Thermostat::doUpdate(String &args, Ohmbrewer::Equipment::args_map_t &argsMap) {
    unsigned long start = millis();

    // If there are any remaining parameters
    if(args.length() > 0) {
        String targetKey = String("target_temp");
        String sensorKey = String("sensor_state");
        String elmKey = String("element_state");

        parseArgs(args, argsMap);

        // If there are any arguments, the will be a new Target Temperature value
        setTargetTemp(argsMap[targetKey].toFloat());

        // The remaining settings are optional/convenience parameters
        if(argsMap.count(sensorKey) != 0) {
            if(argsMap[sensorKey].equalsIgnoreCase("ON")) {
                getSensor()->setState(true);
            } else if(argsMap[sensorKey].equalsIgnoreCase("OFF")) {
                getSensor()->setState(false);
            } else if(argsMap[sensorKey].equalsIgnoreCase("--")) {
                // Do nothing. Intentional.
            } else {
                // Do nothing. TODO: Should probably raise an error code...
            }
        }

        if(argsMap.count(elmKey) != 0) {
            if(argsMap[elmKey].equalsIgnoreCase("ON")) {
                getElement()->setState(true);
            } else if(argsMap[elmKey].equalsIgnoreCase("OFF")) {
                getElement()->setState(false);
            } else if(argsMap[elmKey].equalsIgnoreCase("--")) {
                // Do nothing. Intentional.
            } else {
                // Do nothing. TODO: Should probably raise an error code...
            }
        }

    }


    return millis() - start;
}